Zone check? I dunno, tired :3
This commit is contained in:
TheRealGrayden
parent
2ad0eedff5
commit
ad65dc07ce
|
|
@ -1,151 +1,167 @@
|
|||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
import edu.wpi.first.units.Units;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
|
||||
/**
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
|
||||
* constants. This class should not be used for any other purpose. All constants should be declared
|
||||
* globally (i.e. public static). Do not put anything functional in this class.
|
||||
* The Constants class provides a convenient place for teams to hold robot-wide
|
||||
* numerical or boolean constants. This class should not be used for any other
|
||||
* purpose. All constants should be declared globally (i.e. public static). Do
|
||||
* not put anything functional in this class.
|
||||
*
|
||||
* <p>It is advised to statically import this class (or one of its inner classes) wherever the
|
||||
* constants are needed, to reduce verbosity.
|
||||
* <p>
|
||||
* It is advised to statically import this class (or one of its inner classes)
|
||||
* wherever the constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static class OperatorConstants {
|
||||
public static final int kDriverControllerPort = 0;
|
||||
}
|
||||
|
||||
public static class CameraConstanst {
|
||||
public static class OperatorConstants {
|
||||
|
||||
public static class IDFilters {
|
||||
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{ 8, 9, 10, 11 };
|
||||
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{ 27, 26, 25, 24 };
|
||||
public static final int kDriverControllerPort = 0;
|
||||
}
|
||||
|
||||
public static final String BACK_CAMERA_NAME = "limelight-back";
|
||||
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
|
||||
public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
|
||||
public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
|
||||
public static class CameraConstanst {
|
||||
|
||||
public static final String FRONT_CAMERA_NAME = "limelight-front";
|
||||
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
|
||||
|
||||
}
|
||||
public static class IDFilters {
|
||||
|
||||
public static class AdvantageKitConstants{
|
||||
public enum RobotType{
|
||||
SIM,
|
||||
REAL,
|
||||
REPLAY
|
||||
public static final int[] RED_HUB_SHOOTING_IDS = new int[]{8, 9, 10, 11};
|
||||
public static final int[] BLUE_HUB_SHOOTING_IDS = new int[]{27, 26, 25, 24};
|
||||
}
|
||||
|
||||
public static final String BACK_CAMERA_NAME = "limelight-back";
|
||||
public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0;
|
||||
public static final int BACK_HUB_TRACKING_2D_PIPELINE_NUMBER = 1;
|
||||
public static final int BACK_HUB_TRACKING_3D_PIPELINE_NUMBER = 2;
|
||||
|
||||
public static final String FRONT_CAMERA_NAME = "limelight-front";
|
||||
public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0;
|
||||
public static final double[] BLUE_ZONE_COOR = new double[]{1};
|
||||
public static final double[] RED_ZONE_COOR = new double[]{1};
|
||||
public static final double[] NEUTRAL_ZONE_COOR = new double[]{1};
|
||||
}
|
||||
public static final RobotType CURRENT_MODE = RobotType.REAL;
|
||||
}
|
||||
// Swerve offsets as of 3/2/26
|
||||
public static class SwerveDriveOffsets {
|
||||
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
|
||||
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
|
||||
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
|
||||
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
|
||||
}
|
||||
/*
|
||||
|
||||
public static class AdvantageKitConstants {
|
||||
|
||||
public enum RobotType {
|
||||
SIM,
|
||||
REAL,
|
||||
REPLAY
|
||||
}
|
||||
public static final RobotType CURRENT_MODE = RobotType.REAL;
|
||||
}
|
||||
// Swerve offsets as of 3/2/26
|
||||
|
||||
public static class SwerveDriveOffsets {
|
||||
|
||||
public static final float FRONT_LEFT_MOTOR_OFFSET = -0.336426f;
|
||||
public static final float FRONT_RIGHT_MOTOR_OFFSET = 0.108887f;
|
||||
public static final float BACK_LEFT_MOTOR_OFFSET = -0.492432f;
|
||||
public static final float BACK_RIGHT_MOTOR_OFFSET = 0.470215f;
|
||||
}
|
||||
|
||||
/*
|
||||
* Numbers starting with 1 (E.g. 11, 12, 13) are in the Front Left of the robot
|
||||
* Numbers starting with 2 (E.g. 21, 22, 23) are in the Front Right of the robot
|
||||
* Numbers starting with 3 (E.g. 31, 32, 33) are in the Back Left of the robot
|
||||
* Numbers starting with 4 (E.g. 41, 42, 43) are in the Back Right of the robot
|
||||
* Laters numbers will be added for more subsystems
|
||||
* Single diget numbers (E.g. 1, 2, 3) are extra, non-motor, pieces that require IDs, such as the Pigeon
|
||||
*/
|
||||
public static class SwerveDriveIDs {
|
||||
public static final int FRONT_LEFT_STEER_ID = 11;
|
||||
public static final int FRONT_LEFT_DRIVE_ID = 12;
|
||||
public static final int FRONT_LEFT_ENCODER_ID = 13;
|
||||
public static final int FRONT_RIGHT_STEER_ID = 21;
|
||||
public static final int FRONT_RIGHT_DRIVE_ID = 22;
|
||||
public static final int FRONT_RIGHT_ENCODER_ID = 23;
|
||||
public static final int BACK_LEFT_STEER_ID = 31;
|
||||
public static final int BACK_LEFT_DRIVE_ID = 32;
|
||||
public static final int BACK_LEFT_ENCODER_ID = 33;
|
||||
public static final int BACK_RIGHT_STEER_ID = 41;
|
||||
public static final int BACK_RIGHT_DRIVE_ID = 42;
|
||||
public static final int BACK_RIGHT_ENCODER_ID = 43;
|
||||
}
|
||||
*/
|
||||
public static class SwerveDriveIDs {
|
||||
|
||||
public static class RobotChassisLimits{
|
||||
// Max velocity in meters per second
|
||||
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
|
||||
// In meters per second squared
|
||||
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
|
||||
public static final int FRONT_LEFT_STEER_ID = 11;
|
||||
public static final int FRONT_LEFT_DRIVE_ID = 12;
|
||||
public static final int FRONT_LEFT_ENCODER_ID = 13;
|
||||
public static final int FRONT_RIGHT_STEER_ID = 21;
|
||||
public static final int FRONT_RIGHT_DRIVE_ID = 22;
|
||||
public static final int FRONT_RIGHT_ENCODER_ID = 23;
|
||||
public static final int BACK_LEFT_STEER_ID = 31;
|
||||
public static final int BACK_LEFT_DRIVE_ID = 32;
|
||||
public static final int BACK_LEFT_ENCODER_ID = 33;
|
||||
public static final int BACK_RIGHT_STEER_ID = 41;
|
||||
public static final int BACK_RIGHT_DRIVE_ID = 42;
|
||||
public static final int BACK_RIGHT_ENCODER_ID = 43;
|
||||
}
|
||||
|
||||
// In radians per second
|
||||
public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
|
||||
// In radians per second squared
|
||||
public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
|
||||
}
|
||||
public static class RobotChassisLimits {
|
||||
// Max velocity in meters per second
|
||||
|
||||
//Bird
|
||||
public static class ExtraIDEntities {
|
||||
public static final int PIGEON_ID = 1;
|
||||
}
|
||||
|
||||
public static class IntakeConstants {
|
||||
public static final int DEPLOY_MOTOR_ID = 17;
|
||||
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
|
||||
public static final double MAX_ROBOT_LINEAR_VELOCITY = 3;
|
||||
// In meters per second squared
|
||||
public static final double MAX_ROBOT_LINEAR_VELOCITY_ACCEL = 2;
|
||||
|
||||
public static final int INTAKE_MOTOR_ID = 2;
|
||||
public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
|
||||
}
|
||||
// In radians per second
|
||||
public static final double MAX_ROBOT_ANGULAR_VELOCITY = Math.PI;
|
||||
// In radians per second squared
|
||||
public static final double MAX_ROBOT_ANGULAR_VELOCITY_ACCEL = Math.PI;
|
||||
}
|
||||
|
||||
public static class ShooterConstants {
|
||||
public static final int LAUNCHER_MOTOR_ID = 3;
|
||||
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
|
||||
//Bird
|
||||
public static class ExtraIDEntities {
|
||||
|
||||
public static final int TURRET_MOTOR_ID = 4;
|
||||
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
|
||||
public static final int TURRET_LIMIT_CHANNEL = 9;
|
||||
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
|
||||
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
|
||||
public static final double HOMING_SPEED = .35;
|
||||
public static final double TURRET_GEAR_RATIO = 6;
|
||||
public static final int PIGEON_ID = 1;
|
||||
}
|
||||
|
||||
public static final int HOOD_SERVO_CHANNEL = 0;
|
||||
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
|
||||
public static final int HOOD_ENCODER_ID = 4;
|
||||
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
|
||||
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
|
||||
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
|
||||
public static final double DEFLECTOR_STORED_ANGLE = 0;
|
||||
public static final double DEFLECTOR_SET_ANGLE1 = 100;
|
||||
public static final double DEFLECTOR_SET_ANGLE2 = 200;
|
||||
public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
|
||||
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
|
||||
}
|
||||
|
||||
public static class HopperConstants {
|
||||
public static final int KICKER_MOTOR_ID = 9;
|
||||
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
|
||||
public static class IntakeConstants {
|
||||
|
||||
public static final int SPINDEXER_MOTOR_ID = 8;
|
||||
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
|
||||
}
|
||||
public static final int DEPLOY_MOTOR_ID = 17;
|
||||
public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this
|
||||
|
||||
public static class FieldPoses {
|
||||
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
|
||||
public static final int INTAKE_MOTOR_ID = 2;
|
||||
public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this
|
||||
}
|
||||
|
||||
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
||||
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
||||
public static class ShooterConstants {
|
||||
|
||||
}
|
||||
public static final int LAUNCHER_MOTOR_ID = 3;
|
||||
public static final boolean LAUNCHER_MOTOR_INVERTED = false;
|
||||
|
||||
public static final int TURRET_MOTOR_ID = 4;
|
||||
public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
|
||||
public static final int TURRET_LIMIT_CHANNEL = 9;
|
||||
public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165);
|
||||
public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5);
|
||||
public static final double HOMING_SPEED = .35;
|
||||
public static final double TURRET_GEAR_RATIO = 6;
|
||||
|
||||
public static final int HOOD_SERVO_CHANNEL = 0;
|
||||
public static final boolean HOOD_SERVO_INVERTED = false; // TODO: Test this
|
||||
public static final int HOOD_ENCODER_ID = 4;
|
||||
public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1);
|
||||
public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90);
|
||||
public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0);
|
||||
public static final double DEFLECTOR_STORED_ANGLE = 0;
|
||||
public static final double DEFLECTOR_SET_ANGLE1 = 100;
|
||||
public static final double DEFLECTOR_SET_ANGLE2 = 200;
|
||||
public static final double DEFLECTOR_SET_ANGLE3 = 200; // Return to 300 once hood is fixed
|
||||
public static final double DEFLECTOR_SERVO_RATIO = 1.0;
|
||||
}
|
||||
|
||||
public static class HopperConstants {
|
||||
|
||||
public static final int KICKER_MOTOR_ID = 9;
|
||||
public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive;
|
||||
|
||||
public static final int SPINDEXER_MOTOR_ID = 8;
|
||||
public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive;
|
||||
}
|
||||
|
||||
public static class FieldPoses {
|
||||
|
||||
private static AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
|
||||
|
||||
public static Pose3d BLUE_HUB_POSE = fieldTags.getTagPose(26).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
||||
public static Pose3d RED_HUB_POSE = fieldTags.getTagPose(10).get().transformBy(new Transform3d(new Transform2d(-0.5207, 0, Rotation2d.kZero)));
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@ import frc.robot.commands.AutoShootCommandChassisTurret;
|
|||
import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
|
||||
import frc.robot.commands.RelayCommand;
|
||||
import frc.robot.commands.SetDeflectorCommand;
|
||||
import frc.robot.commands.ZoneCheck;
|
||||
import frc.robot.subsystems.DriveSubsystem;
|
||||
import frc.robot.subsystems.HopperSubsystem;
|
||||
import frc.robot.subsystems.IntakeSubsystem;
|
||||
|
|
@ -83,6 +84,7 @@ public class RobotContainer {
|
|||
m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
|
||||
m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
|
||||
m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
|
||||
m_driverController.b().toggleOnTrue(new ZoneCheck());
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ import frc.robot.subsystems.DriveSubsystem;
|
|||
import frc.robot.subsystems.HopperSubsystem;
|
||||
import frc.robot.subsystems.TurretSubsystem;
|
||||
|
||||
public class ZoneSwap extends Command {
|
||||
public class AnyTarget extends Command {
|
||||
|
||||
// For Chassis Control
|
||||
private DriveSubsystem driveSubsystem;
|
||||
|
|
@ -40,7 +40,7 @@ public class ZoneSwap extends Command {
|
|||
|
||||
private final double chassisRotateSpeed = Math.PI * 0.1;
|
||||
|
||||
public ZoneSwap(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) {
|
||||
public AnyTarget(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem) {
|
||||
this.driveSubsystem = driveSubsystem;
|
||||
this.hopperSubsystem = hopperSubsystem;
|
||||
this.turretSubsystem = turretSubsystem;
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
package frc.robot.commands;
|
||||
|
||||
import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.apriltag.AprilTagFields;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class ZoneCheck extends Command {
|
||||
|
||||
private AprilTagFieldLayout fieldTags = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
|
||||
private Zone zone;
|
||||
|
||||
public enum Zone {
|
||||
Blue,
|
||||
Red,
|
||||
Neutral;
|
||||
}
|
||||
|
||||
public ZoneCheck() {
|
||||
getBotPose();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initialize() {
|
||||
DriverStation.getAlliance().ifPresent((aliance) -> {
|
||||
if (aliance == Alliance.Blue) {
|
||||
switch (zone) {
|
||||
case Blue:
|
||||
System.out.println("Im Home");
|
||||
case Neutral:
|
||||
System.out.println("Uhhhhhhhh not red");
|
||||
;
|
||||
default:
|
||||
System.out.println("BOOOOO, BOOOO RED");
|
||||
;
|
||||
}
|
||||
} else {
|
||||
switch (zone) {
|
||||
case Blue:
|
||||
System.out.println("Screw Blue");
|
||||
;
|
||||
case Neutral:
|
||||
System.out.println("Uhhhhhhhh not red");
|
||||
;
|
||||
default:
|
||||
System.out.println("Im Home");
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
@Override
|
||||
public void execute() {
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFinished() {
|
||||
return false;
|
||||
}
|
||||
|
||||
private double[] getBotPose() {
|
||||
return LimelightHelpers.getBotPose_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
|
||||
}
|
||||
|
||||
private void getZone() {
|
||||
if (getBotPose()[0] >= Constants.CameraConstanst.RED_ZONE_COOR[0]) {
|
||||
zone = Zone.Red;
|
||||
} else if (getBotPose()[0] >= Constants.CameraConstanst.NEUTRAL_ZONE_COOR[0]) {
|
||||
zone = Zone.Neutral;
|
||||
} else {
|
||||
zone = Zone.Blue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -135,7 +135,6 @@ public class DriveSubsystem extends TestableSubsystem {
|
|||
.build())
|
||||
.build();
|
||||
|
||||
|
||||
// Set vision measurement confidence values
|
||||
driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999));
|
||||
|
||||
|
|
@ -166,7 +165,7 @@ public class DriveSubsystem extends TestableSubsystem {
|
|||
(speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()),
|
||||
new PPHolonomicDriveController(
|
||||
new PIDConstants(1.5, 0, 0),
|
||||
new PIDConstants(Math.PI/2, 0, 0)),
|
||||
new PIDConstants(Math.PI / 2, 0, 0)),
|
||||
config,
|
||||
() -> {
|
||||
var alliance = DriverStation.getAlliance();
|
||||
|
|
@ -195,12 +194,12 @@ public class DriveSubsystem extends TestableSubsystem {
|
|||
Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d());
|
||||
|
||||
LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME);
|
||||
if(frontCameraPose.tagCount >= 1){
|
||||
if (frontCameraPose.tagCount >= 1) {
|
||||
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds);
|
||||
Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose);
|
||||
}
|
||||
LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME);
|
||||
if(backCameraPose.tagCount >= 1){
|
||||
if (backCameraPose.tagCount >= 1) {
|
||||
driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds);
|
||||
Logger.recordOutput("BackPoseEstimate", backCameraPose.pose);
|
||||
}
|
||||
|
|
@ -212,15 +211,15 @@ public class DriveSubsystem extends TestableSubsystem {
|
|||
driveSystem.simulationPeriodic();
|
||||
}
|
||||
|
||||
public Command getAutoCommand(){
|
||||
public Command getAutoCommand() {
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
|
||||
public void stopDrive(){
|
||||
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0,0,0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
|
||||
public void stopDrive() {
|
||||
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, 0, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
|
||||
}
|
||||
|
||||
public void rotateChassis(double rotSpeed){
|
||||
public void rotateChassis(double rotSpeed) {
|
||||
driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(0, 0, rotSpeed, driveSystem.getGyro().getRotation2d()), Translation2d.kZero);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue