diff --git a/config/LeftBackCANCoder.json b/config/LeftBackCANCoder.json new file mode 100644 index 0000000..69ab1f3 --- /dev/null +++ b/config/LeftBackCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -240.0293, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/config/LeftFrontCANCoder.json b/config/LeftFrontCANCoder.json new file mode 100644 index 0000000..1a5edcd --- /dev/null +++ b/config/LeftFrontCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -181.230469, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/config/RightBackCANCoder.json b/config/RightBackCANCoder.json new file mode 100644 index 0000000..236a112 --- /dev/null +++ b/config/RightBackCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -40.86914, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/config/RightFrontCANCoder.json b/config/RightFrontCANCoder.json new file mode 100644 index 0000000..ec93eb5 --- /dev/null +++ b/config/RightFrontCANCoder.json @@ -0,0 +1,47 @@ +{ + "Configs": [ + { + "Name": "Integrated Sensor", + "Type": "IntegratedSensor", + "Description": "Configures how the sensor is processed.", + "Ordinal": 0, + "Values": { + "Absolute Sensor Range": 0, + "Magnet Offset (deg)": -270.615234, + "Sensor Direction": false, + "Sensor Initialization Strategy": 0 + } + }, + { + "Name": "Unit Coefficient And String", + "Type": "UnitCoeffGroup", + "Description": "Describes the units to report in API and Self-Test.", + "Ordinal": 0, + "Values": { + "Sensor Coefficient": 0.087890625, + "Sensor Time Base": 1, + "Unit String": "deg" + } + }, + { + "Name": "Custom Params", + "Type": "CustomParams", + "Description": "These are arbitrary values not used by the firmware.", + "Ordinal": 0, + "Values": { + "Custom Param 0": 0, + "Custom Param 1": 0 + } + }, + { + "Name": "Advanced Sensor and Meas", + "Type": "AdvancedSensorMeas", + "Description": "Group of configs relating to advanced sensor features and filters", + "Ordinal": 0, + "Values": { + "Velocity Period": 100, + "Velocity Window": 64 + } + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 93ee12b..4b8c518 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,6 +7,10 @@ package frc4388.robot; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; /** @@ -18,24 +22,120 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final class DriveConstants { - public static final int DRIVE_PIGEON_ID = 6; + public static final class SwerveDriveConstants { - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - public static final class ArmConstants { - public static final int MIN_ARM_LEN = 0; - public static final int MAX_ARM_LEN = 1; - public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } - public static final class LEDConstants { - public static final int LED_SPARK_ID = 0; + public static final double ROTATION_SPEED = 2.0; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + public static final class IDs { + public static final int LEFT_FRONT_WHEEL_ID = 2; + public static final int LEFT_FRONT_STEER_ID = 3; + public static final int LEFT_FRONT_ENCODER_ID = 10; + + public static final int RIGHT_FRONT_WHEEL_ID = 4; + public static final int RIGHT_FRONT_STEER_ID = 5; + public static final int RIGHT_FRONT_ENCODER_ID = 11; + + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; + + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; } - public static final class OIConstants { - public static final int XBOX_DRIVER_ID = 0; - public static final int XBOX_OPERATOR_ID = 1; + public static final class PIDConstants { + public static final int SWERVE_SLOT_IDX = 0; + public static final int SWERVE_PID_LOOP_IDX = 1; + public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); } + + public static final class AutoConstants { + public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0); + public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0); + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, + new TrapezoidProfile.Constraints(0.0, 0.0) + ); + + public static final double PATH_MAX_VEL = -1; // TODO: find the actual value + public static final double PATH_MAX_ACC = -1; // TODO: find the actual value + } + + public static final class Conversions { + public static final int CANCODER_TICKS_PER_ROTATION = 4096; + + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value + + public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value + public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value + + public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + + public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; + + public static final double TICK_TIME_TO_SECONDS = 10; + public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; + } + + public static final class Configurations { + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value + public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value + + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values + // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2022 SwerveDrive values + + // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2023 translated values (don't work) + // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2023 translated values (don't work) + + // public static final double LEFT_FRONT_ENCODER_OFFSET = 0.0 + 90.0; // * 2023 experimentally-derived values (mostly work) + // public static final double RIGHT_FRONT_ENCODER_OFFSET = 0.0; // * 2023 experimentally-derived values (mostly work) + // public static final double LEFT_BACK_ENCODER_OFFSET = 0.0 + 24.0; // * 2023 experimentally-derived values (mostly work) + // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0 + 45.0 + 180.0; // * 2023 experimentally-derived values (mostly work) + + } + + public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value + + // dimensions + public static final double WIDTH = 18.5; // TODO: find the actual value + public static final double HEIGHT = 18.5; // TODO: find the actual value + public static final double HALF_WIDTH = WIDTH / 2.d; + public static final double HALF_HEIGHT = HEIGHT / 2.d; + + // misc + public static final int TIMEOUT_MS = 30; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + } + + public static final class GyroConstants { + public static final int ID = 14; // TODO: find the actual ID + } + + public static final class ArmConstants { + public static final int MIN_ARM_LEN = 0; + public static final int MAX_ARM_LEN = 1; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + } + + public static final class LEDConstants { + // public static final int LED_SPARK_ID = 0; + + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + } + + public static final class OIConstants { + public static final int XBOX_DRIVER_ID = 0; + public static final int XBOX_OPERATOR_ID = 1; + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e145b38..8fc1ca3 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -25,6 +25,7 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; + /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -75,13 +76,6 @@ public class Robot extends TimedRobot { public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ - // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); @@ -113,7 +107,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 99f5cb1..031cd46 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,11 +10,11 @@ package frc4388.robot; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.subsystems.LED; -import frc4388.utility.LEDPatterns; +import frc4388.robot.commands.AutoBalance; +import frc4388.robot.commands.DriveWithInput; +import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -27,10 +27,12 @@ import frc4388.utility.controller.XboxController; */ public class RobotContainer { /* RobotMap */ - private final RobotMap m_robotMap = new RobotMap(); + public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final LED m_robotLED = new LED(m_robotMap.LEDController); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro); + // private final LED m_robotLED = new LED(m_robotMap.LEDController); + /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); @@ -43,11 +45,19 @@ public class RobotContainer { configureButtonBindings(); /* Default Commands */ - // drives the robot with a two-axis input from the driver controller - // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + + m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive, + () -> getDriverController().getLeftXAxis(), + () -> getDriverController().getLeftYAxis(), + () -> getDriverController().getRightXAxis(), + false)); + + // m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + + } + /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses @@ -56,13 +66,19 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - // test command to spin the robot while pressing A on the driver controller + + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotMap.gyro.reset())); + // .onFalse() /* Operator Buttons */ - // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) - .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + + new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); + + // interrupt button + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + .onTrue(new InstantCommand()); } /** @@ -71,7 +87,7 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // no auto + return new InstantCommand(); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5f17853..93a114d 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,14 +7,12 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_Pigeon2; -import edu.wpi.first.wpilibj.motorcontrol.Spark; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; /** @@ -22,19 +20,103 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { + private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); - public RobotMap() { - configureLEDMotorControllers(); - configureDriveMotorControllers(); - } - /* LED Subsystem */ - public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + public SwerveModule leftFront; + public SwerveModule rightFront; + public SwerveModule leftBack; + public SwerveModule rightBack; - void configureLEDMotorControllers() { + + public RobotMap() { + configureLEDMotorControllers(); + configureDriveMotors(); + } + + /* LED Subsystem */ + // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + + void configureLEDMotorControllers() { - } + } - void configureDriveMotorControllers() { - } + // swerve drive subsystem + public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID); + public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID); + public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID); + + public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID); + public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID); + public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID); + + public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID); + public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID); + public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID); + + public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID); + public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID); + public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID); + + + void configureDriveMotors() { + // config factory default + leftFrontWheel.configFactoryDefault(); + leftFrontSteer.configFactoryDefault(); + + rightFrontWheel.configFactoryDefault(); + rightFrontSteer.configFactoryDefault(); + + leftBackWheel.configFactoryDefault(); + leftBackSteer.configFactoryDefault(); + + rightBackWheel.configFactoryDefault(); + rightBackSteer.configFactoryDefault(); + + // config open loop ramp + leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config closed loop ramp + leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS); + + // config neutral deadband + leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS); + + // initialize SwerveModules + this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469); + this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234); + this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297); + this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142); + } } diff --git a/src/main/java/frc4388/robot/commands/AutoBalance.java b/src/main/java/frc4388/robot/commands/AutoBalance.java new file mode 100644 index 0000000..374de0a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoBalance.java @@ -0,0 +1,51 @@ +// 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 frc4388.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.RobotGyro; + +public class AutoBalance extends PelvicInflammatoryDisease { + RobotGyro gyro; + SwerveDrive drive; + + /** Creates a new AutoBalanceTF2. */ + public AutoBalance(RobotGyro gyro, SwerveDrive drive) { + super(1.0, 0, 0, 0); + + this.gyro = gyro; + this.drive = drive; + + addRequirements(drive); + } + + @Override + public double getError() { + var pitch = gyro.getPitch(); + SmartDashboard.putNumber("pitch", pitch); + return pitch; + } + + @Override + public void runWithOutput(double output) { + double out2 = MathUtil.clamp(output / 40, -.5, .5); + if (Math.abs(gyro.getPitch()) < 3) out2 = 0; + drive.drive(out2, 0, 0, false); + } + + @Override + public void initialize() { + super.initialize(); + // this.gyro.reset(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveWithInput.java b/src/main/java/frc4388/robot/commands/DriveWithInput.java new file mode 100644 index 0000000..3b4d638 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveWithInput.java @@ -0,0 +1,75 @@ +// 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 frc4388.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveWithInput extends CommandBase { + + private final SwerveDrive swerve; + + private final Supplier xSpeed; + private final Supplier ySpeed; + private final Supplier rot; + private final boolean fieldRelative; + + private final SlewRateLimiter xLimiter, yLimiter, rotLimiter; + + /** Creates a new DriveWithInput. */ + public DriveWithInput(SwerveDrive swerve, Supplier xSpeed, Supplier ySpeed, Supplier rot, boolean fieldRelative) { + // Use addRequirements() here to declare subsystem dependencies. + this.swerve = swerve; + this.xSpeed = xSpeed; + this.ySpeed = ySpeed; + this.rot = rot; + this.fieldRelative = fieldRelative; + + this.xLimiter = new SlewRateLimiter(3); + this.yLimiter = new SlewRateLimiter(3); + this.rotLimiter = new SlewRateLimiter(3); + + addRequirements(swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double x = xSpeed.get(); + double y = ySpeed.get(); + double r = rot.get(); + + x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI)); + + swerve.drive(x, y, r, fieldRelative); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + System.out.println("----------------------------------------------------------------"); + System.out.println("DriveWithInput ended"); + System.out.println("Interrupted: " + interrupted); + System.out.println("----------------------------------------------------------------"); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java new file mode 100644 index 0000000..fb6a871 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java @@ -0,0 +1,58 @@ +// 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 frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.utility.Gains; + +public abstract class PelvicInflammatoryDisease extends CommandBase { + protected Gains gains; + private double output = 0; + + /** Creates a new PelvicInflammatoryDisease. */ + public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) { + gains = new Gains(kp, ki, kd, kf, 0); + } + + public PelvicInflammatoryDisease(Gains gains) { + this.gains = gains; + } + + /** produces the error from the setpoint */ + public abstract double getError(); + /** figure it out bitch */ + public abstract void runWithOutput(double output); + + // Called when the command is initially scheduled. + @Override + public void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double error = getError(); + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + runWithOutput(output); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { return false; } +} diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java deleted file mode 100644 index 64861bc..0000000 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ /dev/null @@ -1,85 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import frc4388.robot.Constants.DriveConstants; -import frc4388.utility.RobotGyro; -import frc4388.utility.RobotTime; - -/** - * Add your docs here. - */ -public class DiffDrive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private RobotTime m_robotTime = RobotTime.getInstance(); - - private WPI_TalonFX m_leftFrontMotor; - private WPI_TalonFX m_rightFrontMotor; - private WPI_TalonFX m_leftBackMotor; - private WPI_TalonFX m_rightBackMotor; - private DifferentialDrive m_driveTrain; - private RobotGyro m_gyro; - - /** - * Add your docs here. - */ - public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor, - WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) { - - m_leftFrontMotor = leftFrontMotor; - m_rightFrontMotor = rightFrontMotor; - m_leftBackMotor = leftBackMotor; - m_rightBackMotor = rightBackMotor; - m_driveTrain = driveTrain; - m_gyro = gyro; - } - - @Override - public void periodic() { - m_gyro.updatePigeonDeltas(); - - if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) { - updateSmartDashboard(); - } - } - - /** - * Add your docs here. - */ - public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); - } - - /** - * Add your docs here. - */ - public void tankDriveWithInput(double leftMove, double rightMove) { - m_leftFrontMotor.set(leftMove); - m_rightFrontMotor.set(rightMove); - } - - /** - * Add your docs here. - */ - private void updateSmartDashboard() { - - // Examples of the functionality of RobotGyro - SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon); - SmartDashboard.putNumber("Turn Rate", m_gyro.getRate()); - SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch()); - // SmartDashboard.putData(m_gyro); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java new file mode 100644 index 0000000..d93f892 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -0,0 +1,152 @@ +// 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 frc4388.robot.subsystems; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; + +public class SwerveDrive extends SubsystemBase { + + public SwerveModule leftFront; + public SwerveModule rightFront; + public SwerveModule leftBack; + public SwerveModule rightBack; + + private SwerveModule[] modules; + + private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + + // private SwerveDriveOdometry odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default + + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) { + this.leftFront = leftFront; + this.rightFront = rightFront; + this.leftBack = leftBack; + this.rightBack = rightBack; + + this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; + } + + // WPILib swerve drive example + public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) { + // SwerveModuleState[] states = kinematics.toSwerveModuleStates( + // fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d()) + // : new ChassisSpeeds(xSpeed, ySpeed, rot) + // ); + SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED)); + setModuleStates(states); + } + + /** + * Set each module of the swerve drive to the corresponding desired state. + * @param desiredStates Array of module states to set. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + for (int i = 0; i < desiredStates.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = desiredStates[i]; + module.setDesiredState(state); + } + } + + /** + * Updates the odometry of the SwerveDrive. + */ + // public void updateOdometry() { + // odometry.update( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + // } + + /** + * Gets the odometry of the SwerveDrive. + * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). + */ + // public Pose2d getOdometry() { + // return odometry.getPoseMeters(); + // } + + /** + * Sets the odometry of the SwerveDrive. + * @param pose Pose to set the odometry to. + */ + // public void setOdometry(Pose2d pose) { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // pose + // ); + // } + + /** + * Resets the odometry of the SwerveDrive to 0. + * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions. + */ + // public void resetOdometry() { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // new Pose2d() + // ); + // } + + public SwerveDriveKinematics getKinematics() { + return this.kinematics; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + // updateOdometry(); + } + + /** + * Shifts gear from high to low, or vice versa. + * @param shift true to shift to high, false to shift to low + */ + public void highSpeed(boolean shift) { + this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..4bbbefb --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -0,0 +1,146 @@ +// 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 frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.utility.Gains; + +public class SwerveModule extends SubsystemBase { + public WPI_TalonFX driveMotor; + public WPI_TalonFX angleMotor; + private CANCoder encoder; + + public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; + + /** Creates a new SwerveModule. */ + public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) { + this.driveMotor = driveMotor; + this.angleMotor = angleMotor; + this.encoder = encoder; + + TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + angleConfig.slot0.kP = swerveGains.kP; + angleConfig.slot0.kI = swerveGains.kI; + angleConfig.slot0.kD = swerveGains.kD; + + // use the CANcoder as the remote sensor for the primary TalonFX PID + angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID(); + angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder; + angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; + angleMotor.configAllSettings(angleConfig); + + encoder.configMagnetOffset(offset); + } + + /** + * Get the drive motor of the SwerveModule + * @return the drive motor of the SwerveModule + */ + public WPI_TalonFX getDriveMotor() { + return this.driveMotor; + } + + /** + * Get the angle motor of the SwerveModule + * @return the angle motor of the SwerveModule + */ + public WPI_TalonFX getAngleMotor() { + return this.angleMotor; + } + + /** + * Get the CANcoder of the SwerveModule + * @return the CANcoder of the SwerveModule + */ + public CANCoder getEncoder() { + return this.encoder; + } + + /** + * Get the angle of a SwerveModule as a Rotation2d + * @return the angle of a SwerveModule as a Rotation2d + */ + public Rotation2d getAngle() { + // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. + return Rotation2d.fromDegrees(encoder.getAbsolutePosition()); + } + + public void stop() { + driveMotor.set(0); + angleMotor.set(0); + } + + public void rotateToAngle(double angle) { + angleMotor.set(TalonFXControlMode.Position, angle); + } + + /** + * Get state of swerve module + * @return speed in m/s and angle in degrees + */ + public SwerveModuleState getState() { + return new SwerveModuleState( + Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + getAngle() + ); + } + + /** + * Returns the current position of the SwerveModule + * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. + */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); + } + + /** + * Set the speed and rotation of the SwerveModule from a SwerveModuleState object + * @param desiredState a SwerveModuleState representing the desired new state of the module + */ + public void setDesiredState(SwerveModuleState desiredState) { + Rotation2d currentRotation = this.getAngle(); + + SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + + // calculate the difference between our current rotational position and our new rotational position + Rotation2d rotationDelta = state.angle.minus(currentRotation); + + // calculate the new absolute position of the SwerveModule based on the difference in rotation + double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; + + // convert the CANCoder from its position reading to ticks + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); + angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); + + double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + + driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND); + // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); + } + + public void reset(double position) { + encoder.setPositionToAbsolute(); + } + + public double getCurrent() { + return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent(); + } + + public double getVoltage() { + return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage())); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java new file mode 100644 index 0000000..7cda1e0 --- /dev/null +++ b/src/main/java/frc4388/utility/Gains.java @@ -0,0 +1,77 @@ +// 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 frc4388.utility; + +/** Add your docs here. */ +public class Gains { + public double kP; + public double kI; + public double kD; + public double kF; + public int kIZone; + public double kPeakOutput; + public double kMaxOutput; + public double kMinOutput; + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = kPeakOutput; + this.kMaxOutput = kPeakOutput; + this.kMinOutput = -kPeakOutput; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kPeakOutput = 1.0; + this.kMaxOutput = 1.0; + this.kMinOutput = -1.0; + } + + /** + * Creates Gains object for PIDs + * @param kP The P value. + * @param kI The I value. + * @param kD The D value. + * @param kF The F value. + * @param kIZone The zone of the I value. + * @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0. + * @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0. + */ + public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kF = kF; + this.kIZone = kIZone; + this.kMaxOutput = kMaxOutput; + this.kMinOutput = kMinOutput; + this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput); + } +} \ No newline at end of file diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index ef239ca..a037914 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -7,8 +7,7 @@ package frc4388.utility; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.kauailabs.navx.frc.AHRS; // import edu.wpi.first.wpilibj.GyroBase; @@ -21,18 +20,21 @@ import edu.wpi.first.math.MathUtil; public class RobotGyro implements Gyro { private RobotTime m_robotTime = RobotTime.getInstance(); - private PigeonIMU m_pigeon = null; + private WPI_Pigeon2 m_pigeon = null; private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX private double m_lastPigeonAngle; private double m_deltaPigeonAngle; + private double pitchZero = 0; + private double rollZero = 0; + /** * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro */ - public RobotGyro(PigeonIMU gyro) { + public RobotGyro(WPI_Pigeon2 gyro) { m_pigeon = gyro; m_isGyroAPigeon = true; } @@ -46,6 +48,16 @@ public class RobotGyro implements Gyro { m_isGyroAPigeon = false; } + /** + * Resets yaw, pitch, and roll. + */ + public void resetZeroValues() { + if (!m_isGyroAPigeon) return; + + pitchZero = m_pigeon.getPitch(); + rollZero = m_pigeon.getRoll(); + } + /** * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note * that the getRate() method for a navX will likely be much more accurate than for a pigeon. @@ -75,7 +87,7 @@ public class RobotGyro implements Gyro { @Override public void calibrate() { if (m_isGyroAPigeon) { - m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); + m_pigeon.calibrate(); } else { m_navX.calibrate(); } @@ -83,6 +95,8 @@ public class RobotGyro implements Gyro { @Override public void reset() { + resetZeroValues(); + if (m_isGyroAPigeon) { m_pigeon.setYaw(0); } else { @@ -99,9 +113,10 @@ public class RobotGyro implements Gyro { * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { - double[] angles = new double[3]; - m_pigeon.getYawPitchRoll(angles); - return angles; + double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); + + return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; } @Override @@ -113,6 +128,10 @@ public class RobotGyro implements Gyro { } } + public double getYaw() { + return this.getAngle(); + } + /** * Gets an absolute heading of the robot * @return heading from -180 to 180 degrees @@ -166,7 +185,7 @@ public class RobotGyro implements Gyro { } } - public PigeonIMU getPigeon(){ + public WPI_Pigeon2 getPigeon(){ return m_pigeon; }