Skip to content

Commit

Permalink
Update to Phoenix 6 and WPILib 2024.1.1-beta-4
Browse files Browse the repository at this point in the history
  • Loading branch information
@rachitkakkar
rachitkakkar committed Dec 24, 2023
1 parent 8230127 commit b90f8d7
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 90 deletions.
2 changes: 1 addition & 1 deletion build.gradle
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
plugins {
id "java-library"
id "maven-publish"
id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3"
id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-4"
id "edu.wpi.first.WpilibTools" version "1.3.0"
}

Expand Down
114 changes: 64 additions & 50 deletions src/main/java/org/lasarobotics/hardware/ctre/CANCoder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,18 @@
import org.littletonrobotics.junction.AutoLog;
import org.littletonrobotics.junction.Logger;

import com.ctre.phoenix.ErrorCode;
import com.ctre.phoenix.sensors.CANCoderStatusFrame;
import com.ctre.phoenix.sensors.SensorTimeBase;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.configs.CANcoderConfiguration;

/** CTRE CANCoder */
public class CANCoder implements LoggableHardware, AutoCloseable {
/** CANCoder Status Frame */
public enum CANCoderFrame {
ABSOLUTE_POSITION,
POSITION,
VELOCITY
}

/** CANCoder ID */
public static class ID {
public final String name;
Expand All @@ -38,48 +44,52 @@ public static class CANCoderInputs {
public double velocity = 0.0;
}

private com.ctre.phoenix.sensors.CANCoder m_canCoder;
private com.ctre.phoenix6.hardware.CANcoder m_canCoder;

private ID m_id;
private CANCoderInputsAutoLogged m_inputs;

private double m_absolutePositionCoefficient = 1.0;
private double m_relativePositionCoefficient = 1.0;
private double m_velocityCoefficient = 1.0;

public CANCoder(ID id) {
this.m_id = id;
this.m_canCoder = new com.ctre.phoenix.sensors.CANCoder(m_id.deviceID, m_id.bus.name);
this.m_canCoder = new com.ctre.phoenix6.hardware.CANcoder(m_id.deviceID, m_id.bus.name);
}

/**
* Gets the absolute position of the sensor.
* The absolute position may be unsigned (for example: [0,360) deg), or signed (for example: [-180,+180) deg). This is determined by a configuration. The default selection is unsigned.
* The units are determined by the coefficient and unit-string configuration params, default is degrees.
* The absolute position may be unsigned (for example: [0,360) deg), or signed (for example: [-180,+180) deg). This is determined by a configuration. The default selection is unsigned.
* The units are determined by the internal coefficient, default is rotations.
* Note: this signal is not affected by calls to SetPosition().
* @return The position of the sensor.
*/
private double getAbsolutePosition() {
return m_canCoder.getAbsolutePosition();
return m_canCoder.getAbsolutePosition().getValue() * m_absolutePositionCoefficient;
}

/**
* Gets the position of the sensor. This may be relative or absolute depending on configuration.
* The units are determined by the coefficient and unit-string configuration params, default is degrees.
* The units are determined by the internal coefficient, default is rotations.
* @return The position of the sensor.
*/
private double getRelativePosition() {
return m_canCoder.getPosition();
return m_canCoder.getPosition().getValue() * m_relativePositionCoefficient;
}

/**
* Gets the velocity of the sensor.
* The units are determined by the coefficient and unit-string configuration params, default is degrees per second.
* @return The Velocity of the sensor.
* The units are determined by the internal coefficient, default is rotations per second.
* @return The velocity of the sensor.
*/
private double getVelocity() {
return m_canCoder.getVelocity();
return m_canCoder.getVelocity().getValue() * m_velocityCoefficient;
}

/**
* Gets the velocity of the sensor.
* The units are determined by the coefficient and unit-string configuration params, default is degrees per second.
* The units are determined by the internal coefficient, default is rotations.
* @return The Velocity of the sensor.
*/
private void updateInputs() {
Expand Down Expand Up @@ -114,73 +124,77 @@ public ID getID() {
return m_id;
}

/**
* Choose what units you want the API to get/set. This also impacts the units displayed in Self-Test in Tuner.
* Depending on your mechanism, you may want to scale rotational units (deg, radians, rotations), or scale to a distance (inches, centimeters).
* @param sensorCoefficient
* Scalar to multiply the CANCoder's native 12-bit resolute sensor. Defaults to 0.087890625 to produce degrees.
* @param unitString
* String holding the unit to report in. This impacts all routines (except for ConfigMagnetOffset) and the self-test in Tuner.
* The string value itself is arbitrary. The max number of letters will depend on firmware versioning, but generally CANCoder
* supports up to eight letters. However, common units such as "centimeters" are supported explicitly despite exceeding the eight-letter limit.
* Default is "deg"
* @param sensorTimeBase
* Desired denominator to report velocity in. This impacts GetVelocity and the reported velocity in self-test in Tuner.
* Default is "Per Second".
* @return Error Code generated by function. 0 indicates no error.
/**
* Sets the Absolute Position coefficent to allow control of units.
* @param absolutePositionCoefficient Scalar to multiply the absolute position by when it is returned. Defaults to 1.0.
*/
public ErrorCode configFeedbackCoefficient(double sensorCoefficient, String unitString, SensorTimeBase sensorTimeBase) {
return m_canCoder.configFeedbackCoefficient(sensorCoefficient, unitString, sensorTimeBase);
public void setAbsolutePositionCoefficient(double absolutePositionCoefficient) {
m_absolutePositionCoefficient = absolutePositionCoefficient;
}

/**
* Sets the position of the sensor to match the magnet's "Absolute Sensor".
* The units are determined by the coefficient and unit-string configuration params, default is degrees.
* @return ErrorCode generated by function. 0 indicates no error.
* Sets the Relative Position coefficent to allow control of units.
* @param relativePositionCoefficient Scalar to multiply the relative position by when it is returned. Defaults to 1.0.
*/
public ErrorCode setPositionToAbsolute() {
return m_canCoder.setPositionToAbsolute();
public void setRelativePositionCoefficient(double relativePositionCoefficient) {
m_relativePositionCoefficient = relativePositionCoefficient;
}

/**
* Sets the Velocity coefficent to allow control of units.
* @param velocityCoefficient Scalar to multiply the absolute Velocity by when it is returned. Defaults to 1.0.
*/
public void setVelocityCoefficient(double velocityCoefficient) {
m_velocityCoefficient = velocityCoefficient;
}

/**
* Sets the position of the sensor to specified value
* The units are determined by the coefficient and unit-string configuration params, default is degrees.
* The units are determined by the coefficient and unit-string configuration params, default is rotations.
* @param position Position to reset to
* @return ErrorCode generated by function. 0 indicates no error.
* @return StatusCode generated by function. 0 indicates no error.
*/
public ErrorCode resetPosition(double position) {
return m_canCoder.setPosition(position);
public StatusCode resetPosition(double position) {
return m_canCoder.setPosition(position / m_relativePositionCoefficient);
}

/**
* Sets the position of the sensor to zero
* The units are determined by the coefficient and unit-string configuration params, default is degrees.
* @return ErrorCode generated by function. 0 indicates no error.
* @return StatusCode generated by function. 0 indicates no error.
*/
public ErrorCode resetPosition() {
public StatusCode resetPosition() {
return resetPosition(0.0);
}

/**
* Configures all persistent settings to defaults (overloaded so timeoutMs is 50 ms).
*
* @return Error Code generated by function. 0 indicates no error.
* @return Status Code generated by function. 0 indicates no error.
*/
public ErrorCode configFactoryDefault() {
return m_canCoder.configFactoryDefault();
public StatusCode configFactoryDefault() {
return m_canCoder.getConfigurator().apply(new CANcoderConfiguration());
}

/**
* Sets the period of the given status frame.
*
* @param statusFrame
* Frame whose period is to be changed.
* @param periodMs
* Period in ms for the given frame.
* @return Error Code generated by function. 0 indicates no error.
* Frame whose period is to be changed. Default is "Absolute Position".
* @param frequencyHz
* Frequency in Hz for the given frame.
* @return Status Code generated by function. 0 indicates no error.
*/
public ErrorCode setStatusFramePeriod(CANCoderStatusFrame statusFrame, int periodMs) {
return m_canCoder.setStatusFramePeriod(statusFrame, periodMs);
public StatusCode setStatusFramePeriod(CANCoderFrame statusFrame, int frequencyHz) {
switch (statusFrame) {
case ABSOLUTE_POSITION:
return m_canCoder.getAbsolutePosition().setUpdateFrequency(frequencyHz);
case POSITION:
return m_canCoder.getPosition().setUpdateFrequency(frequencyHz);
case VELOCITY:
return m_canCoder.getVelocity().setUpdateFrequency(frequencyHz);
}
return m_canCoder.getAbsolutePosition().setUpdateFrequency(frequencyHz);
}

@Override
Expand Down
82 changes: 43 additions & 39 deletions src/main/java/org/lasarobotics/hardware/ctre/Pidgeon2.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,16 +9,22 @@
import org.littletonrobotics.junction.AutoLog;
import org.littletonrobotics.junction.Logger;

import com.ctre.phoenix.ErrorCode;
import com.ctre.phoenix.sensors.Pigeon2.AxisDirection;
import com.ctre.phoenix.sensors.PigeonIMU_ControlFrame;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.configs.MountPoseConfigs;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;

import edu.wpi.first.math.geometry.Rotation2d;

/** CTRE Pidgeon 2.0 */
public class Pidgeon2 implements LoggableHardware, AutoCloseable {
/** Pigeon Status Frame */
public enum PigeonStatusFrame {
PITCH,
YAW,
ROLL
}

/** Pidgeon ID */
public static class ID {
public final String name;
Expand Down Expand Up @@ -50,22 +56,22 @@ public static class Pidgeon2Inputs {
public Rotation2d rotation2d = GlobalConstants.ROTATION_ZERO;
}

private WPI_Pigeon2 m_pidgeon;
private Pigeon2 m_pidgeon;

private ID m_id;
private Pidgeon2InputsAutoLogged m_inputs;

public Pidgeon2(ID id) {
this.m_id = id;
this.m_pidgeon = new WPI_Pigeon2(id.deviceID, id.bus.name);
this.m_pidgeon = new Pigeon2(id.deviceID, id.bus.name);
}

/**
* Get the pitch from the Pigeon
* @return Pitch
*/
private double getPitch() {
return m_pidgeon.getPitch();
return m_pidgeon.getPitch().getValue();
}

/**
Expand Down Expand Up @@ -107,7 +113,7 @@ private Rotation2d getRotation2d() {
* @return Roll
*/
private double getRoll() {
return m_pidgeon.getRoll();
return m_pidgeon.getRoll().getValue();
}

/**
Expand Down Expand Up @@ -162,49 +168,47 @@ public ID getID() {
/**
* Configures all persistent settings to defaults (overloaded so timeoutMs is 50 ms).
*
* @return Error Code generated by function. 0 indicates no error.
* @return Status Code generated by function. 0 indicates no error.
*/
public ErrorCode configFactoryDefault() {
return m_pidgeon.configFactoryDefault();
}

/**
* Configure the Mount Pose using the primary axis.
* This is useful if the Pigeon 2.0 is mounted straight, and you only
* need to describe what axis is forward and what axis is up.
*
* @param forward Axis that points forward from the robot
* @param up Axis that points up from the robot
* @return OK if successful, InvalidParamValue if both forward and up are of the same primary axis, otherwise config return.
*/
public ErrorCode configMountPose(AxisDirection forward, AxisDirection up) {
return m_pidgeon.configMountPose(forward, up);
public StatusCode configFactoryDefault() {
return m_pidgeon.getConfigurator().apply(new Pigeon2Configuration());
}

/**
* Sets the period of the given control frame.
* Configure the Mount Pose using pitch, roll, and yaw.
*
* @param frame
* Frame whose period is to be changed.
* @param periodMs
* Period in ms for the given frame.
* @return Error Code generated by function. 0 indicates no error.
* @param pitch The mounting calibration pitch-component
* @param roll The mounting calibration roll-component
* @param yaw The mounting calibration yaw-component
* @return Status Code of the set command.
*/
public ErrorCode setControlFramePeriod(PigeonIMU_ControlFrame frame, int periodMs) {
return m_pidgeon.setControlFramePeriod(frame, periodMs);
public StatusCode configMountPose(double pitch, double roll, double yaw) {
var toApply = new MountPoseConfigs();
toApply.MountPosePitch = pitch;
toApply.MountPoseRoll = roll;
toApply.MountPoseYaw = yaw;
return m_pidgeon.getConfigurator().apply(toApply);
}

/**
* Sets the period of the given status frame.
*
* @param statusFrame
* Frame whose period is to be changed.
* @param periodMs
* Period in ms for the given frame.
* @return Error Code generated by function. 0 indicates no error.
* Frame whose period is to be changed. Default is "Pitch".
* @param frequencyHz
* Frequency in Hz for the given frame.
* @return Status Code generated by function. 0 indicates no error.
*/
public ErrorCode setStatusFramePeriod(PigeonIMU_StatusFrame statusFrame, int periodMs) {
return m_pidgeon.setStatusFramePeriod(statusFrame, periodMs);
public StatusCode setStatusFramePeriod(PigeonStatusFrame statusFrame, int frequencyHz) {
switch (statusFrame) {
case PITCH:
return m_pidgeon.getPitch().setUpdateFrequency(frequencyHz);
case YAW:
return m_pidgeon.getYaw().setUpdateFrequency(frequencyHz);
case ROLL:
return m_pidgeon.getRoll().setUpdateFrequency(frequencyHz);
}
return m_pidgeon.getPitch().setUpdateFrequency(frequencyHz);
}

/**
Expand Down

0 comments on commit b90f8d7

Please sign in to comment.