Skip to content
This repository has been archived by the owner on Jan 13, 2025. It is now read-only.

Commit

Permalink
update 2024 code to new SparkMax, Pigeon, CANcoder APIs. (#13)
Browse files Browse the repository at this point in the history 
* update 2024 code to new SparkMax, Pigeon, CANcoder APIs.

* fixing log argument issues
  • Loading branch information
@dejabot
dejabot committed Jan 20, 2024
1 parent 2b93f02 commit e7e8f42
Show file tree
Hide file tree
Showing 7 changed files with 75 additions and 31 deletions.
2 changes: 1 addition & 1 deletion src/main/java/com/team766/hal/GyroReader.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

public interface GyroReader {
enum Type {
PIGEON,
Pigeon2,
}

/**
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/com/team766/hal/wpilib/CANSparkMaxMotorController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
39 changes: 27 additions & 12 deletions src/main/java/com/team766/hal/wpilib/PigeonGyro.java
View file
Original file line number Diff line number Diff line change
@@ -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;

Expand All @@ -12,9 +19,7 @@ public PigeonGyro(int canId, String canBus) {
}

@Override
public void calibrate() {
pigeon.zeroGyroBiasNow();
}
public void calibrate() {}

@Override
public void reset() {
Expand All @@ -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<Double> 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<Double> 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();
}
}
2 changes: 1 addition & 1 deletion src/main/java/com/team766/hal/wpilib/WPIRobotProvider.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<String> canBus =
ConfigFileReader.getInstance().getString(configPrefix + ".CANBus");
return new PigeonGyro(index, canBus.get());
Expand Down
25 changes: 20 additions & 5 deletions src/main/java/com/team766/odometry/Odometry.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -140,6 +143,18 @@ private void updateCurrentPositions() {
*/

for (int i = 0; i < motorCount; i++) {

StatusSignal<Double> 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)),
Expand All @@ -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 =
Expand Down
12 changes: 6 additions & 6 deletions src/main/java/com/team766/robot/common/mechanisms/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand All @@ -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(
Expand Down
22 changes: 18 additions & 4 deletions src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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;

/*
Expand All @@ -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;
Expand All @@ -51,8 +55,18 @@ public SwerveModule(
}

private double computeEncoderOffset() {
StatusSignal<Double> 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();
}

/**
Expand Down

0 comments on commit e7e8f42

Please sign in to comment.