From 0cce4674bfeba0e73ffbe05523b4a74f809e4d30 Mon Sep 17 00:00:00 2001 From: dejabot <104333734+dejabot@users.noreply.github.com> Date: Fri, 19 Jan 2024 17:09:33 -0800 Subject: [PATCH] update 2024 code to new SparkMax, Pigeon, CANcoder APIs. (#13) * update 2024 code to new SparkMax, Pigeon, CANcoder APIs. * fixing log argument issues --- src/main/java/com/team766/hal/GyroReader.java | 2 +- .../wpilib/CANSparkMaxMotorController.java | 4 +- .../com/team766/hal/wpilib/PigeonGyro.java | 39 +++++++++++++------ .../team766/hal/wpilib/WPIRobotProvider.java | 2 +- .../java/com/team766/odometry/Odometry.java | 25 +++++++++--- .../robot/common/mechanisms/Drive.java | 12 +++--- .../robot/common/mechanisms/SwerveModule.java | 22 +++++++++-- 7 files changed, 75 insertions(+), 31 deletions(-) diff --git a/src/main/java/com/team766/hal/GyroReader.java b/src/main/java/com/team766/hal/GyroReader.java index f52935b92..05a9749a4 100755 --- a/src/main/java/com/team766/hal/GyroReader.java +++ b/src/main/java/com/team766/hal/GyroReader.java @@ -2,7 +2,7 @@ public interface GyroReader { enum Type { - PIGEON, + Pigeon2, } /** diff --git a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java index 56b2b0a17..c4351a0cd 100644 --- a/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java +++ b/src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java @@ -5,7 +5,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAnalogSensor; +import com.revrobotics.SparkAnalogSensor; import com.team766.hal.MotorController; import com.team766.hal.MotorControllerCommandFailedException; import com.team766.logging.LoggerExceptionUtils; @@ -149,7 +149,7 @@ public void setSelectedFeedbackSensor(final FeedbackDevice feedbackDevice) { switch (feedbackDevice) { case Analog: { - SparkMaxAnalogSensor analog = getAnalog(SparkMaxAnalogSensor.Mode.kAbsolute); + SparkAnalogSensor analog = getAnalog(SparkAnalogSensor.Mode.kAbsolute); revErrorToException(ExceptionTarget.LOG, analog.setInverted(sensorInverted)); sensorPositionSupplier = analog::getPosition; sensorVelocitySupplier = analog::getVelocity; diff --git a/src/main/java/com/team766/hal/wpilib/PigeonGyro.java b/src/main/java/com/team766/hal/wpilib/PigeonGyro.java index 3db9ccb27..30b037562 100644 --- a/src/main/java/com/team766/hal/wpilib/PigeonGyro.java +++ b/src/main/java/com/team766/hal/wpilib/PigeonGyro.java @@ -1,9 +1,16 @@ package com.team766.hal.wpilib; -import com.ctre.phoenix.ErrorCode; -import com.ctre.phoenix.sensors.Pigeon2; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.Pigeon2; import com.team766.hal.GyroReader; - +import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.logging.Severity; + +// TODO: add support for configuring the Pigeon2's mountpose +// https://api.ctr-electronics.com/phoenix6/release/java/com/ctre/phoenix6/hardware/core/CorePigeon2.html +// TODO: add support for getting the robot's heading as a Rotation2d, etc. +// https://api.ctr-electronics.com/phoenix6/release/java/com/ctre/phoenix6/hardware/Pigeon2.html public class PigeonGyro implements GyroReader { private final Pigeon2 pigeon; @@ -12,9 +19,7 @@ public PigeonGyro(int canId, String canBus) { } @Override - public void calibrate() { - pigeon.zeroGyroBiasNow(); - } + public void calibrate() {} @Override public void reset() { @@ -23,23 +28,33 @@ public void reset() { @Override public double getAngle() { - return pigeon.getYaw(); + return pigeon.getAngle(); } @Override public double getPitch() { - return pigeon.getPitch(); + StatusSignal value = pigeon.getPitch(); + if (value.getStatus().isOK()) { + return value.getValueAsDouble(); + } + Logger.get(Category.GYRO) + .logData(Severity.ERROR, "Unable to get pitch: %s", value.toString()); + return 0; } @Override public double getRoll() { - return pigeon.getRoll(); + StatusSignal value = pigeon.getRoll(); + if (value.getStatus().isOK()) { + return value.getValueAsDouble(); + } + Logger.get(Category.GYRO) + .logData(Severity.ERROR, "Unable to get roll: %s", value.toString()); + return 0; } @Override public double getRate() { - double[] xyz = new double[3]; - ErrorCode eCode = pigeon.getRawGyro(xyz); - return eCode == ErrorCode.OK ? xyz[2] : 0; + return pigeon.getRate(); } } diff --git a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java index d0a3774e5..fc4471521 100755 --- a/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java +++ b/src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java @@ -192,7 +192,7 @@ public GyroReader getGyro(final int index, String configPrefix) { .getEnum(GyroReader.Type.class, configPrefix + ".type"); if (type.hasValue()) { - if (type.get() == GyroReader.Type.PIGEON) { + if (type.get() == GyroReader.Type.Pigeon2) { ValueProvider canBus = ConfigFileReader.getInstance().getString(configPrefix + ".CANBus"); return new PigeonGyro(index, canBus.get()); diff --git a/src/main/java/com/team766/odometry/Odometry.java b/src/main/java/com/team766/odometry/Odometry.java index 7bd204533..6f4ef2abe 100644 --- a/src/main/java/com/team766/odometry/Odometry.java +++ b/src/main/java/com/team766/odometry/Odometry.java @@ -1,11 +1,14 @@ package com.team766.odometry; -import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; import com.team766.framework.LoggingBase; import com.team766.hal.GyroReader; import com.team766.hal.MotorController; import com.team766.library.RateLimiter; import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.logging.Severity; import com.team766.robot.gatorade.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -22,7 +25,7 @@ public class Odometry extends LoggingBase { private GyroReader gyro; private MotorController[] motorList; // The order of CANCoders should be the same as in motorList - private CANCoder[] CANCoderList; + private CANcoder[] CANCoderList; private int motorCount; private PointDir[] prevPositions; @@ -55,7 +58,7 @@ public class Odometry extends LoggingBase { public Odometry( GyroReader gyro, MotorController[] motors, - CANCoder[] CANCoders, + CANcoder[] CANCoders, Point[] wheelLocations, double wheelCircumference, double gearRatio, @@ -140,6 +143,18 @@ private void updateCurrentPositions() { */ for (int i = 0; i < motorCount; i++) { + + StatusSignal positionStatus = CANCoderList[i].getAbsolutePosition(); + if (!positionStatus.getStatus().isOK()) { + Logger.get(Category.ODOMETRY) + .logData( + Severity.WARNING, + "Unable to read CANCoder: %s", + positionStatus.getStatus().toString()); + continue; + } + double absolutePosition = positionStatus.getValueAsDouble(); + // prevPositions[i] = new PointDir(currentPosition.getX() + 0.5 * // DISTANCE_BETWEEN_WHEELS / Math.sin(Math.PI / motorCount) * // Math.cos(currentPosition.getHeading() + ((Math.PI + 2 * Math.PI * i) / motorCount)), @@ -149,14 +164,14 @@ private void updateCurrentPositions() { // This following line only works if the average of wheel positions is (0,0) prevPositions[i].set( currentPosition.add(wheelPositions[i]), currPositions[i].getHeading()); - currPositions[i].setHeading(-CANCoderList[i].getAbsolutePosition() + gyroPosition); + currPositions[i].setHeading(-absolutePosition + gyroPosition); angleChange = currPositions[i].getHeading() - prevPositions[i].getHeading(); double yaw = -Math.toRadians(gyro.getAngle()); double roll = Math.toRadians(gyro.getRoll()); double pitch = Math.toRadians(gyro.getPitch()); - double w = Math.toRadians(CANCoderList[i].getAbsolutePosition()); + double w = Math.toRadians(absolutePosition); Vector2D u = new Vector2D(Math.cos(yaw) * Math.cos(pitch), Math.sin(yaw) * Math.cos(pitch)); Vector2D v = diff --git a/src/main/java/com/team766/robot/common/mechanisms/Drive.java b/src/main/java/com/team766/robot/common/mechanisms/Drive.java index ac0f44776..221531ba0 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/Drive.java +++ b/src/main/java/com/team766/robot/common/mechanisms/Drive.java @@ -2,7 +2,7 @@ import static com.team766.robot.gatorade.constants.ConfigConstants.*; -import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix6.hardware.CANcoder; import com.team766.framework.Mechanism; import com.team766.hal.GyroReader; import com.team766.hal.MotorController; @@ -48,10 +48,10 @@ public Drive() { 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); + 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); @@ -64,7 +64,7 @@ public Drive() { currentPosition = new PointDir(0, 0, 0); MotorController[] motorList = new MotorController[] {driveFR, driveFL, driveBL, driveBR}; - CANCoder[] encoderList = new CANCoder[] {encoderFR, encoderFL, encoderBL, encoderBR}; + CANcoder[] encoderList = new CANcoder[] {encoderFR, encoderFL, encoderBL, encoderBR}; Point[] wheelPositions = new Point[] { new Point( diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java index 61f1331b4..53247f6a2 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java +++ b/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java @@ -1,8 +1,12 @@ package com.team766.robot.common.mechanisms; -import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; import com.team766.hal.MotorController; import com.team766.hal.MotorController.ControlMode; +import com.team766.logging.Category; +import com.team766.logging.Logger; +import com.team766.logging.Severity; import com.team766.robot.gatorade.constants.SwerveDriveConstants; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.apache.commons.math3.geometry.euclidean.twod.Vector2D; @@ -15,7 +19,7 @@ public class SwerveModule { private final String modulePlacement; private final MotorController drive; private final MotorController steer; - private final CANCoder encoder; + private final CANcoder encoder; private final double offset; /* @@ -38,7 +42,7 @@ public SwerveModule( String modulePlacement, MotorController drive, MotorController steer, - CANCoder encoder) { + CANcoder encoder) { this.modulePlacement = modulePlacement; this.drive = drive; this.steer = steer; @@ -51,8 +55,18 @@ public SwerveModule( } private double computeEncoderOffset() { + StatusSignal value = encoder.getAbsolutePosition(); + if (!value.getStatus().isOK()) { + Logger.get(Category.DRIVE) + .logData( + Severity.ERROR, + "%s unable to read encoder: %s", + modulePlacement, + value.getStatus().toString()); + return 0; // ?? + } return (steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR) % 360 - - encoder.getAbsolutePosition(); + - value.getValueAsDouble(); } /**