From 35a2ac2cddafcefbc12c077352804e3b9bdf0aee Mon Sep 17 00:00:00 2001 From: dejabot <104333734+dejabot@users.noreply.github.com> Date: Sun, 12 Nov 2023 06:06:50 +0000 Subject: [PATCH 1/5] refactor swerve code into modules prepare for AdvantageKit integration by refactoring swerve code into separate SwerveModules, one for each corner. --- .../com/team766/robot/mechanisms/Drive.java | 191 +++++------------- .../robot/mechanisms/SwerveModule.java | 75 +++++++ 2 files changed, 126 insertions(+), 140 deletions(-) create mode 100644 src/main/java/com/team766/robot/mechanisms/SwerveModule.java diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index 25d4b91..3a3e6cb 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -4,7 +4,6 @@ import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; -import com.team766.hal.MotorController.ControlMode; import com.team766.logging.Category; import static com.team766.robot.constants.ConfigConstants.*; import com.team766.robot.constants.SwerveDriveConstants; @@ -17,39 +16,11 @@ public class Drive extends Mechanism { - // Drive motors - private MotorController m_DriveFR; - private MotorController m_DriveFL; - private MotorController m_DriveBR; - private MotorController m_DriveBL; - - // Motors to turn each wheel in place - private MotorController m_SteerFR; - private MotorController m_SteerFL; - private MotorController m_SteerBR; - private MotorController m_SteerBL; - - // Absolute encoders (cancoders) - private CANCoder e_FrontRight; - private CANCoder e_FrontLeft; - private CANCoder e_BackRight; - private CANCoder e_BackLeft; - - /* - * Specific offsets to align each wheel - * These are assigned upon startup with setEncoderOffset() - */ - private double offsetFR; - private double offsetFL; - private double offsetBR; - private double offsetBL; - - /* - * Factor that converts between motor units and degrees - * Multiply to convert from degrees to motor units - * Divide to convert from motor units to degrees - */ - private final double ENCODER_CONVERSION_FACTOR = (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + // SwerveModules + private final SwerveModule swerveFR; + private final SwerveModule swerveFL; + private final SwerveModule swerveBR; + private final SwerveModule swerveBL; // TODO: rework odometry so it doesn't have to go through drive @@ -58,99 +29,46 @@ public class Drive extends Mechanism { // variable representing current position private static PointDir currentPosition; - // other variables to set up odometry - private MotorController[] motorList; - private CANCoder[] CANCoderList; - private Point[] wheelPositions; - public Drive() { - loggerCategory = Category.DRIVE; - // Initializations of motors - - // Initialize the drive motors - m_DriveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); - m_DriveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); - m_DriveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); - m_DriveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); - - // Initialize the steering motors - m_SteerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); - m_SteerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); - m_SteerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); - m_SteerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); - - // Initialize the encoders - e_FrontRight = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); - e_FrontLeft = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); - e_BackRight = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); - e_BackLeft = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); - - // Current limit for motors to avoid breaker problems - m_DriveFR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveFL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBR.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_DriveBL.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - m_SteerFR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerFL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBR.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - m_SteerBL.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + + // create the drive motors + MotorController driveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); + MotorController driveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); + MotorController driveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); + MotorController driveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); + + // create the steering motors + MotorController steerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); + MotorController steerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); + MotorController steerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); + MotorController steerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); + + // create the encoders + CANCoder encoderFR = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderFL = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBR = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBL = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); + + // initialize the swerve modules + swerveFR = new SwerveModule("FR", driveFR, steerFR, encoderFR); + swerveFL = new SwerveModule("FL", driveFL, steerFL, encoderFL); + swerveBR = new SwerveModule("BR", driveBR, steerBR, encoderBR); + swerveBL = new SwerveModule("BL", driveBL, steerBL, encoderBL); // Sets up odometry currentPosition = new PointDir(0, 0, 0); - motorList = new MotorController[] {m_DriveFR, m_DriveFL, m_DriveBL, - m_DriveBR}; - CANCoderList = new CANCoder[] {e_FrontRight, e_FrontLeft, e_BackLeft, e_BackRight}; - wheelPositions = + MotorController[] motorList = new MotorController[] { driveFR, driveFL, driveBL, + driveBR }; + CANCoder[] encoderList = new CANCoder[] { encoderFR, encoderFL, encoderBR, encoderBL}; + Point[] wheelPositions = new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)}; log("MotorList Length: " + motorList.length); - log("CANCoderList Length: " + CANCoderList.length); - swerveOdometry = new Odometry(motorList, CANCoderList, wheelPositions, OdometryInputConstants.WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); - - // Sets the offset value for each steering motor so that each is aligned - setEncoderOffset(); - } - - /** - * Sets just the steer for a specific module - * Can be used to turn the wheels without moving - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteer(MotorController steerMotor, Vector2D vector, double offset) { - - // Calculates the angle of the vector from -180° to 180° - final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); - - // Add 360 * number of full rotations to vectorTheta, then add offset - final double angleDegrees = vectorTheta + 360*(Math.round((steerMotor.getSensorPosition()/ENCODER_CONVERSION_FACTOR - offset - vectorTheta)/360)) + offset; - - // Sets the degree of the steer wheel - // Needs to multiply by encoderconversionfactor to translate into a unit the motor understands - steerMotor.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR*angleDegrees); - - SmartDashboard.putNumber("Angle", angleDegrees); - } - - /** - * Sets the motion for a specific module - * @param driveMotor the drive motor of this module - * @param steerMotor the steer motor of this module - * @param vector the vector specifying the module's motion - * @param offset the offset for this module - */ - public void controlModuleSteerAndPower(MotorController driveMotor, MotorController steerMotor, Vector2D vector, double offset) { - checkContextOwnership(); - - controlModuleSteer(steerMotor, vector, offset); - - // Sets the power to the magnitude of the vector - driveMotor.set(vector.getNorm()); - + log("CANCoderList Length: " + encoderList.length); + swerveOdometry = new Odometry(motorList, encoderList, wheelPositions, OdometryInputConstants. WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); } /** @@ -162,10 +80,14 @@ public void controlModuleSteerAndPower(MotorController driveMotor, MotorControll public void controlRobotOriented(double x, double y, double turn) { // Finds the vectors for turning and for translation of each module, and adds them // Applies this for each module - controlModuleSteerAndPower(m_DriveFL, m_SteerFL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X).normalize()), offsetFL); - controlModuleSteerAndPower(m_DriveFR, m_SteerFR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X).normalize()), offsetFR); - controlModuleSteerAndPower(m_DriveBR, m_SteerBR, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X).normalize()), offsetBR); - controlModuleSteerAndPower(m_DriveBL, m_SteerBL, new Vector2D(x, y).add(turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X).normalize()), offsetBL); + swerveFL.driveAndSteer(new Vector2D(x, y).add( + turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X).normalize())); + swerveFR.driveAndSteer(new Vector2D(x, y).add( + turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X).normalize())); + swerveBR.driveAndSteer(new Vector2D(x, y).add( + turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X).normalize())); + swerveBL.driveAndSteer(new Vector2D(x, y).add( + turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X).normalize())); } /** @@ -182,36 +104,25 @@ public void controlFieldOriented(double yawRad, double x, double y, double turn) controlRobotOriented(Math.cos(-yawRad) * x - Math.sin(-yawRad) * y, Math.sin(-yawRad) * x + Math.cos(-yawRad) * y, turn); } - /* - * Compares the absolute encoder to the motor encoder to find each motor's offset - * This helps each wheel to always be aligned - */ - public void setEncoderOffset() { - offsetFR = (m_SteerFR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_FrontRight.getAbsolutePosition(); - offsetFL = (m_SteerFL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_FrontLeft.getAbsolutePosition(); - offsetBR = (m_SteerBR.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_BackRight.getAbsolutePosition(); - offsetBL = (m_SteerBL.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - e_BackLeft.getAbsolutePosition(); - } - /* * Stops each drive motor */ public void stopDrive() { checkContextOwnership(); - m_DriveFR.stopMotor(); - m_DriveFL.stopMotor(); - m_DriveBR.stopMotor(); - m_DriveBL.stopMotor(); + swerveFR.stopDrive(); + swerveFL.stopDrive(); + swerveBR.stopDrive(); + swerveBL.stopDrive(); } /* * Turns wheels in a cross formation to prevent robot from moving */ public void setCross() { - controlModuleSteer(m_SteerFL, new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X), offsetFL); - controlModuleSteer(m_SteerFR, new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X), offsetFR); - controlModuleSteer(m_SteerBL, new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X), offsetBL); - controlModuleSteer(m_SteerBR, new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X), offsetBR); + swerveFL.steer(new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X)); + swerveFR.steer(new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X)); + swerveBL.steer(new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X)); + swerveBR.steer(new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X)); } // TODO: rework odometry so it doesn't have to go through drive diff --git a/src/main/java/com/team766/robot/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/mechanisms/SwerveModule.java new file mode 100644 index 0000000..2124cf4 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/SwerveModule.java @@ -0,0 +1,75 @@ +package com.team766.robot.mechanisms; + +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; +import com.ctre.phoenix.sensors.CANCoder; +import com.team766.hal.MotorController; +import com.team766.hal.MotorController.ControlMode; +import com.team766.robot.constants.SwerveDriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class SwerveModule { + private final String modulePlacement; + private final MotorController drive; + private final MotorController steer; + private final CANCoder encoder; + private final double offset; + + /* + * Factor that converts between motor units and degrees + * Multiply to convert from degrees to motor units + * Divide to convert from motor units to degrees + */ + private static final double ENCODER_CONVERSION_FACTOR = (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + + public SwerveModule(String modulePlacement, MotorController drive, MotorController steer, CANCoder encoder) { + this.modulePlacement = modulePlacement; + this.drive = drive; + this.steer = steer; + this.encoder = encoder; + this.offset = computeEncoderOffset(); + + // Current limit for motors to avoid breaker problems + drive.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + steer.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + } + + private double computeEncoderOffset() { + return (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - encoder.getAbsolutePosition(); + } + + /** + * Controls just the steer for this module. + * Can be used to turn the wheels without moving + * @param vector the vector specifying the module's motion + */ + public void steer(Vector2D vector) { + // Calculates the angle of the vector from -180° to 180° + final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); + + // Add 360 * number of full rotations to vectorTheta, then add offset + final double angleDegrees = vectorTheta + 360*(Math.round((steer.getSensorPosition()/ENCODER_CONVERSION_FACTOR - offset - vectorTheta)/360)) + offset; + + // Sets the degree of the steer wheel + // Needs to multiply by encoderconversionfactor to translate into a unit the motor understands + steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR*angleDegrees); + + SmartDashboard.putNumber("[" + modulePlacement + "]" + "Angle", angleDegrees); + } + + /** + * Controls both steer and power (based on the target vector) for this module. + * @param vector the vector specifying the module's motion + */ + public void driveAndSteer(Vector2D vector) { + // apply the steer + steer(vector); + + // sets the power to the magnitude of the vector + // TODO: does this need to be clamped to a specific range? + drive.set(vector.getNorm()); + } + + public void stopDrive() { + drive.stopMotor(); + } +} \ No newline at end of file From 05a3f81886b2312ba4122b2887f0257b728ed491 Mon Sep 17 00:00:00 2001 From: dejabot <104333734+dejabot@users.noreply.github.com> Date: Sun, 12 Nov 2023 06:26:55 +0000 Subject: [PATCH 2/5] checking context ownership in Drive public methods --- src/main/java/com/team766/robot/mechanisms/Drive.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index 3a3e6cb..c558200 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -78,6 +78,8 @@ public Drive() { * @param turn the turn value from the rotation joystick */ public void controlRobotOriented(double x, double y, double turn) { + checkContextOwnership(); + // Finds the vectors for turning and for translation of each module, and adds them // Applies this for each module swerveFL.driveAndSteer(new Vector2D(x, y).add( @@ -98,6 +100,8 @@ public void controlRobotOriented(double x, double y, double turn) { * @param turn the turn value from the rotation joystick */ public void controlFieldOriented(double yawRad, double x, double y, double turn) { + checkContextOwnership(); + // Applies a rotational translation to controlRobotOriented // Counteracts the forward direction changing when the robot turns // TODO: change to inverse rotation matrix (rather than negative angle) @@ -119,6 +123,8 @@ public void stopDrive() { * Turns wheels in a cross formation to prevent robot from moving */ public void setCross() { + checkContextOwnership(); + swerveFL.steer(new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X)); swerveFR.steer(new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X)); swerveBL.steer(new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X)); From 2d3463bfcee282fb998590d9cae7e92c506e81a5 Mon Sep 17 00:00:00 2001 From: dejabot <104333734+dejabot@users.noreply.github.com> Date: Sun, 12 Nov 2023 06:33:11 +0000 Subject: [PATCH 3/5] fix mistaken swap in array order for encoders --- src/main/java/com/team766/robot/mechanisms/Drive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index c558200..003fddd 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -60,7 +60,7 @@ public Drive() { currentPosition = new PointDir(0, 0, 0); MotorController[] motorList = new MotorController[] { driveFR, driveFL, driveBL, driveBR }; - CANCoder[] encoderList = new CANCoder[] { encoderFR, encoderFL, encoderBR, encoderBL}; + CANCoder[] encoderList = new CANCoder[] { encoderFR, encoderFL, encoderBL, encoderBR}; Point[] wheelPositions = new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), @@ -124,7 +124,7 @@ public void stopDrive() { */ public void setCross() { checkContextOwnership(); - + swerveFL.steer(new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X)); swerveFR.steer(new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X)); swerveBL.steer(new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X)); From f8e05fdcbbf0f70d643393a0e8d5b0b9b57e09de Mon Sep 17 00:00:00 2001 From: dejabot <104333734+dejabot@users.noreply.github.com> Date: Sun, 12 Nov 2023 06:37:59 +0000 Subject: [PATCH 4/5] add javadoc for SwerveModule. --- .../robot/mechanisms/SwerveModule.java | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/mechanisms/SwerveModule.java index 2124cf4..5bcf511 100644 --- a/src/main/java/com/team766/robot/mechanisms/SwerveModule.java +++ b/src/main/java/com/team766/robot/mechanisms/SwerveModule.java @@ -7,6 +7,10 @@ import com.team766.robot.constants.SwerveDriveConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +/** + * Encapsulates the motors and encoders used for each physical swerve module and + * provides driving and steering controls for each module. + */ public class SwerveModule { private final String modulePlacement; private final MotorController drive; @@ -21,6 +25,14 @@ public class SwerveModule { */ private static final double ENCODER_CONVERSION_FACTOR = (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + /** + * Creates a new SwerveModule. + * + * @param modulePlacement String description of the placement for this module, eg "FL". + * @param drive Drive MotorController for this module. + * @param steer Steer MotorController for this module. + * @param encoder CANCoder for this module. + */ public SwerveModule(String modulePlacement, MotorController drive, MotorController steer, CANCoder encoder) { this.modulePlacement = modulePlacement; this.drive = drive; @@ -50,7 +62,7 @@ public void steer(Vector2D vector) { final double angleDegrees = vectorTheta + 360*(Math.round((steer.getSensorPosition()/ENCODER_CONVERSION_FACTOR - offset - vectorTheta)/360)) + offset; // Sets the degree of the steer wheel - // Needs to multiply by encoderconversionfactor to translate into a unit the motor understands + // Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor understands steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR*angleDegrees); SmartDashboard.putNumber("[" + modulePlacement + "]" + "Angle", angleDegrees); @@ -65,10 +77,13 @@ public void driveAndSteer(Vector2D vector) { steer(vector); // sets the power to the magnitude of the vector - // TODO: does this need to be clamped to a specific range? + // TODO: does this need to be clamped to a specific range, eg btn -1 and 1? drive.set(vector.getNorm()); } + /** + * Stops the drive motor for this module. + */ public void stopDrive() { drive.stopMotor(); } From 71c25268884fff7a275d8bfd32e74f7e0041f212 Mon Sep 17 00:00:00 2001 From: dejabot <104333734+dejabot@users.noreply.github.com> Date: Sun, 26 Nov 2023 21:38:55 +0000 Subject: [PATCH 5/5] fix code formatting with spotless --- .../com/team766/robot/mechanisms/Drive.java | 321 ++++++++++-------- .../robot/mechanisms/SwerveModule.java | 149 ++++---- 2 files changed, 261 insertions(+), 209 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java index 003fddd..a3fbbaf 100644 --- a/src/main/java/com/team766/robot/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -1,155 +1,192 @@ package com.team766.robot.mechanisms; +import static com.team766.robot.constants.ConfigConstants.*; + import com.ctre.phoenix.sensors.CANCoder; import com.team766.framework.Mechanism; import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; import com.team766.logging.Category; -import static com.team766.robot.constants.ConfigConstants.*; -import com.team766.robot.constants.SwerveDriveConstants; -import com.team766.robot.constants.OdometryInputConstants; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; import com.team766.odometry.Odometry; import com.team766.odometry.Point; import com.team766.odometry.PointDir; +import com.team766.robot.constants.OdometryInputConstants; +import com.team766.robot.constants.SwerveDriveConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; public class Drive extends Mechanism { - // SwerveModules - private final SwerveModule swerveFR; - private final SwerveModule swerveFL; - private final SwerveModule swerveBR; - private final SwerveModule swerveBL; - - // TODO: rework odometry so it doesn't have to go through drive - - // declaration of odometry object - private Odometry swerveOdometry; - // variable representing current position - private static PointDir currentPosition; - - public Drive() { - loggerCategory = Category.DRIVE; - - // create the drive motors - MotorController driveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); - MotorController driveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); - MotorController driveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); - MotorController driveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); - - // create the steering motors - MotorController steerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); - MotorController steerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); - MotorController steerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); - MotorController steerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); - - // create the encoders - CANCoder encoderFR = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); - CANCoder encoderFL = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); - CANCoder encoderBR = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); - CANCoder encoderBL = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); - - // initialize the swerve modules - swerveFR = new SwerveModule("FR", driveFR, steerFR, encoderFR); - swerveFL = new SwerveModule("FL", driveFL, steerFL, encoderFL); - swerveBR = new SwerveModule("BR", driveBR, steerBR, encoderBR); - swerveBL = new SwerveModule("BL", driveBL, steerBL, encoderBL); - - // Sets up odometry - currentPosition = new PointDir(0, 0, 0); - MotorController[] motorList = new MotorController[] { driveFR, driveFL, driveBL, - driveBR }; - CANCoder[] encoderList = new CANCoder[] { encoderFR, encoderFL, encoderBL, encoderBR}; - Point[] wheelPositions = - new Point[] {new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), - new Point(-OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2)}; - log("MotorList Length: " + motorList.length); - log("CANCoderList Length: " + encoderList.length); - swerveOdometry = new Odometry(motorList, encoderList, wheelPositions, OdometryInputConstants. WHEEL_CIRCUMFERENCE, OdometryInputConstants.GEAR_RATIO, OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, OdometryInputConstants.RATE_LIMITER_TIME); - } - - /** - * Maps parameters to robot oriented swerve movement - * @param x the x value for the position joystick - * @param y the y value for the position joystick - * @param turn the turn value from the rotation joystick - */ - public void controlRobotOriented(double x, double y, double turn) { - checkContextOwnership(); - - // Finds the vectors for turning and for translation of each module, and adds them - // Applies this for each module - swerveFL.driveAndSteer(new Vector2D(x, y).add( - turn, new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X).normalize())); - swerveFR.driveAndSteer(new Vector2D(x, y).add( - turn, new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X).normalize())); - swerveBR.driveAndSteer(new Vector2D(x, y).add( - turn, new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X).normalize())); - swerveBL.driveAndSteer(new Vector2D(x, y).add( - turn, new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X).normalize())); - } - - /** - * Uses controlRobotOriented() to control the robot relative to the field - * @param yawRad the robot gyro's current yaw value in radians - * @param x the x value for the position joystick - * @param y the y value for the position joystick - * @param turn the turn value from the rotation joystick - */ - public void controlFieldOriented(double yawRad, double x, double y, double turn) { - checkContextOwnership(); - - // Applies a rotational translation to controlRobotOriented - // Counteracts the forward direction changing when the robot turns - // TODO: change to inverse rotation matrix (rather than negative angle) - controlRobotOriented(Math.cos(-yawRad) * x - Math.sin(-yawRad) * y, Math.sin(-yawRad) * x + Math.cos(-yawRad) * y, turn); - } - - /* - * Stops each drive motor - */ - public void stopDrive() { - checkContextOwnership(); - swerveFR.stopDrive(); - swerveFL.stopDrive(); - swerveBR.stopDrive(); - swerveBL.stopDrive(); - } - - /* - * Turns wheels in a cross formation to prevent robot from moving - */ - public void setCross() { - checkContextOwnership(); - - swerveFL.steer(new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X)); - swerveFR.steer(new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X)); - swerveBL.steer(new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X)); - swerveBR.steer(new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X)); - } - - // TODO: rework odometry so it doesn't have to go through drive - // TODO: figure out why odometry x and y are swapped - public PointDir getCurrentPosition() { - return currentPosition; - } - - public void setCurrentPosition(Point P) { - swerveOdometry.setCurrentPosition(P); - } - - public void resetCurrentPosition() { - swerveOdometry.setCurrentPosition(new Point(0, 0)); - } - - // Odometry - @Override - public void run() { - currentPosition = swerveOdometry.run(); - log(currentPosition.toString()); - SmartDashboard.putString("position", currentPosition.toString()); - } -} \ No newline at end of file + // SwerveModules + private final SwerveModule swerveFR; + private final SwerveModule swerveFL; + private final SwerveModule swerveBR; + private final SwerveModule swerveBL; + + // TODO: rework odometry so it doesn't have to go through drive + + // declaration of odometry object + private Odometry swerveOdometry; + // variable representing current position + private static PointDir currentPosition; + + public Drive() { + loggerCategory = Category.DRIVE; + + // create the drive motors + MotorController driveFR = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_RIGHT); + MotorController driveFL = RobotProvider.instance.getMotor(DRIVE_DRIVE_FRONT_LEFT); + MotorController driveBR = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_RIGHT); + MotorController driveBL = RobotProvider.instance.getMotor(DRIVE_DRIVE_BACK_LEFT); + + // create the steering motors + MotorController steerFR = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_RIGHT); + MotorController steerFL = RobotProvider.instance.getMotor(DRIVE_STEER_FRONT_LEFT); + MotorController steerBR = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_RIGHT); + MotorController steerBL = RobotProvider.instance.getMotor(DRIVE_STEER_BACK_LEFT); + + // create the encoders + CANCoder encoderFR = new CANCoder(2, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderFL = new CANCoder(4, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBR = new CANCoder(3, SwerveDriveConstants.SWERVE_CANBUS); + CANCoder encoderBL = new CANCoder(1, SwerveDriveConstants.SWERVE_CANBUS); + + // initialize the swerve modules + swerveFR = new SwerveModule("FR", driveFR, steerFR, encoderFR); + swerveFL = new SwerveModule("FL", driveFL, steerFL, encoderFL); + swerveBR = new SwerveModule("BR", driveBR, steerBR, encoderBR); + swerveBL = new SwerveModule("BL", driveBL, steerBL, encoderBL); + + // Sets up odometry + currentPosition = new PointDir(0, 0, 0); + MotorController[] motorList = new MotorController[] {driveFR, driveFL, driveBL, driveBR}; + CANCoder[] encoderList = new CANCoder[] {encoderFR, encoderFL, encoderBL, encoderBR}; + Point[] wheelPositions = + new Point[] { + new Point( + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point( + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point( + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2), + new Point( + -OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2, + OdometryInputConstants.DISTANCE_BETWEEN_WHEELS / 2) + }; + log("MotorList Length: " + motorList.length); + log("CANCoderList Length: " + encoderList.length); + swerveOdometry = + new Odometry( + motorList, + encoderList, + wheelPositions, + OdometryInputConstants.WHEEL_CIRCUMFERENCE, + OdometryInputConstants.GEAR_RATIO, + OdometryInputConstants.ENCODER_TO_REVOLUTION_CONSTANT, + OdometryInputConstants.RATE_LIMITER_TIME); + } + + /** + * Maps parameters to robot oriented swerve movement + * @param x the x value for the position joystick + * @param y the y value for the position joystick + * @param turn the turn value from the rotation joystick + */ + public void controlRobotOriented(double x, double y, double turn) { + checkContextOwnership(); + + // Finds the vectors for turning and for translation of each module, and adds them + // Applies this for each module + swerveFL.driveAndSteer( + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.FL_Y, SwerveDriveConstants.FL_X) + .normalize())); + swerveFR.driveAndSteer( + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.FR_Y, SwerveDriveConstants.FR_X) + .normalize())); + swerveBR.driveAndSteer( + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.BR_Y, SwerveDriveConstants.BR_X) + .normalize())); + swerveBL.driveAndSteer( + new Vector2D(x, y) + .add( + turn, + new Vector2D(SwerveDriveConstants.BL_Y, SwerveDriveConstants.BL_X) + .normalize())); + } + + /** + * Uses controlRobotOriented() to control the robot relative to the field + * @param yawRad the robot gyro's current yaw value in radians + * @param x the x value for the position joystick + * @param y the y value for the position joystick + * @param turn the turn value from the rotation joystick + */ + public void controlFieldOriented(double yawRad, double x, double y, double turn) { + checkContextOwnership(); + + // Applies a rotational translation to controlRobotOriented + // Counteracts the forward direction changing when the robot turns + // TODO: change to inverse rotation matrix (rather than negative angle) + controlRobotOriented( + Math.cos(-yawRad) * x - Math.sin(-yawRad) * y, + Math.sin(-yawRad) * x + Math.cos(-yawRad) * y, + turn); + } + + /* + * Stops each drive motor + */ + public void stopDrive() { + checkContextOwnership(); + swerveFR.stopDrive(); + swerveFL.stopDrive(); + swerveBR.stopDrive(); + swerveBL.stopDrive(); + } + + /* + * Turns wheels in a cross formation to prevent robot from moving + */ + public void setCross() { + checkContextOwnership(); + + swerveFL.steer(new Vector2D(SwerveDriveConstants.FL_Y, -SwerveDriveConstants.FL_X)); + swerveFR.steer(new Vector2D(SwerveDriveConstants.FR_Y, -SwerveDriveConstants.FR_X)); + swerveBL.steer(new Vector2D(SwerveDriveConstants.BL_Y, -SwerveDriveConstants.BL_X)); + swerveBR.steer(new Vector2D(SwerveDriveConstants.BR_Y, -SwerveDriveConstants.BR_X)); + } + + // TODO: rework odometry so it doesn't have to go through drive + // TODO: figure out why odometry x and y are swapped + public PointDir getCurrentPosition() { + return currentPosition; + } + + public void setCurrentPosition(Point P) { + swerveOdometry.setCurrentPosition(P); + } + + public void resetCurrentPosition() { + swerveOdometry.setCurrentPosition(new Point(0, 0)); + } + + // Odometry + @Override + public void run() { + currentPosition = swerveOdometry.run(); + log(currentPosition.toString()); + SmartDashboard.putString("position", currentPosition.toString()); + } +} diff --git a/src/main/java/com/team766/robot/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/mechanisms/SwerveModule.java index 5bcf511..46a0da2 100644 --- a/src/main/java/com/team766/robot/mechanisms/SwerveModule.java +++ b/src/main/java/com/team766/robot/mechanisms/SwerveModule.java @@ -1,90 +1,105 @@ package com.team766.robot.mechanisms; -import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; import com.ctre.phoenix.sensors.CANCoder; import com.team766.hal.MotorController; import com.team766.hal.MotorController.ControlMode; import com.team766.robot.constants.SwerveDriveConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; /** - * Encapsulates the motors and encoders used for each physical swerve module and + * Encapsulates the motors and encoders used for each physical swerve module and * provides driving and steering controls for each module. */ public class SwerveModule { - private final String modulePlacement; - private final MotorController drive; - private final MotorController steer; - private final CANCoder encoder; - private final double offset; + private final String modulePlacement; + private final MotorController drive; + private final MotorController steer; + private final CANCoder encoder; + private final double offset; - /* - * Factor that converts between motor units and degrees - * Multiply to convert from degrees to motor units - * Divide to convert from motor units to degrees - */ - private static final double ENCODER_CONVERSION_FACTOR = (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; + /* + * Factor that converts between motor units and degrees + * Multiply to convert from degrees to motor units + * Divide to convert from motor units to degrees + */ + private static final double ENCODER_CONVERSION_FACTOR = + (150.0 / 7.0) /*steering gear ratio*/ * (2048.0 / 360.0) /*encoder units to degrees*/; - /** - * Creates a new SwerveModule. - * - * @param modulePlacement String description of the placement for this module, eg "FL". - * @param drive Drive MotorController for this module. - * @param steer Steer MotorController for this module. - * @param encoder CANCoder for this module. - */ - public SwerveModule(String modulePlacement, MotorController drive, MotorController steer, CANCoder encoder) { - this.modulePlacement = modulePlacement; - this.drive = drive; - this.steer = steer; - this.encoder = encoder; - this.offset = computeEncoderOffset(); + /** + * Creates a new SwerveModule. + * + * @param modulePlacement String description of the placement for this module, eg "FL". + * @param drive Drive MotorController for this module. + * @param steer Steer MotorController for this module. + * @param encoder CANCoder for this module. + */ + public SwerveModule( + String modulePlacement, + MotorController drive, + MotorController steer, + CANCoder encoder) { + this.modulePlacement = modulePlacement; + this.drive = drive; + this.steer = steer; + this.encoder = encoder; + this.offset = computeEncoderOffset(); - // Current limit for motors to avoid breaker problems - drive.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); - steer.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); - } + // Current limit for motors to avoid breaker problems + drive.setCurrentLimit(SwerveDriveConstants.DRIVE_MOTOR_CURRENT_LIMIT); + steer.setCurrentLimit(SwerveDriveConstants.STEER_MOTOR_CURRENT_LIMIT); + } - private double computeEncoderOffset() { - return (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - encoder.getAbsolutePosition(); - } + private double computeEncoderOffset() { + return (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 + - encoder.getAbsolutePosition(); + } - /** - * Controls just the steer for this module. - * Can be used to turn the wheels without moving - * @param vector the vector specifying the module's motion - */ - public void steer(Vector2D vector) { - // Calculates the angle of the vector from -180° to 180° - final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); + /** + * Controls just the steer for this module. + * Can be used to turn the wheels without moving + * @param vector the vector specifying the module's motion + */ + public void steer(Vector2D vector) { + // Calculates the angle of the vector from -180° to 180° + final double vectorTheta = Math.toDegrees(Math.atan2(vector.getY(), vector.getX())); - // Add 360 * number of full rotations to vectorTheta, then add offset - final double angleDegrees = vectorTheta + 360*(Math.round((steer.getSensorPosition()/ENCODER_CONVERSION_FACTOR - offset - vectorTheta)/360)) + offset; + // Add 360 * number of full rotations to vectorTheta, then add offset + final double angleDegrees = + vectorTheta + + 360 + * (Math.round( + (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR + - offset + - vectorTheta) + / 360)) + + offset; - // Sets the degree of the steer wheel - // Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor understands - steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR*angleDegrees); + // Sets the degree of the steer wheel + // Needs to multiply by ENCODER_CONVERSION_FACTOR to translate into a unit the motor + // understands + steer.set(ControlMode.Position, ENCODER_CONVERSION_FACTOR * angleDegrees); - SmartDashboard.putNumber("[" + modulePlacement + "]" + "Angle", angleDegrees); - } + SmartDashboard.putNumber("[" + modulePlacement + "]" + "Angle", angleDegrees); + } - /** - * Controls both steer and power (based on the target vector) for this module. - * @param vector the vector specifying the module's motion - */ - public void driveAndSteer(Vector2D vector) { - // apply the steer - steer(vector); + /** + * Controls both steer and power (based on the target vector) for this module. + * @param vector the vector specifying the module's motion + */ + public void driveAndSteer(Vector2D vector) { + // apply the steer + steer(vector); - // sets the power to the magnitude of the vector - // TODO: does this need to be clamped to a specific range, eg btn -1 and 1? - drive.set(vector.getNorm()); - } + // sets the power to the magnitude of the vector + // TODO: does this need to be clamped to a specific range, eg btn -1 and 1? + drive.set(vector.getNorm()); + } - /** - * Stops the drive motor for this module. - */ - public void stopDrive() { - drive.stopMotor(); - } -} \ No newline at end of file + /** + * Stops the drive motor for this module. + */ + public void stopDrive() { + drive.stopMotor(); + } +}