diff --git a/Log_23-06-23_19-38-49.wpilog b/Log_23-06-23_19-38-49.wpilog deleted file mode 100644 index abe5659f..00000000 Binary files a/Log_23-06-23_19-38-49.wpilog and /dev/null differ diff --git a/Log_23-06-23_19-45-26.wpilog b/Log_23-06-23_19-45-26.wpilog deleted file mode 100644 index caf31e13..00000000 Binary files a/Log_23-06-23_19-45-26.wpilog and /dev/null differ diff --git a/README.md b/README.md index d1b417b5..8987e5b1 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,7 @@ Huskie Robotics, FRC Team 3061's, starter project and library focused on a swerv ---- * git must be installed as the GVersion plugin depends upon it * [CTRE's Phoenix 6](https://store.ctr-electronics.com/software/) (both free and licensed options are supported) +* [RevLib](https://docs.revrobotics.com/sparkmax/software-resources/spark-max-api-information) * [AdvantageKit](https://github.com/Mechanical-Advantage/AdvantageKit/blob/main/README.md) * [PathPlanner](https://github.com/mjansen4857/pathplanner) * [PhotonLib](https://photonvision.org) @@ -40,6 +41,7 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can **Tuning** ---- +* If you are using all CTRE hardware, use the Swerve Project Generator in Tuner X. Copy the values from the generated TunerConstants.java file into your RobotConfig subclass, DrivetrainIOCTRE.java, and SwerveConstants.java as appropriate. If you are not using all CTRE hardware, do the following. * Checking motor and encoder directions: * The drive motor, angle motor, and angle encoder constants in SwerveModuleConstants should be configured appropriately for the MK4 and MK4i swerve modules. However, it doesn't hurt to verify. * If you want to verify the following, the ```Subsystem``` example subsystem can be used with ```TESTING``` set to true to directly control the associated motor. Set ```MOTOR_CAN_ID``` in ```SubsystemConstants``` to the appropriate CAN ID and ensure that CAN ID is not specified for any swerve motor in ```DefaultRobotConfig```. Use Shuffleboard to set the motor power. @@ -47,7 +49,7 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can * When the angle motor is supplied a positive input, it should rotate the swerve module wheel such that the wheel rotates in the CCW direction; if not, negate. * When the angle motor rotates the swerve module wheel in a CCW direction, the CANcoder should increase its reading; if not, set to negate. * Setting Steer Offsets (e.g., ```FRONT_LEFT_MODULE_STEER_OFFSET_ROT```) in your ```RobotConfig``` subclass (e.g., DefaultRobotConfig.java): - * For finding the offsets, use a piece of 1x1 metal that is straight against the forks of the front and back modules (on the left and right side) to ensure that the modules are straight + * For finding the offsets, use a piece of 2x1 extrusion that is straight against the forks of the front and back modules (on the left and right side) to ensure that the modules are straight * Point the bevel gears of all the wheels in the same direction (either facing left or right), and preferably you should have the wheels facing in the direction where a positive input to the drive motor drives forward; if for some reason you set the offsets with the wheels backwards, you can change the appropriate ```DRIVE_MOTOR_INVERTED``` in ```SwerveModuleConstants``` to fix * Open Phoenix Tuner 6 and, for each CANcoder on a swerve module, copy the negated rotation value of the "Absolute Position No Offset" signal to the ```STEER_OFFSET``` constants. For example, if the CANcoder "Absolute Position No Offset" signal is 0.104004, specify a value of -0.104004 for the corresponding constant. * Checking geometry: @@ -69,6 +71,7 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can 16. rotate the cart in a CW direction and verify that the gyro is decreasing 17. use Phoenix Tuner to flash the LEDs on the Falcon 500s and CANcoders to ensure that the CAN IDs are properly assigned to the front-left, front-right, back-left, and back-right positions (all of the above may behave as expected even if the modules aren't assigned to the appropriate corners) * Angle characterization values (```ANGLE_KS```, ```ANGLE_KV```, ```ANGLE_KA```) in in your ```RobotConfig``` subclass (e.g., DefaultRobotConfig.java): + * set ```TUNING_MODE``` in Constants.java to true (characterization analysis doesn't normally run) * in Shuffleboard, set the "Auto Routine" chooser to "Swerve Rotate Characterization" * start the autonomous period * the ```FeedForwardCharacterization``` command will run and output the KS, KV, and KA values @@ -76,7 +79,7 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can * Angle Motor PID Values (```ANGLE_KP```, ```ANGLE_KI```, ```ANGLE_KD```) in your ```RobotConfig``` subclass (e.g., DefaultRobotConfig.java): * set ```TUNING_MODE``` in Constants.java to true * open Shuffleboard, go to the SmartDashboard tab, and see controls for each of the PID values; values can be changed via these controls as you interactively tune the controller - * start with a low P value (10.0) + * start with a P value of 100.0 * double until the module starts oscillating around the set point * scale back by searching for the value (for example, if it starts oscillating at a P of 100, then try (100 -> 50 -> 75 -> etc.)) until the module overshoots the setpoint but corrects with no oscillation * graph the ```anglePositionErrorDeg``` in Advantage Scope for each swerve module to assist in tuning @@ -85,6 +88,7 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can * copy the values from the Shuffleboard controls into SwerveModuleConstants.java * set ```TUNING_MODE``` in Constants.java to false * Drive characterization values (```DRIVE_KS```, ```DRIVE_KV```, ```DRIVE_KA```) in in your ```RobotConfig``` subclass (e.g., DefaultRobotConfig.java): + * set ```TUNING_MODE``` in Constants.java to true (characterization analysis doesn't normally run) * in Shuffleboard, set the "Auto Routine" chooser to "Swerve Drive Characterization" * ensure that there is about 40' of carpet in front of the robot before enabling the auto routine; if there is less, increase ```RAMP_RATE_VOLTS_PER_SECOND``` in ```FeedForwardCharacterization``` * start the autonomous period @@ -95,6 +99,7 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can * open Shuffleboard, go to the SmartDashboard tab, and see controls for each of the PID values; values can be changed via these controls as you interactively tune the controller * in Shuffleboard, set the "Auto Routine" chooser to "Drive Velocity Tuning" * tune ```DRIVE_KP``` until it doesn't overshoot and doesn't oscillate around a target velocity + * start with a P value of 0.2 (DrivetrainIOCTRE) or 0.005 (DrivetrainIOGeneric) * graph the ```driveVelocityErrorMetersPerSec``` in Advantage Scope for each swerve module to assist in tuning * copy the values from the Shuffleboard controls into SwerveModuleConstants.java * set ```TUNING_MODE``` in Constants.java to false @@ -120,5 +125,3 @@ To add an additional robot, create a new subclass of ```RobotConfig``` (you can * general AdvantageKit logging code, AdvantageKit-enabled Gyro classes, swerve module simulation, and drive characterization from Mechanical Advantage's [SwerveDevelopment](https://github.com/Mechanical-Advantage/SwerveDevelopment) * AdvantageKit-enabled pneumatics classes from Mechanical Advantage's 2022 [robot code](https://github.com/Mechanical-Advantage/RobotCode2022) * FaultReporter (originally AdvancedSubsystem), SubsystemFault, SelfChecking classes from [Ranger Robotics](https://github.com/3015RangerRobotics/2023Public) -* Talon factories from Citrus Circuits 2022 [robot code](https://github.com/frc1678/C2022) -* FaultReporter (originally AdvancedSubsystem), SubsystemFault, SelfChecking classes from Ranger Robotics 2023 [robot code](https://github.com/3015RangerRobotics/2023Public) diff --git a/src/main/java/frc/lib/team3061/RobotConfig.java b/src/main/java/frc/lib/team3061/RobotConfig.java index 0977b0cb..6314c56e 100644 --- a/src/main/java/frc/lib/team3061/RobotConfig.java +++ b/src/main/java/frc/lib/team3061/RobotConfig.java @@ -67,6 +67,8 @@ public double getSwerveAngleKD() { /* * Returns the voltage needed to overcome the swerve module's static friction. Defaults to 0. + * This constant will be provided directly to the hardware. Therefore, the units must be that + * as expected by the hardware. * * @return the voltage needed to overcome the swerve module's static friction */ @@ -76,6 +78,8 @@ public double getSwerveAngleKS() { /** * Returns the voltage needed to hold (or "cruise") at a given constant velocity. Defaults to 0. + * This constant will be provided directly to the hardware. Therefore, the units must be that as + * expected by the hardware. * * @return the voltage needed to hold (or "cruise") at a given constant velocity */ @@ -85,6 +89,8 @@ public double getSwerveAngleKV() { /** * Returns the voltage needed to induce a given acceleration in the motor shaft. Defaults to 0. + * This constant will be provided directly to the hardware. Therefore, the units must be that as + * expected by the hardware. * * @return the voltage needed to induce a given acceleration in the motor shaft */ @@ -124,7 +130,9 @@ public double getSwerveDriveKD() { } /** - * Returns the voltage needed to overcome the drivetrain's static friction. Defaults to 0. + * Returns the voltage needed to overcome the drivetrain's static friction. Defaults to 0. This + * constant will be provided directly to the hardware. Therefore, the units must be that as + * expected by the hardware. * * @return the voltage needed to overcome the drivetrain's static friction */ @@ -134,6 +142,8 @@ public double getDriveKS() { /** * Returns the voltage needed to hold (or "cruise") at a given constant velocity. Defaults to 0. + * This constant will be provided directly to the hardware. Therefore, the units must be that as + * expected by the hardware. * * @return the voltage needed to hold (or "cruise") at a given constant velocity */ @@ -143,6 +153,8 @@ public double getDriveKV() { /** * Returns the voltage needed to induce a given acceleration in the motor shaft. Defaults to 0. + * This constant will be provided directly to the hardware. Therefore, the units must be that as + * expected by the hardware. * * @return the voltage needed to induce a given acceleration in the motor shaft */ diff --git a/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java index 83928de0..fb596087 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java +++ b/src/main/java/frc/lib/team3061/drivetrain/Drivetrain.java @@ -368,7 +368,6 @@ public void stop() { */ @Override public void periodic() { - // FIXME: document that TUNING_MODE must be set to true when doing characterization if (Constants.TUNING_MODE) { this.prevSpeeds = new ChassisSpeeds( @@ -652,7 +651,6 @@ public boolean isMoveToPoseEnabled() { } private Command getSystemCheckCommand() { - // FIXME: add system check commands return Commands.sequence(Commands.sequence(Commands.waitSeconds(0.25))) .until(() -> !FaultReporter.getInstance().getFaults(SUBSYSTEM_NAME).isEmpty()) .andThen(Commands.runOnce(() -> drive(0, 0, 0, true, false), this)); diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java index b69e24ae..3669fc45 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOCTRE.java @@ -1,7 +1,5 @@ package frc.lib.team3061.drivetrain; -import static frc.lib.team3061.drivetrain.swerve.SwerveConstants.*; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; @@ -83,21 +81,14 @@ public SwerveModuleSignals(TalonFX driveMotor, TalonFX steerMotor) { RobotConfig.getInstance().getSwerveAngleKP(), RobotConfig.getInstance().getSwerveAngleKI(), RobotConfig.getInstance().getSwerveAngleKD(), - RobotConfig.getInstance().getSwerveAngleKV() - * 2 - * Math.PI, // convert from V/(radians/s) to V/(rotations/s) + RobotConfig.getInstance().getSwerveAngleKV(), RobotConfig.getInstance().getSwerveAngleKS()); - // FIXME: clean this up private static final CustomSlotGains driveGains = new CustomSlotGains( RobotConfig.getInstance().getSwerveDriveKP(), RobotConfig.getInstance().getSwerveDriveKI(), RobotConfig.getInstance().getSwerveDriveKD(), - RobotConfig.getInstance().getDriveKV() - / Conversions.mpsToFalconRPS( - 1.0, - MK4I_L2_WHEEL_CIRCUMFERENCE, - MK4I_L2_DRIVE_GEAR_RATIO), // convert from V/(m/s) to V/(rotations/s) + RobotConfig.getInstance().getDriveKV(), RobotConfig.getInstance().getDriveKS()); // The closed-loop output type to use for the steer motors @@ -539,7 +530,6 @@ public void setGyroOffset(double expectedYaw) { try { m_stateLock.writeLock().lock(); - // FIXME: check the math here not sure if we should add or subtract m_fieldRelativeOffset = getState().Pose.getRotation().plus(Rotation2d.fromDegrees(expectedYaw)); } finally { @@ -557,16 +547,6 @@ public void resetPose(Pose2d pose) { this.seedFieldRelative(pose); } - @Override - public void setDriveMotorVoltage(double voltage) { - // FIXME: implement later with Idle Swerve Request object? - } - - @Override - public void setSteerMotorVoltage(double voltage) { - // FIXME: implement later with Idle Swerve Request object? - } - @Override public void setBrakeMode(boolean enable) { for (SwerveModule swerveModule : this.Modules) { diff --git a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOGeneric.java b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOGeneric.java index c1bb5ff4..b00a5fa9 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOGeneric.java +++ b/src/main/java/frc/lib/team3061/drivetrain/DrivetrainIOGeneric.java @@ -157,8 +157,6 @@ public void updateInputs(DrivetrainIOInputsCollection inputs) { moduleDeltas[index] = new SwerveModulePosition( current.distanceMeters - previous.distanceMeters, current.angle); - // FIXME: I don't think this assignment is needed... - previous.distanceMeters = current.distanceMeters; } Twist2d twist = kinematics.toTwist2d(moduleDeltas); @@ -238,7 +236,7 @@ public void driveFieldRelative( @Override public void driveFieldRelativeFacingAngle( double xVelocity, double yVelocity, Rotation2d targetDirection, boolean isOpenLoop) { - // FIXME: add support for holding a rotation angle + // currently this is not supported and this code is the same as driveFieldRelative this.targetChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( xVelocity, yVelocity, 0.0, Rotation2d.fromDegrees(this.robotRotationDeg)); diff --git a/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java index 46f77f6d..5e8083d5 100644 --- a/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java +++ b/src/main/java/frc/lib/team3061/drivetrain/swerve/SwerveConstants.java @@ -48,7 +48,6 @@ public enum SwerveType { public static final boolean MK4_L2_ANGLE_MOTOR_INVERTED = false; public static final boolean MK4_L2_CAN_CODER_INVERTED = false; - // FIXME: tune this; perhaps set to 0? public static final double OPEN_LOOP_RAMP = 0.25; public static final double CLOSED_LOOP_RAMP = 0.0; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a7fe4ce9..25fbc01f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -37,7 +37,6 @@ public final class Constants { public enum RobotType { ROBOT_2023_NOVA_CTRE, ROBOT_2023_NOVA, - ROBOT_2023_MK4I, ROBOT_DEFAULT, ROBOT_SIMBOT, ROBOT_SIMBOT_CTRE @@ -64,7 +63,6 @@ public static Mode getMode() { case ROBOT_DEFAULT: case ROBOT_2023_NOVA_CTRE: case ROBOT_2023_NOVA: - case ROBOT_2023_MK4I: return RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; case ROBOT_SIMBOT: diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59d77dec..977222d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,7 +39,6 @@ import frc.robot.commands.RotateToAngle; import frc.robot.commands.TeleopSwerve; import frc.robot.configs.DefaultRobotConfig; -import frc.robot.configs.MK4IRobotConfig; import frc.robot.configs.NovaCTRERobotConfig; import frc.robot.configs.NovaRobotConfig; import frc.robot.operator_interface.OISelector; @@ -89,118 +88,19 @@ public RobotContainer() { switch (Constants.getRobot()) { case ROBOT_2023_NOVA_CTRE: { - DrivetrainIO drivetrainIO = new DrivetrainIOCTRE(); - drivetrain = new Drivetrain(drivetrainIO); - - String[] cameraNames = config.getCameraNames(); - VisionIO[] visionIOs = new VisionIO[cameraNames.length]; - for (int i = 0; i < visionIOs.length; i++) { - visionIOs[i] = new VisionIOPhotonVision(cameraNames[i]); - } - vision = new Vision(visionIOs); - // subsystem = new Subsystem(new SubsystemIOTalonFX()); - subsystem = new Subsystem(new SubsystemIO() {}); + createCTRESubsystems(); break; } case ROBOT_DEFAULT: case ROBOT_2023_NOVA: - case ROBOT_2023_MK4I: case ROBOT_SIMBOT: { - int[] driveMotorCANIDs = config.getSwerveDriveMotorCANIDs(); - int[] steerMotorCANDIDs = config.getSwerveSteerMotorCANIDs(); - int[] steerEncoderCANDIDs = config.getSwerveSteerEncoderCANIDs(); - double[] steerOffsets = config.getSwerveSteerOffsets(); - SwerveModuleIO flModule = - new SwerveModuleIOTalonFXPhoenix6( - 0, - driveMotorCANIDs[0], - steerMotorCANDIDs[0], - steerEncoderCANDIDs[0], - steerOffsets[0]); - - SwerveModuleIO frModule = - new SwerveModuleIOTalonFXPhoenix6( - 1, - driveMotorCANIDs[1], - steerMotorCANDIDs[1], - steerEncoderCANDIDs[1], - steerOffsets[1]); - - SwerveModuleIO blModule = - new SwerveModuleIOTalonFXPhoenix6( - 2, - driveMotorCANIDs[2], - steerMotorCANDIDs[2], - steerEncoderCANDIDs[2], - steerOffsets[2]); - - SwerveModuleIO brModule = - new SwerveModuleIOTalonFXPhoenix6( - 3, - driveMotorCANIDs[3], - steerMotorCANDIDs[3], - steerEncoderCANDIDs[3], - steerOffsets[3]); - - GyroIO gyro = new GyroIOPigeon2Phoenix6(config.getGyroCANID()); - DrivetrainIO drivetrainIO = - new DrivetrainIOGeneric(gyro, flModule, frModule, blModule, brModule); - drivetrain = new Drivetrain(drivetrainIO); - - // subsystem = new Subsystem(new SubsystemIOTalonFX()); - subsystem = new Subsystem(new SubsystemIO() {}); - - if (Constants.getRobot() == Constants.RobotType.ROBOT_DEFAULT) { - new Pneumatics(new PneumaticsIORev()); - } - - if (Constants.getRobot() == Constants.RobotType.ROBOT_SIMBOT) { - AprilTagFieldLayout layout; - try { - layout = new AprilTagFieldLayout(VisionConstants.APRILTAG_FIELD_LAYOUT_PATH); - } catch (IOException e) { - layout = new AprilTagFieldLayout(new ArrayList<>(), 16.4592, 8.2296); - } - vision = - new Vision( - new VisionIO[] { - new VisionIOSim( - layout, - drivetrain::getPose, - RobotConfig.getInstance().getRobotToCameraTransforms()[0]) - }); - } else { - String[] cameraNames = config.getCameraNames(); - VisionIO[] visionIOs = new VisionIO[cameraNames.length]; - for (int i = 0; i < visionIOs.length; i++) { - visionIOs[i] = new VisionIOPhotonVision(cameraNames[i]); - } - vision = new Vision(visionIOs); - } + createSubsystems(); break; } case ROBOT_SIMBOT_CTRE: { - DrivetrainIO drivetrainIO = new DrivetrainIOCTRE(); - drivetrain = new Drivetrain(drivetrainIO); - - // new Pneumatics(new PneumaticsIO() {}); - AprilTagFieldLayout layout; - try { - layout = new AprilTagFieldLayout(VisionConstants.APRILTAG_FIELD_LAYOUT_PATH); - } catch (IOException e) { - layout = new AprilTagFieldLayout(new ArrayList<>(), 16.4592, 8.2296); - } - vision = - new Vision( - new VisionIO[] { - new VisionIOSim( - layout, - drivetrain::getPose, - RobotConfig.getInstance().getRobotToCameraTransforms()[0]) - }); - // subsystem = new Subsystem(new SubsystemIO() {}); + createCTRESimSubsystems(); break; } @@ -247,12 +147,104 @@ private void createRobotConfig() { case ROBOT_SIMBOT: config = new NovaRobotConfig(); break; - case ROBOT_2023_MK4I: - config = new MK4IRobotConfig(); - break; } } + private void createCTRESubsystems() { + DrivetrainIO drivetrainIO = new DrivetrainIOCTRE(); + drivetrain = new Drivetrain(drivetrainIO); + + String[] cameraNames = config.getCameraNames(); + VisionIO[] visionIOs = new VisionIO[cameraNames.length]; + for (int i = 0; i < visionIOs.length; i++) { + visionIOs[i] = new VisionIOPhotonVision(cameraNames[i]); + } + vision = new Vision(visionIOs); + + // FIXME: create the hardware-specific subsystem class + subsystem = new Subsystem(new SubsystemIO() {}); + } + + private void createSubsystems() { + int[] driveMotorCANIDs = config.getSwerveDriveMotorCANIDs(); + int[] steerMotorCANDIDs = config.getSwerveSteerMotorCANIDs(); + int[] steerEncoderCANDIDs = config.getSwerveSteerEncoderCANIDs(); + double[] steerOffsets = config.getSwerveSteerOffsets(); + SwerveModuleIO flModule = + new SwerveModuleIOTalonFXPhoenix6( + 0, driveMotorCANIDs[0], steerMotorCANDIDs[0], steerEncoderCANDIDs[0], steerOffsets[0]); + + SwerveModuleIO frModule = + new SwerveModuleIOTalonFXPhoenix6( + 1, driveMotorCANIDs[1], steerMotorCANDIDs[1], steerEncoderCANDIDs[1], steerOffsets[1]); + + SwerveModuleIO blModule = + new SwerveModuleIOTalonFXPhoenix6( + 2, driveMotorCANIDs[2], steerMotorCANDIDs[2], steerEncoderCANDIDs[2], steerOffsets[2]); + + SwerveModuleIO brModule = + new SwerveModuleIOTalonFXPhoenix6( + 3, driveMotorCANIDs[3], steerMotorCANDIDs[3], steerEncoderCANDIDs[3], steerOffsets[3]); + + GyroIO gyro = new GyroIOPigeon2Phoenix6(config.getGyroCANID()); + DrivetrainIO drivetrainIO = + new DrivetrainIOGeneric(gyro, flModule, frModule, blModule, brModule); + drivetrain = new Drivetrain(drivetrainIO); + + // FIXME: create the hardware-specific subsystem class + subsystem = new Subsystem(new SubsystemIO() {}); + + if (Constants.getRobot() == Constants.RobotType.ROBOT_DEFAULT) { + new Pneumatics(new PneumaticsIORev()); + } + + if (Constants.getRobot() == Constants.RobotType.ROBOT_SIMBOT) { + AprilTagFieldLayout layout; + try { + layout = new AprilTagFieldLayout(VisionConstants.APRILTAG_FIELD_LAYOUT_PATH); + } catch (IOException e) { + layout = new AprilTagFieldLayout(new ArrayList<>(), 16.4592, 8.2296); + } + vision = + new Vision( + new VisionIO[] { + new VisionIOSim( + layout, + drivetrain::getPose, + RobotConfig.getInstance().getRobotToCameraTransforms()[0]) + }); + } else { + String[] cameraNames = config.getCameraNames(); + VisionIO[] visionIOs = new VisionIO[cameraNames.length]; + for (int i = 0; i < visionIOs.length; i++) { + visionIOs[i] = new VisionIOPhotonVision(cameraNames[i]); + } + vision = new Vision(visionIOs); + } + } + + private void createCTRESimSubsystems() { + DrivetrainIO drivetrainIO = new DrivetrainIOCTRE(); + drivetrain = new Drivetrain(drivetrainIO); + + AprilTagFieldLayout layout; + try { + layout = new AprilTagFieldLayout(VisionConstants.APRILTAG_FIELD_LAYOUT_PATH); + } catch (IOException e) { + layout = new AprilTagFieldLayout(new ArrayList<>(), 16.4592, 8.2296); + } + vision = + new Vision( + new VisionIO[] { + new VisionIOSim( + layout, + drivetrain::getPose, + RobotConfig.getInstance().getRobotToCameraTransforms()[0]) + }); + + // FIXME: create the hardware-specific subsystem class + } + /** * Creates the field from the defined regions and transition points from one region to its * neighbor. The field is used to generate paths. diff --git a/src/main/java/frc/robot/configs/MK4IRobotConfig.java b/src/main/java/frc/robot/configs/MK4IRobotConfig.java deleted file mode 100644 index f998bbac..00000000 --- a/src/main/java/frc/robot/configs/MK4IRobotConfig.java +++ /dev/null @@ -1,366 +0,0 @@ -package frc.robot.configs; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; - -/* - * Refer to the README for how to represent your robot's configuration. For more information on - * these methods, refer to the documentation in the RobotConfig class. - */ -public class MK4IRobotConfig extends RobotConfig { - - private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 13; - private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 12; - private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 14; - private static final double FRONT_LEFT_MODULE_STEER_OFFSET_ROT = 37.35 / 360.0; - - private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 16; - private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 15; - private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 17; - private static final double FRONT_RIGHT_MODULE_STEER_OFFSET_ROT = 83.49 / 360.0; - - private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 7; - private static final int BACK_LEFT_MODULE_STEER_MOTOR = 6; - private static final int BACK_LEFT_MODULE_STEER_ENCODER = 8; - private static final double BACK_LEFT_MODULE_STEER_OFFSET_ROT = 259.62 / 360.0; - - private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 10; - private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 9; - private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 11; - private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = 9.14 / 360.0; - - private static final int GYRO_ID = 18; - - private static final double TRACKWIDTH_METERS = 0.574675; // 22.625 inches - private static final double WHEELBASE_METERS = 0.619125; // 24.375 inches - private static final double ROBOT_WIDTH_WITH_BUMPERS = 0.8255; // meters - private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.8255; // meters - - /* Angle Motor PID Values */ - private static final double ANGLE_KP = 9.609384164222876; - private static final double ANGLE_KI = 0.0; - private static final double ANGLE_KD = 0.28828152492668624; - - /* Drive Motor PID Values */ - private static final double DRIVE_KP = 0.2402346041055719; - private static final double DRIVE_KI = 0.0; - private static final double DRIVE_KD = 0.013212903225806451; - - private static final double DRIVE_KS = 0.25988; - private static final double DRIVE_KV = 2.46330; - private static final double DRIVE_KA = 0.12872; - - private static final SwerveType SWERVE_TYPE = SwerveType.MK4I; - - private static final double MAX_VELOCITY_METERS_PER_SECOND = 4.78; - private static final double MAX_COAST_VELOCITY_METERS_PER_SECOND = 0.05; - private static final double SLOW_MODE_MULTIPLIER = 0.75; - - private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = 4.0; - private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 2.0 * Math.PI; - - private static final String CAN_BUS_NAME = "canbus1"; - - private static final String CAMERA_NAME = "OV9281"; - - private static final Transform3d ROBOT_TO_CAMERA = - new Transform3d(new Translation3d(0.356, 0.0, 0.465), new Rotation3d(0, 0, 0)); - - private static final int PNEUMATICS_HUB_ID = 20; - private static final int FLOW_SENSOR_CHANNEL = 0; - private static final int REV_HIGH_PRESSURE_SENSOR_CHANNEL = 0; - private static final int REV_LOW_PRESSURE_SENSOR_CHANNEL = 1; - - private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 2.0; - private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2.0; - private static final double AUTO_DRIVE_P_CONTROLLER = 6.0; - private static final double AUTO_DRIVE_I_CONTROLLER = 0.0; - private static final double AUTO_DRIVE_D_CONTROLLER = 0.0; - private static final double AUTO_TURN_P_CONTROLLER = 0.4; - private static final double AUTO_TURN_I_CONTROLLER = 1.6; - private static final double AUTO_TURN_D_CONTROLLER = 1.0; - - // Drive to Pose constants - private static final double DRIVE_TO_POSE_DRIVE_KP = 2.5; - private static final double DRIVE_TO_POSE_DRIVE_KD = 0.0; - private static final double DRIVE_TO_POSE_THETA_KP = 18.0; - private static final double DRIVE_TO_POSE_THETA_KI = 10.0; - private static final double DRIVE_TO_POSE_THETA_KD = 0.0; - private static final double DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS = 0.08; - private static final double DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS = 0.008; - - @Override - public double getSwerveAngleKP() { - return ANGLE_KP; - } - - @Override - public double getSwerveAngleKI() { - return ANGLE_KI; - } - - @Override - public double getSwerveAngleKD() { - return ANGLE_KD; - } - - @Override - public double getSwerveDriveKP() { - return DRIVE_KP; - } - - @Override - public double getSwerveDriveKI() { - return DRIVE_KI; - } - - @Override - public double getSwerveDriveKD() { - return DRIVE_KD; - } - - @Override - public double getDriveKS() { - return DRIVE_KS; - } - - @Override - public double getDriveKV() { - return DRIVE_KV; - } - - @Override - public double getDriveKA() { - return DRIVE_KA; - } - - @Override - public SwerveType getSwerveType() { - return SWERVE_TYPE; - } - - @Override - public int[] getSwerveDriveMotorCANIDs() { - return new int[] { - FRONT_LEFT_MODULE_DRIVE_MOTOR, - FRONT_RIGHT_MODULE_DRIVE_MOTOR, - BACK_LEFT_MODULE_DRIVE_MOTOR, - BACK_RIGHT_MODULE_DRIVE_MOTOR - }; - } - - @Override - public int[] getSwerveSteerMotorCANIDs() { - return new int[] { - FRONT_LEFT_MODULE_STEER_MOTOR, - FRONT_RIGHT_MODULE_STEER_MOTOR, - BACK_LEFT_MODULE_STEER_MOTOR, - BACK_RIGHT_MODULE_STEER_MOTOR - }; - } - - @Override - public int[] getSwerveSteerEncoderCANIDs() { - return new int[] { - FRONT_LEFT_MODULE_STEER_ENCODER, - FRONT_RIGHT_MODULE_STEER_ENCODER, - BACK_LEFT_MODULE_STEER_ENCODER, - BACK_RIGHT_MODULE_STEER_ENCODER - }; - } - - @Override - public double[] getSwerveSteerOffsets() { - return new double[] { - FRONT_LEFT_MODULE_STEER_OFFSET_ROT, - FRONT_RIGHT_MODULE_STEER_OFFSET_ROT, - BACK_LEFT_MODULE_STEER_OFFSET_ROT, - BACK_RIGHT_MODULE_STEER_OFFSET_ROT - }; - } - - @Override - public int getGyroCANID() { - return GYRO_ID; - } - - @Override - public double getTrackwidth() { - return TRACKWIDTH_METERS; - } - - @Override - public double getWheelbase() { - return WHEELBASE_METERS; - } - - @Override - public double getRobotWidthWithBumpers() { - return ROBOT_WIDTH_WITH_BUMPERS; - } - - @Override - public double getRobotLengthWithBumpers() { - return ROBOT_LENGTH_WITH_BUMPERS; - } - - @Override - public Transform3d[] getRobotToCameraTransforms() { - return new Transform3d[] {ROBOT_TO_CAMERA}; - } - - @Override - public double getRobotMaxVelocity() { - return MAX_VELOCITY_METERS_PER_SECOND; - } - - @Override - public double getRobotMaxDriveAcceleration() { - return MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED; - } - - @Override - public double getRobotMaxTurnAcceleration() { - return MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED; - } - - @Override - public double getRobotSlowModeMultiplier() { - return SLOW_MODE_MULTIPLIER; - } - - @Override - public double getRobotMaxCoastVelocity() { - return MAX_COAST_VELOCITY_METERS_PER_SECOND; - } - - @Override - public double getAutoMaxSpeed() { - return AUTO_MAX_SPEED_METERS_PER_SECOND; - } - - @Override - public double getAutoMaxAcceleration() { - return AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED; - } - - @Override - public double getAutoDriveKP() { - return AUTO_DRIVE_P_CONTROLLER; - } - - @Override - public double getAutoDriveKI() { - return AUTO_DRIVE_I_CONTROLLER; - } - - @Override - public double getAutoDriveKD() { - return AUTO_DRIVE_D_CONTROLLER; - } - - @Override - public double getAutoTurnKP() { - return AUTO_TURN_P_CONTROLLER; - } - - @Override - public double getAutoTurnKI() { - return AUTO_TURN_I_CONTROLLER; - } - - @Override - public double getAutoTurnKD() { - return AUTO_TURN_D_CONTROLLER; - } - - @Override - public String getCANBusName() { - return CAN_BUS_NAME; - } - - @Override - public String[] getCameraNames() { - return new String[] {CAMERA_NAME}; - } - - @Override - public int getPneumaticsHubCANID() { - return PNEUMATICS_HUB_ID; - } - - @Override - public int getFlowSensorChannel() { - return FLOW_SENSOR_CHANNEL; - } - - @Override - public int getRevHighPressureSensorChannel() { - return REV_HIGH_PRESSURE_SENSOR_CHANNEL; - } - - @Override - public int getRevLowPressureSensorChannel() { - return REV_LOW_PRESSURE_SENSOR_CHANNEL; - } - - @Override - public double getDriveToPoseDriveKP() { - return DRIVE_TO_POSE_DRIVE_KP; - } - - @Override - public double getDriveToPoseDriveKD() { - return DRIVE_TO_POSE_DRIVE_KD; - } - - @Override - public double getDriveToPoseThetaKI() { - return DRIVE_TO_POSE_THETA_KI; - } - - @Override - public double getDriveToPoseThetaKP() { - return DRIVE_TO_POSE_THETA_KP; - } - - @Override - public double getDriveToPoseThetaKD() { - return DRIVE_TO_POSE_THETA_KD; - } - - @Override - public double getDriveToPoseDriveMaxVelocity() { - return 0.5; - } - - @Override - public double getDriveToPoseDriveMaxAcceleration() { - return getAutoMaxAcceleration(); - } - - @Override - public double getDriveToPoseTurnMaxVelocity() { - return getDriveToPoseDriveMaxVelocity() - / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0); - } - - @Override - public double getDriveToPoseTurnMaxAcceleration() { - return getDriveToPoseDriveMaxAcceleration() - / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0); - } - - @Override - public double getDriveToPoseDriveTolerance() { - return DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS; - } - - @Override - public double getDriveToPoseThetaTolerance() { - return DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS; - } -} diff --git a/src/main/java/frc/robot/configs/NovaCTRERobotConfig.java b/src/main/java/frc/robot/configs/NovaCTRERobotConfig.java index 323f0510..6b58237d 100644 --- a/src/main/java/frc/robot/configs/NovaCTRERobotConfig.java +++ b/src/main/java/frc/robot/configs/NovaCTRERobotConfig.java @@ -1,5 +1,7 @@ package frc.robot.configs; +import static frc.lib.team3061.drivetrain.swerve.SwerveConstants.*; + import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -46,7 +48,8 @@ public class NovaCTRERobotConfig extends RobotConfig { private static final double ANGLE_KD = 0.05; private static final double ANGLE_KS = 0.1891233333; - private static final double ANGLE_KV = 0.4399866667; + private static final double ANGLE_KV = + 0.4399866667 * 2 * Math.PI; // convert from V/(radians/s) to V/(rotations/s) private static final double ANGLE_KA = 0.001663333333; /* Drive Motor PID Values */ @@ -55,7 +58,10 @@ public class NovaCTRERobotConfig extends RobotConfig { private static final double DRIVE_KD = 0.0; private static final double DRIVE_KS = 0.4004375; - private static final double DRIVE_KV = 2.7637325; + private static final double DRIVE_KV = + 2.7637325 + * MK4I_L2_DRIVE_GEAR_RATIO + / MK4I_L2_WHEEL_CIRCUMFERENCE; // convert from V/(m/s) to V/(rotations/s) private static final double DRIVE_KA = 0.0139575; private static final SwerveType SWERVE_TYPE = SwerveType.MK4I; diff --git a/src/main/java/frc/robot/configs/SierraRobotConfig.java b/src/main/java/frc/robot/configs/SierraRobotConfig.java deleted file mode 100644 index 8d044010..00000000 --- a/src/main/java/frc/robot/configs/SierraRobotConfig.java +++ /dev/null @@ -1,359 +0,0 @@ -package frc.robot.configs; - -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import frc.lib.team3061.RobotConfig; -import frc.lib.team3061.drivetrain.swerve.SwerveConstants.SwerveType; - -/* - * Refer to the README for how to represent your robot's configuration. For more information on - * these methods, refer to the documentation in the RobotConfig class. - */ -public class SierraRobotConfig extends RobotConfig { - - private static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 7; - private static final int FRONT_LEFT_MODULE_STEER_MOTOR = 6; - private static final int FRONT_LEFT_MODULE_STEER_ENCODER = 8; - private static final double FRONT_LEFT_MODULE_STEER_OFFSET_ROT = 119.26 / 360.0; - - private static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 13; - private static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 12; - private static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 14; - private static final double FRONT_RIGHT_MODULE_STEER_OFFSET_ROT = 102.44 / 360.0; - - private static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 10; - private static final int BACK_LEFT_MODULE_STEER_MOTOR = 9; - private static final int BACK_LEFT_MODULE_STEER_ENCODER = 11; - private static final double BACK_LEFT_MODULE_STEER_OFFSET_ROT = 172.44 / 360.0; - - private static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 16; - private static final int BACK_RIGHT_MODULE_STEER_MOTOR = 15; - private static final int BACK_RIGHT_MODULE_STEER_ENCODER = 17; - private static final double BACK_RIGHT_MODULE_STEER_OFFSET_ROT = 40.78 / 360.0; - - private static final int GYRO_ID = 18; - - private static final double TRACKWIDTH_METERS = 0.5715; // 22.5 inches - private static final double WHEELBASE_METERS = 0.5969; // 23.5 inches - private static final double ROBOT_WIDTH_WITH_BUMPERS = 0.89; // meters - private static final double ROBOT_LENGTH_WITH_BUMPERS = 0.91; // meters - - /* Angle Motor PID Values */ - private static final double ANGLE_KP = 14.414076246334309; - private static final double ANGLE_KI = 0.0; - private static final double ANGLE_KD = 0.28828152492668624; - - /* Drive Motor PID Values */ - private static final double DRIVE_KP = 0.2402346041055719; - private static final double DRIVE_KI = 0.0; - private static final double DRIVE_KD = 0.0; - - private static final double DRIVE_KS = 0.67841; - private static final double DRIVE_KV = 2.27615; - private static final double DRIVE_KA = 0.12872; - - private static final SwerveType SWERVE_TYPE = SwerveType.MK4; - - private static final double MAX_VELOCITY_METERS_PER_SECOND = 4.25; - private static final double MAX_COAST_VELOCITY_METERS_PER_SECOND = 0.05; - private static final double MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED = 4.0; - private static final double MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 4.0; - private static final double SLOW_MODE_MULTIPLIER = 0.75; - - private static final String CAN_BUS_NAME = ""; - - private static final String CAMERA_NAME = "OV9281"; - - private static final Transform3d ROBOT_TO_CAMERA_0 = - new Transform3d(new Translation3d(0.254, 0, 0.648), new Rotation3d(0, 0, 0)); - - private static final int PNEUMATICS_HUB_ID = 20; - private static final int FLOW_SENSOR_CHANNEL = 0; - private static final int REV_HIGH_PRESSURE_SENSOR_CHANNEL = 0; - private static final int REV_LOW_PRESSURE_SENSOR_CHANNEL = 1; - - private static final double AUTO_MAX_SPEED_METERS_PER_SECOND = 2.0; - private static final double AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 2.0; - private static final double AUTO_DRIVE_P_CONTROLLER = 3.5; - private static final double AUTO_DRIVE_I_CONTROLLER = 0.0; - private static final double AUTO_DRIVE_D_CONTROLLER = 0.0; - private static final double AUTO_TURN_P_CONTROLLER = 10.0; - private static final double AUTO_TURN_I_CONTROLLER = 0.0; - private static final double AUTO_TURN_D_CONTROLLER = 0.0; - - // Drive to Pose constants - private static final double DRIVE_TO_POSE_DRIVE_KP = 2.5; - private static final double DRIVE_TO_POSE_DRIVE_KD = 0.0; - private static final double DRIVE_TO_POSE_THETA_KP = 7.0; - private static final double DRIVE_TO_POSE_THETA_KD = 0.0; - private static final double DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS = 0.01; - private static final double DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS = 0.035; - - @Override - public double getSwerveAngleKP() { - return ANGLE_KP; - } - - @Override - public double getSwerveAngleKI() { - return ANGLE_KI; - } - - @Override - public double getSwerveAngleKD() { - return ANGLE_KD; - } - - @Override - public double getSwerveDriveKP() { - return DRIVE_KP; - } - - @Override - public double getSwerveDriveKI() { - return DRIVE_KI; - } - - @Override - public double getSwerveDriveKD() { - return DRIVE_KD; - } - - @Override - public double getDriveKS() { - return DRIVE_KS; - } - - @Override - public double getDriveKV() { - return DRIVE_KV; - } - - @Override - public double getDriveKA() { - return DRIVE_KA; - } - - @Override - public SwerveType getSwerveType() { - return SWERVE_TYPE; - } - - @Override - public int[] getSwerveDriveMotorCANIDs() { - return new int[] { - FRONT_LEFT_MODULE_DRIVE_MOTOR, - FRONT_RIGHT_MODULE_DRIVE_MOTOR, - BACK_LEFT_MODULE_DRIVE_MOTOR, - BACK_RIGHT_MODULE_DRIVE_MOTOR - }; - } - - @Override - public int[] getSwerveSteerMotorCANIDs() { - return new int[] { - FRONT_LEFT_MODULE_STEER_MOTOR, - FRONT_RIGHT_MODULE_STEER_MOTOR, - BACK_LEFT_MODULE_STEER_MOTOR, - BACK_RIGHT_MODULE_STEER_MOTOR - }; - } - - @Override - public int[] getSwerveSteerEncoderCANIDs() { - return new int[] { - FRONT_LEFT_MODULE_STEER_ENCODER, - FRONT_RIGHT_MODULE_STEER_ENCODER, - BACK_LEFT_MODULE_STEER_ENCODER, - BACK_RIGHT_MODULE_STEER_ENCODER - }; - } - - @Override - public double[] getSwerveSteerOffsets() { - return new double[] { - FRONT_LEFT_MODULE_STEER_OFFSET_ROT, - FRONT_RIGHT_MODULE_STEER_OFFSET_ROT, - BACK_LEFT_MODULE_STEER_OFFSET_ROT, - BACK_RIGHT_MODULE_STEER_OFFSET_ROT - }; - } - - @Override - public int getGyroCANID() { - return GYRO_ID; - } - - @Override - public double getTrackwidth() { - return TRACKWIDTH_METERS; - } - - @Override - public double getWheelbase() { - return WHEELBASE_METERS; - } - - @Override - public double getRobotWidthWithBumpers() { - return ROBOT_WIDTH_WITH_BUMPERS; - } - - @Override - public double getRobotLengthWithBumpers() { - return ROBOT_LENGTH_WITH_BUMPERS; - } - - @Override - public Transform3d[] getRobotToCameraTransforms() { - return new Transform3d[] {ROBOT_TO_CAMERA_0}; - } - - @Override - public double getRobotMaxVelocity() { - return MAX_VELOCITY_METERS_PER_SECOND; - } - - @Override - public double getRobotMaxDriveAcceleration() { - return MAX_DRIVE_ACCELERATION_METERS_PER_SECOND_SQUARED; - } - - @Override - public double getRobotMaxTurnAcceleration() { - return MAX_TURN_ACCELERATION_RADIANS_PER_SECOND_SQUARED; - } - - @Override - public double getRobotSlowModeMultiplier() { - return SLOW_MODE_MULTIPLIER; - } - - @Override - public double getRobotMaxCoastVelocity() { - return MAX_COAST_VELOCITY_METERS_PER_SECOND; - } - - @Override - public double getAutoMaxSpeed() { - return AUTO_MAX_SPEED_METERS_PER_SECOND; - } - - @Override - public double getAutoMaxAcceleration() { - return AUTO_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED; - } - - @Override - public double getAutoDriveKP() { - return AUTO_DRIVE_P_CONTROLLER; - } - - @Override - public double getAutoDriveKI() { - return AUTO_DRIVE_I_CONTROLLER; - } - - @Override - public double getAutoDriveKD() { - return AUTO_DRIVE_D_CONTROLLER; - } - - @Override - public double getAutoTurnKP() { - return AUTO_TURN_P_CONTROLLER; - } - - @Override - public double getAutoTurnKI() { - return AUTO_TURN_I_CONTROLLER; - } - - @Override - public double getAutoTurnKD() { - return AUTO_TURN_D_CONTROLLER; - } - - @Override - public String getCANBusName() { - return CAN_BUS_NAME; - } - - @Override - public String[] getCameraNames() { - return new String[] {CAMERA_NAME}; - } - - @Override - public int getPneumaticsHubCANID() { - return PNEUMATICS_HUB_ID; - } - - @Override - public int getFlowSensorChannel() { - return FLOW_SENSOR_CHANNEL; - } - - @Override - public int getRevHighPressureSensorChannel() { - return REV_HIGH_PRESSURE_SENSOR_CHANNEL; - } - - @Override - public int getRevLowPressureSensorChannel() { - return REV_LOW_PRESSURE_SENSOR_CHANNEL; - } - - @Override - public double getDriveToPoseDriveKP() { - return DRIVE_TO_POSE_DRIVE_KP; - } - - @Override - public double getDriveToPoseDriveKD() { - return DRIVE_TO_POSE_DRIVE_KD; - } - - @Override - public double getDriveToPoseThetaKP() { - return DRIVE_TO_POSE_THETA_KP; - } - - @Override - public double getDriveToPoseThetaKD() { - return DRIVE_TO_POSE_THETA_KD; - } - - @Override - public double getDriveToPoseDriveMaxVelocity() { - return getAutoMaxSpeed(); - } - - @Override - public double getDriveToPoseDriveMaxAcceleration() { - return getAutoMaxAcceleration(); - } - - @Override - public double getDriveToPoseTurnMaxVelocity() { - return getDriveToPoseDriveMaxVelocity() - / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0); - } - - @Override - public double getDriveToPoseTurnMaxAcceleration() { - return getDriveToPoseDriveMaxAcceleration() - / Math.hypot(getTrackwidth() / 2.0, getWheelbase() / 2.0); - } - - @Override - public double getDriveToPoseDriveTolerance() { - return DRIVE_TO_POSE_DRIVE_TOLERANCE_METERS; - } - - @Override - public double getDriveToPoseThetaTolerance() { - return DRIVE_TO_POSE_THETA_TOLERANCE_RADIANS; - } -}