CALIBRATION_DATASETS = new HashMap<>();
+ static {
+ CALIBRATION_DATASETS.put(SwerveChassis.Module.FRONT_LEFT, FRONT_LEFT);
+ CALIBRATION_DATASETS.put(SwerveChassis.Module.FRONT_RIGHT, FRONT_RIGHT);
+ CALIBRATION_DATASETS.put(SwerveChassis.Module.BACK_LEFT, BACK_LEFT);
+ CALIBRATION_DATASETS.put(SwerveChassis.Module.BACK_RIGHT, BACK_RIGHT);
}
}
diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/DecoyModule.java b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/DecoyModule.java
new file mode 100644
index 00000000..e60c363d
--- /dev/null
+++ b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/DecoyModule.java
@@ -0,0 +1,95 @@
+package edu.greenblitz.pegasus.subsystems.swerve;
+
+import edu.greenblitz.pegasus.utils.PIDObject;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+
+public class DecoyModule implements SwerveModule{
+ public DecoyModule() {
+
+ }
+
+ @Override
+ public void setModuleState(SwerveModuleState moduleState) {
+
+ }
+
+ @Override
+ public void rotateToAngle(double angle) {
+
+ }
+
+ @Override
+ public double getModuleAngle() {
+ return 0;
+ }
+
+ @Override
+ public double getCurrentVelocity() {
+ return 0;
+ }
+
+ @Override
+ public void resetEncoderToValue(double angle) {
+
+ }
+
+ @Override
+ public void resetEncoderToValue() {
+
+ }
+
+ @Override
+ public void resetEncoderByAbsoluteEncoder(SwerveChassis.Module module) {
+
+ }
+
+ @Override
+ public void configLinPID(PIDObject pidObject) {
+
+ }
+
+ @Override
+ public void configAnglePID(PIDObject pidObject) {
+
+ }
+
+ @Override
+ public void setLinSpeed(double speed) {
+
+ }
+
+ @Override
+ public void stop() {
+
+ }
+
+ @Override
+ public double getTargetAngle() {
+ return 0;
+ }
+
+ @Override
+ public double getTargetVel() {
+ return 0;
+ }
+
+ @Override
+ public SwerveModuleState getModuleState() {
+ return new SwerveModuleState();
+ }
+
+ @Override
+ public double getAbsoluteEncoderValue() {
+ return 0;
+ }
+
+ @Override
+ public void setRotPowerOnlyForCalibrations(double power) {
+
+ }
+
+ @Override
+ public void setLinPowerOnlyForCalibrations(double power) {
+
+ }
+}
diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/KazaSwerveModule.java b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/KazaSwerveModule.java
new file mode 100644
index 00000000..4a29617f
--- /dev/null
+++ b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/KazaSwerveModule.java
@@ -0,0 +1,176 @@
+package edu.greenblitz.pegasus.subsystems.swerve;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.CANSparkMaxLowLevel;
+import edu.greenblitz.pegasus.RobotMap;
+import edu.greenblitz.pegasus.utils.Dataset;
+import edu.greenblitz.pegasus.utils.GBMath;
+import edu.greenblitz.pegasus.utils.PIDObject;
+import edu.greenblitz.pegasus.utils.motors.GBSparkMax;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.AnalogInput;
+
+import static edu.greenblitz.pegasus.RobotMap.Pegasus.General.Motors.NEO_PHYSICAL_TICKS_TO_RADIANS;
+
+public class KazaSwerveModule implements SwerveModule {
+
+ public double targetAngle;
+ public double targetVel;
+ private GBSparkMax angleMotor;
+ private GBSparkMax linearMotor;
+ private AnalogInput lamprey;
+ private SimpleMotorFeedforward feedforward;
+
+
+ public KazaSwerveModule(int angleMotorID, int linearMotorID, int lampreyID, boolean linInverted) {
+ //SET ANGLE MOTOR
+ angleMotor = new GBSparkMax(angleMotorID, CANSparkMaxLowLevel.MotorType.kBrushless);
+ angleMotor.config(RobotMap.Pegasus.Swerve.KazaSwerve.baseAngConfObj);
+
+ linearMotor = new GBSparkMax(linearMotorID, CANSparkMaxLowLevel.MotorType.kBrushless);
+ linearMotor.config(RobotMap.Pegasus.Swerve.KazaSwerve.baseLinConfObj.withInverted(linInverted));
+
+ lamprey = new AnalogInput(lampreyID);
+ lamprey.setAverageBits(2);
+ this.feedforward = new SimpleMotorFeedforward(RobotMap.Pegasus.Swerve.ks, RobotMap.Pegasus.Swerve.kv, RobotMap.Pegasus.Swerve.ka);;
+ }
+
+
+ public KazaSwerveModule(KazaSwerveModuleConfigObject KazaModuleConfigObject){
+ this(KazaModuleConfigObject.angleMotorID,
+ KazaModuleConfigObject.linearMotorID,
+ KazaModuleConfigObject.AbsoluteEncoderID,
+ KazaModuleConfigObject.linInverted);
+ }
+
+ public static class KazaSwerveModuleConfigObject{
+ private int angleMotorID;
+ private int linearMotorID;
+ private int AbsoluteEncoderID;
+ private boolean linInverted;
+
+
+
+ public KazaSwerveModuleConfigObject(int angleMotorID, int linearMotorID, int AbsoluteEncoderID, boolean linInverted){
+ this.angleMotorID = angleMotorID;
+ this.linearMotorID = linearMotorID;
+ this.AbsoluteEncoderID = AbsoluteEncoderID;
+ this.linInverted = linInverted;
+ }
+ }
+
+
+
+ /** sets to module to be at the given module state */
+ @Override
+ public void setModuleState(SwerveModuleState moduleState) {
+ setLinSpeed(moduleState.speedMetersPerSecond);
+ rotateToAngle(moduleState.angle.getRadians());
+ }
+
+ /** gets a target angle in radians, sets the internal PIDController to the shortest route to the angle
+ * relative to the encoder module angle*/
+ @Override
+ public void rotateToAngle(double angle) {
+
+ double diff = GBMath.modulo(angle - getModuleAngle(), 2 * Math.PI);
+ diff -= diff > Math.PI ? 2*Math.PI : 0;
+ angle = getModuleAngle() + diff;
+
+ angleMotor.getPIDController().setReference(angle, ControlType.kPosition);
+
+ targetAngle = angle;
+ }
+
+
+ /** get the module angle by radians */
+ @Override
+ public double getModuleAngle() {
+ return angleMotor.getEncoder().getPosition();
+ }
+
+ @Override
+ public double getCurrentVelocity() {
+ return (linearMotor.getEncoder().getVelocity());
+ }
+
+
+
+ /** resetEncoderToValue - reset the angular encoder to RADIANS */
+ @Override
+ public void resetEncoderToValue(double angle) {
+ angleMotor.getEncoder().setPosition(angle);
+ }
+
+ @Override
+ public void resetEncoderToValue(){
+ angleMotor.getEncoder().setPosition(0);
+ }
+
+
+ @Override
+ public void resetEncoderByAbsoluteEncoder(SwerveChassis.Module module){
+ double lampreyDeterminedValue = Calibration.CALIBRATION_DATASETS.get(module).linearlyInterpolate(getAbsoluteEncoderValue())[0] * NEO_PHYSICAL_TICKS_TO_RADIANS;
+ if (Double.isNaN(lampreyDeterminedValue)){
+ throw new RuntimeException("the problem is in the reset encoder function");
+ }
+ resetEncoderToValue(lampreyDeterminedValue);
+ }
+ @Override
+ public void configLinPID(PIDObject pidObject) {
+ linearMotor.configPID(pidObject);
+ }
+
+ @Override
+ public void configAnglePID(PIDObject pidObject) {
+ angleMotor.configPID(pidObject);
+ }
+
+
+ @Override
+ public void setLinSpeed(double speed) {
+ linearMotor.getPIDController().setReference(speed,ControlType.kVelocity, 0, feedforward.calculate(speed));
+ }
+
+ @Override
+ public void stop(){
+ angleMotor.set(0);
+ linearMotor.set(0);
+ }
+
+ //only for debugging
+
+ @Override
+ public double getTargetAngle() {
+ return targetAngle;
+ }
+
+ @Override
+ public double getTargetVel() {
+ return targetVel;
+ }
+
+ @Override
+ public SwerveModuleState getModuleState(){
+ return new SwerveModuleState(getCurrentVelocity(),new Rotation2d(this.getModuleAngle()));
+ }
+
+ /** get the lamprey's angle raw voltage*/
+ @Override
+ public double getAbsoluteEncoderValue(){
+ return lamprey.getVoltage();
+ }
+ @Override
+ public void setRotPowerOnlyForCalibrations(double power){
+ angleMotor.set(power);
+ }
+ @Override
+ public void setLinPowerOnlyForCalibrations(double power){
+ linearMotor.set(power);
+ }
+
+
+}
\ No newline at end of file
diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SdsSwerveModule.java b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SdsSwerveModule.java
new file mode 100644
index 00000000..b970b228
--- /dev/null
+++ b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SdsSwerveModule.java
@@ -0,0 +1,213 @@
+package edu.greenblitz.pegasus.subsystems.swerve;
+
+import com.ctre.phoenix.motorcontrol.*;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.revrobotics.CANSparkMax;
+import edu.greenblitz.pegasus.RobotMap;
+import edu.greenblitz.pegasus.utils.GBMath;
+import edu.greenblitz.pegasus.utils.PIDObject;
+import edu.greenblitz.pegasus.utils.motors.GBFalcon;
+import edu.greenblitz.pegasus.utils.motors.GBSparkMax;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SdsSwerveModule implements SwerveModule{
+ public double targetAngle;
+ public double targetVel;
+ private GBFalcon angleMotor;
+ private GBFalcon linearMotor;
+ private DutyCycleEncoder magEncoder;
+ private SimpleMotorFeedforward feedforward;
+
+ public SdsSwerveModule(int angleMotorID, int linearMotorID, int AbsoluteEncoderID, boolean linInverted,double magEncoderOffset) {
+ //SET ANGLE MOTO
+ angleMotor = new GBFalcon(angleMotorID);
+ angleMotor.config(new GBFalcon.FalconConfObject(RobotMap.Pegasus.Swerve.SdsSwerve.baseAngConfObj));
+
+
+ linearMotor = new GBFalcon(linearMotorID);
+ linearMotor.config(new GBFalcon.FalconConfObject(RobotMap.Pegasus.Swerve.SdsSwerve.baseLinConfObj).withInverted(linInverted));
+
+ magEncoder = new DutyCycleEncoder(AbsoluteEncoderID);
+ this.magEncoder.setPositionOffset(magEncoderOffset);
+ SmartDashboard.putNumber("lol", magEncoder.getPositionOffset());
+
+ this.feedforward = new SimpleMotorFeedforward(RobotMap.Pegasus.Swerve.ks, RobotMap.Pegasus.Swerve.kv, RobotMap.Pegasus.Swerve.ka);;
+ }
+
+ public SdsSwerveModule(SdsSwerveModuleConfigObject SdsModuleConfigObject){
+ this(SdsModuleConfigObject.angleMotorID,
+ SdsModuleConfigObject.linearMotorID,
+ SdsModuleConfigObject.AbsoluteEncoderID,
+ SdsModuleConfigObject.linInverted,
+ SdsModuleConfigObject.magEncoderOffset);
+ }
+
+ public static class SdsSwerveModuleConfigObject{
+ private int angleMotorID;
+ private int linearMotorID;
+ private int AbsoluteEncoderID;
+ private boolean linInverted;
+ private double magEncoderOffset;
+
+ public SdsSwerveModuleConfigObject(int angleMotorID, int linearMotorID, int AbsoluteEncoderID, boolean linInverted,double magEncoderOffset){
+ this.angleMotorID = angleMotorID;
+ this.linearMotorID = linearMotorID;
+ this.AbsoluteEncoderID = AbsoluteEncoderID;
+ this.linInverted = linInverted;
+ this.magEncoderOffset = magEncoderOffset;
+ }
+ }
+
+
+ public static double convertRadsToTicks(double angInRads){
+ return angInRads/RobotMap.Pegasus.Swerve.SdsSwerve.angleTicksToRadians;
+ }
+ public static double convertTicksToRads(double angInTicks){
+ return angInTicks*RobotMap.Pegasus.Swerve.SdsSwerve.angleTicksToRadians;
+ }
+
+ /**
+ * @param moduleState - @class {@link SwerveModuleState} to set the module to (angle and velocity)
+ */
+ @Override
+ public void setModuleState(SwerveModuleState moduleState) {
+ setLinSpeed(moduleState.speedMetersPerSecond);
+ rotateToAngle(moduleState.angle.getRadians());
+ }
+
+ /**
+ * @param angleInRads - angle to turn to in radians
+ */
+ @Override
+ public void rotateToAngle(double angleInRads) {
+ double diff = GBMath.modulo(angleInRads - getModuleAngle(), 2 * Math.PI);
+ diff -= diff > Math.PI ? 2*Math.PI : 0;
+ angleInRads = getModuleAngle() + diff;
+
+ angleMotor.set(ControlMode.Position,convertRadsToTicks(angleInRads));
+
+ targetAngle = angleInRads;
+ }
+
+ /**
+ * @return get the module angle in radians
+ */
+ @Override
+ //maybe not after gear ratio plz check ~ noam
+ public double getModuleAngle() {
+ return convertTicksToRads(angleMotor.getSelectedSensorPosition()) ;
+ }
+
+ /**
+ * @return returns the current linear motor velocity
+ */
+ @Override
+ public double getCurrentVelocity() {
+ return linearMotor.getSelectedSensorVelocity() * RobotMap.Pegasus.Swerve.SdsSwerve.linTicksToMetersPerSecond;
+ }
+
+ /**
+ * @param angleInRads - Position to set for the angular encoder (in raw sensor units).
+ */
+ @Override
+ public void resetEncoderToValue(double angleInRads) {
+ angleMotor.setSelectedSensorPosition(convertRadsToTicks(angleInRads));
+ }
+
+ /**
+ * sets the encoder current position to 0;
+ */
+ @Override
+ public void resetEncoderToValue() {
+ angleMotor.setSelectedSensorPosition(0);
+ }
+
+ @Override
+ public void resetEncoderByAbsoluteEncoder(SwerveChassis.Module module) {
+ angleMotor.setSelectedSensorPosition(getAbsoluteEncoderValue() * RobotMap.Pegasus.Swerve.SdsSwerve.magEncoderTicksToFalconTicks);
+ }
+
+ @Override
+ public void configLinPID(PIDObject pidObject) {
+ linearMotor.configPID(pidObject);
+ }
+
+ @Override
+ public void configAnglePID(PIDObject pidObject) {
+ angleMotor.configPID(pidObject);
+ }
+
+ /**
+ * @param speed - double of the wanted speed (m/s) of the linear motor, uses PID and feedForward
+ */
+ @Override
+ public void setLinSpeed(double speed) {
+ linearMotor.set(
+ TalonFXControlMode.Velocity,
+ speed / RobotMap.Pegasus.Swerve.SdsSwerve.linTicksToMetersPerSecond,
+ DemandType.ArbitraryFeedForward,
+ feedforward.calculate(speed));
+ }
+
+ /**
+ * stops the angular and linear module.
+ */
+ @Override
+ public void stop() {
+ linearMotor.set(ControlMode.PercentOutput,0);
+ angleMotor.set(ControlMode.PercentOutput,0);
+ }
+
+ /**
+ * @return returns the target angle of the angular motor
+ */
+ @Override
+ public double getTargetAngle() {
+ return targetAngle;
+ }
+
+ /**
+ * @return get the linear motor's target velocity
+ */
+ @Override
+ public double getTargetVel() {
+ return targetVel;
+ }
+
+ /**
+ * @return return the @class {@link SwerveModuleState} object of the angle and velocity of the module
+ */
+ @Override
+ public SwerveModuleState getModuleState() {
+ return new SwerveModuleState(
+ getCurrentVelocity(),
+ new Rotation2d(getModuleAngle())
+ );
+ }
+
+ /**
+ * get the magEncoder's angle value in rotations
+ * */
+ @Override
+ public double getAbsoluteEncoderValue() {
+ return magEncoder.get();
+ }
+
+ /**
+ * @param power - double between 1 to -1 to set the percentage of the motors rotation
+ */
+ @Override
+ public void setRotPowerOnlyForCalibrations(double power) {
+ angleMotor.set(ControlMode.PercentOutput, power);
+ }
+ @Override
+ public void setLinPowerOnlyForCalibrations(double power){
+ linearMotor.set(ControlMode.PercentOutput, power);
+ }
+
+}
diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveChassis.java b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveChassis.java
index a0227c08..62c6dc1d 100644
--- a/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveChassis.java
+++ b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveChassis.java
@@ -1,86 +1,78 @@
package edu.greenblitz.pegasus.subsystems.swerve;
import edu.greenblitz.pegasus.RobotMap;
+import edu.greenblitz.pegasus.subsystems.Limelight;
import edu.greenblitz.pegasus.utils.PigeonGyro;
import edu.greenblitz.pegasus.subsystems.GBSubsystem;
import edu.greenblitz.pegasus.utils.GBMath;
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.*;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.greenblitz.pegasus.utils.PigeonGyro;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+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.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
public class SwerveChassis extends GBSubsystem {
private final SwerveModule frontRight, frontLeft, backRight, backLeft;
- // private final PigeonGyro pigeonGyro;
private final PigeonGyro pigeonGyro;
- private final SwerveDriveOdometry localizer;
private final SwerveDriveKinematics kinematics;
+ private final SwerveDrivePoseEstimator poseEstimator;
+ private final Field2d field = new Field2d();
-
+
public enum Module {
- FRONT_RIGHT,
FRONT_LEFT,
- BACK_RIGHT,
- BACK_LEFT
+ FRONT_RIGHT,
+ BACK_LEFT,
+ BACK_RIGHT
}
-
public SwerveChassis() {
-
- this.frontRight = new SwerveModule(
- RobotMap.Pegasus.Swerve.Module1.SteerMotorID,
- RobotMap.Pegasus.Swerve.Module1.linMotorID,
- RobotMap.Pegasus.Swerve.Module1.lampryID,
- RobotMap.Pegasus.Swerve.Module1.INVERTED
- );
-
- this.frontLeft = new SwerveModule(
- RobotMap.Pegasus.Swerve.Module2.SteerMotorID,
- RobotMap.Pegasus.Swerve.Module2.linMotorID,
- RobotMap.Pegasus.Swerve.Module2.lampryID,
- RobotMap.Pegasus.Swerve.Module2.INVERTED
- );
-
- this.backRight = new SwerveModule(
- RobotMap.Pegasus.Swerve.Module3.SteerMotorID,
- RobotMap.Pegasus.Swerve.Module3.linMotorID,
- RobotMap.Pegasus.Swerve.Module3.lampryID,
- RobotMap.Pegasus.Swerve.Module3.INVERTED
- );
-
- this.backLeft = new SwerveModule(
- RobotMap.Pegasus.Swerve.Module4.SteerMotorID,
- RobotMap.Pegasus.Swerve.Module4.linMotorID,
- RobotMap.Pegasus.Swerve.Module4.lampryID,
- RobotMap.Pegasus.Swerve.Module4.INVERTED
- );
+ this.frontLeft = new KazaSwerveModule(RobotMap.Pegasus.Swerve.KazaModule1);
+ this.frontRight = new KazaSwerveModule(RobotMap.Pegasus.Swerve.KazaModule2);
+ this.backLeft = new KazaSwerveModule(RobotMap.Pegasus.Swerve.KazaModule3);
+ this.backRight = new KazaSwerveModule(RobotMap.Pegasus.Swerve.KazaModule4);
this.pigeonGyro = new PigeonGyro(RobotMap.Pegasus.gyro.pigeonID);
-
+
this.kinematics = new SwerveDriveKinematics(
RobotMap.Pegasus.Swerve.SwerveLocationsInSwerveKinematicsCoordinates
);
- this.localizer = new SwerveDriveOdometry(
- this.kinematics,
- new Rotation2d(this.getChassisAngle()),
- RobotMap.Pegasus.Swerve.initialRobotPosition
- );
-
+ this.poseEstimator = new SwerveDrivePoseEstimator(this.getPigeonAngle(), Limelight.getInstance().estimateLocationByVision(), this.kinematics,
+ new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01),
+ new MatBuilder<>(Nat.N1(), Nat.N1()).fill(0.1),//0.02),
+ new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1,0.1,0.01));//0.1, 0.1, 0.01));
+ SmartDashboard.putData("field",getField());
+ field.getObject("apriltag").setPose(RobotMap.Pegasus.Vision.apriltagLocation.toPose2d());
}
-
-
+
+
private static SwerveChassis instance;
+
public static SwerveChassis getInstance() {
- if (instance == null){
+ if (instance == null) {
instance = new SwerveChassis();
}
return instance;
}
+
@Override
- public void periodic() {
- localizer.update(new Rotation2d(getChassisAngle()),
- frontLeft.getModuleState(), frontRight.getModuleState(),
- backLeft.getModuleState(), backRight.getModuleState());
+ public void periodic(){
+ updatePoseEstimation();
+ field.setRobotPose(getRobotPose());
}
/**
@@ -99,85 +91,109 @@ private SwerveModule getModule(Module module) {
}
return null;
}
- /** stops all the modules (power(0)) */
+
+ /**
+ * stops all the modules (power(0))
+ */
public void stop() {
frontRight.stop();
frontLeft.stop();
backRight.stop();
backLeft.stop();
}
+
+ /**
+ * resetting all the angle motor's encoders to 0
+ */
+ public void resetEncodersByCalibrationRod(){
+ getModule(Module.FRONT_LEFT).resetEncoderToValue(0);
+ getModule(Module.FRONT_RIGHT).resetEncoderToValue(0);
+ getModule(Module.BACK_LEFT).resetEncoderToValue(0);
+ getModule(Module.BACK_RIGHT).resetEncoderToValue(0);
+ }
- /** resetting all the angle motor's encoders to 0 */
public void resetAllEncoders() {
- getModule(Module.FRONT_LEFT).resetEncoderByLamprey(Calibration.FRONT_LEFT);
- getModule(Module.FRONT_RIGHT).resetEncoderByLamprey(Calibration.FRONT_RIGHT);
- getModule(Module.BACK_LEFT).resetEncoderByLamprey(Calibration.BACK_LEFT);
- getModule(Module.BACK_RIGHT).resetEncoderByLamprey(Calibration.BACK_RIGHT);
+ getModule(Module.FRONT_LEFT).resetEncoderByAbsoluteEncoder(Module.FRONT_LEFT);
+ getModule(Module.FRONT_RIGHT).resetEncoderByAbsoluteEncoder(Module.FRONT_RIGHT);
+ getModule(Module.BACK_LEFT).resetEncoderByAbsoluteEncoder(Module.BACK_LEFT);
+ getModule(Module.BACK_RIGHT).resetEncoderByAbsoluteEncoder(Module.BACK_RIGHT);
+
}
-
-
-// public void resetAllEncodersByLamprey() {
-// getModule(Module.FRONT_LEFT).resetEncoderByLamprey();
-// getModule(Module.FRONT_RIGHT).resetEncoderByLamprey();
-// getModule(Module.BACK_LEFT).resetEncoderByLamprey();
-// getModule(Module.BACK_RIGHT).resetEncoderByLamprey();
-//
-// }
-//
+
+
+
+
/**
* all code below is self-explanatory - well, after a long time It's maybe not self-explanatory
*
* ALL IN RADIANS, NOT DEGREES
*/
-
- /** get the lamprey voltage of a specific module supposedly between 0 and 5*/
- public double getModuleLampreyVoltage(Module module) {
- return getModule(module).getLampreyVoltage();
+
+ /**
+ * get the absolute encoder value of a specific module
+ */
+ public double getModuleAbsoluteValue(Module module) {
+ return getModule(module).getAbsoluteEncoderValue();
}
-//
-// public double getLampreyAngle(Module module) {
-// return getModule(module).getLampreyAngle();
-// }
+
public double getModuleAngle(Module module) {
return getModule(module).getModuleAngle();
}
-
- /** make the pigeon (gyro) set this angle to be the angle in radians*/
+
+ /**
+ * make the pigeon (gyro) set this angle to be the angle in radians
+ */
public void resetChassisAngle(double angInRads) {
-
+
pigeonGyro.setYaw(angInRads);
}
-
- /** when no parameter is given reset the chassis angle to 0 */
-
- public void resetChassisAngle(){
+
+ /**
+ * when no parameter is given reset the chassis angle to 0
+ */
+
+ public void resetChassisAngle() {
pigeonGyro.setYaw(0);
+ poseEstimator.resetPosition(new Pose2d(), getPigeonAngle());
}
/** returns chassis angle in radians */
- public double getChassisAngle() {
- return GBMath.modulo(pigeonGyro.getYaw(), 2 * Math.PI);
+ private Rotation2d getPigeonAngle() {
+ return new Rotation2d(pigeonGyro.getYaw());
}
- /** get module target angle (radians) */
+ public double getChassisAngle(){
+ return getRobotPose().getRotation().getRadians();
+
+ }
+
+ /**
+ * get module target angle (radians)
+ */
public double getTarget(Module module) {//todo make more informative name
return getModule(module).getTargetAngle();
-
+
}
-
- /** setting module states to all 4 modules */
- public void setModuleStates(SwerveModuleState[] states){
+
+ /**
+ * setting module states to all 4 modules
+ */
+ public void setModuleStates(SwerveModuleState[] states) {
setModuleStateForModule(Module.FRONT_LEFT,
- SwerveModuleState.optimize(states[0],new Rotation2d(getModuleAngle(Module.FRONT_LEFT))));
+ SwerveModuleState.optimize(states[0], new Rotation2d(getModuleAngle(Module.FRONT_LEFT))));
setModuleStateForModule(Module.FRONT_RIGHT,
- SwerveModuleState.optimize(states[1],new Rotation2d(getModuleAngle(Module.FRONT_RIGHT))));
+ SwerveModuleState.optimize(states[1], new Rotation2d(getModuleAngle(Module.FRONT_RIGHT))));
setModuleStateForModule(Module.BACK_LEFT,
- SwerveModuleState.optimize(states[2],new Rotation2d(getModuleAngle(Module.BACK_LEFT))));
+ SwerveModuleState.optimize(states[2], new Rotation2d(getModuleAngle(Module.BACK_LEFT))));
setModuleStateForModule(Module.BACK_RIGHT,
- SwerveModuleState.optimize(states[3],new Rotation2d(getModuleAngle(Module.BACK_RIGHT))));
+ SwerveModuleState.optimize(states[3], new Rotation2d(getModuleAngle(Module.BACK_RIGHT))));
+
-
+ }
+
+ public ChassisSpeeds getCurSpeed(){
+ return kinematics.toChassisSpeeds(frontLeft.getModuleState(),frontRight.getModuleState(),backLeft.getModuleState(),backRight.getModuleState());
}
public void moveByChassisSpeeds(double forwardSpeed, double leftwardSpeed, double angSpeed, double currentAng) {
@@ -190,58 +206,60 @@ public void moveByChassisSpeeds(double forwardSpeed, double leftwardSpeed, doubl
setModuleStates(states);
}
- public SwerveDriveKinematics getKinematics(){
- return this.kinematics;
+ public ChassisSpeeds getChassisSpeeds(){
+ return kinematics.toChassisSpeeds(getModuleState(Module.FRONT_LEFT),
+ getModuleState(Module.FRONT_RIGHT),
+ getModuleState(Module.BACK_LEFT),
+ getModuleState(Module.BACK_RIGHT));
}
- public SwerveDriveOdometry getLocalizer (){
- return this.localizer;
- }
- public Pose2d getLocation(){
- return this.localizer.getPoseMeters();
- }
- public void resetLocalizer(){localizer.resetPosition(new Pose2d(),new Rotation2d());}
- public PigeonGyro getPigeonGyro() {
- return pigeonGyro;
+ public SwerveDriveKinematics getKinematics() {
+ return this.kinematics;
}
- @Deprecated
- public void moveByAngle(double angle, SwerveModule module){
-
+ public PigeonGyro getPigeonGyro() {
+ return pigeonGyro;
}
-
-
- /**
- * moving a single module to radians by power.
- * */
+
/**
* moving a single module by module state
- * */
+ */
private void setModuleStateForModule(Module module, SwerveModuleState state) {
getModule(module).setModuleState(state);
}
-
- /** moving the chassis linear by angle (radians) and speed */
- public void moveChassisLin(double angle, double speed) {
- setModuleStateForModule(Module.FRONT_RIGHT, new SwerveModuleState(speed,new Rotation2d(angle)));
- setModuleStateForModule(Module.FRONT_LEFT, new SwerveModuleState(speed,new Rotation2d(angle)));
- setModuleStateForModule(Module.BACK_RIGHT, new SwerveModuleState(speed,new Rotation2d(angle)));
- setModuleStateForModule(Module.BACK_LEFT, new SwerveModuleState(speed,new Rotation2d(angle)));
+
+ /**
+ * rotates chassis by percentOutput [-1, 1]
+ */
+ public void rotateChassisByPower(double percentOutput) {
+ for (int i = 0; i < Module.values().length; i++) {
+ Translation2d translationFromCenter = RobotMap.Pegasus.Swerve.SwerveLocationsInSwerveKinematicsCoordinates[i];
+ getModule(Module.values()[i]).rotateToAngle(Math.atan2(translationFromCenter.getY(), translationFromCenter.getX()) + Math.PI * 0.5);
+ getModule(Module.values()[i]).setLinPowerOnlyForCalibrations(percentOutput);
+ }
}
-
-
-
+
+
/**
* for calibration purposes
*/
public void rotateModuleByPower(Module module, double power) {
getModule(module).setRotPowerOnlyForCalibrations(power);
}
+ public void updatePoseEstimation(){ //todo NaN protection
+ poseEstimator.update(getPigeonAngle(),
+ frontLeft.getModuleState(), frontRight.getModuleState(),
+ backLeft.getModuleState(), backRight.getModuleState());
+ if(Limelight.getInstance().FindTarget()){poseEstimator.addVisionMeasurement(Limelight.getInstance().estimateLocationByVision(),Limelight.getInstance().getTimeStamp());}
+ }
+ public Pose2d getRobotPose(){return poseEstimator.getEstimatedPosition();}
+
+ public Sendable getField(){return field;}
public SwerveModuleState getModuleState (Module module){
- return getModule(module).getModuleState();
+ return getModule(module).getModuleState();
}
-
+
}
diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveModule.java b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveModule.java
index e474190c..8534fc09 100644
--- a/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveModule.java
+++ b/src/main/java/edu/greenblitz/pegasus/subsystems/swerve/SwerveModule.java
@@ -1,164 +1,41 @@
package edu.greenblitz.pegasus.subsystems.swerve;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMax.ControlType;
-import com.revrobotics.CANSparkMaxLowLevel;
-import edu.greenblitz.pegasus.RobotMap;
-import edu.greenblitz.pegasus.utils.Dataset;
-import edu.greenblitz.pegasus.utils.GBMath;
import edu.greenblitz.pegasus.utils.PIDObject;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.AnalogInput;
-
-public class SwerveModule {
-
-
- private int isReversed = 1;
- public double targetAngle;
- public double targetVel;
- private CANSparkMax angleMotor;
- private CANSparkMax linearMotor;
- private AnalogInput lamprey;
- private SimpleMotorFeedforward feedforward;
- private double wheelCirc;
-
- public SwerveModule (int angleMotorID, int linearMotorID, int lampreyID, boolean linInverted) {
- //SET ANGLE MOTO
- angleMotor = new CANSparkMax(angleMotorID, CANSparkMaxLowLevel.MotorType.kBrushless);
- angleMotor.setSmartCurrentLimit(30);
- angleMotor.setClosedLoopRampRate(0.4);
- angleMotor.setInverted(RobotMap.Pegasus.Swerve.angleMotorInverted);
- angleMotor.setIdleMode(CANSparkMax.IdleMode.kBrake);
- angleMotor.getEncoder().setPositionConversionFactor(RobotMap.Pegasus.Swerve.angleTicksToRadians);
- angleMotor.getEncoder().setVelocityConversionFactor(RobotMap.Pegasus.Swerve.ANG_GEAR_RATIO);
-
- linearMotor = new CANSparkMax(linearMotorID, CANSparkMaxLowLevel.MotorType.kBrushless);
- linearMotor.setSmartCurrentLimit(30);
- linearMotor.setClosedLoopRampRate(0.4);
- linearMotor.setOpenLoopRampRate(0.4);
- linearMotor.setInverted(linInverted);
- linearMotor.getEncoder().setPositionConversionFactor(RobotMap.Pegasus.Swerve.linTicksToMeters);
-
- lamprey = new AnalogInput(lampreyID);
- lamprey.setAverageBits(2);
- configAnglePID(RobotMap.Pegasus.Swerve.angPID);
- configLinPID(RobotMap.Pegasus.Swerve.linPID);
- this.feedforward = new SimpleMotorFeedforward(RobotMap.Pegasus.Swerve.ks, RobotMap.Pegasus.Swerve.kv, RobotMap.Pegasus.Swerve.ka);;
- this.wheelCirc = RobotMap.Pegasus.Swerve.WHEEL_CIRC;
- }
-
-// public double getLampreyAngle() { // in radians;
-// int val = lamprey.getValue();
-// if(val < minLampreyVal) minLampreyVal = val;
-// if(val > maxLampreyVal) maxLampreyVal = val;
-// return (lamprey.getValue() - minLampreyVal) / (maxLampreyVal - minLampreyVal) * Math.PI * 2;
-// }
-
-
- /** sets to module to be at the given module state */
- public void setModuleState(SwerveModuleState moduleState) {
- setLinSpeed(moduleState.speedMetersPerSecond);
- rotateToAngle(moduleState.angle.getRadians());
- }
-
- private void rotateToAngle(double angle) {
-
- double diff = GBMath.modulo(angle - getModuleAngle(), 2 * Math.PI);
- diff -= diff > Math.PI ? 2*Math.PI : 0;
- angle = getModuleAngle() + diff;
-
- angleMotor.getPIDController().setReference(angle, ControlType.kPosition);
-
- targetAngle = angle;
- }
-
-
- /** get the module angle by radians */
- public double getModuleAngle() {
- return angleMotor.getEncoder().getPosition();
- }
-
- public double getCurrentVelocity() {
- return (linearMotor.getEncoder().getVelocity() / RobotMap.Pegasus.Swerve.linTicksToWheelToRPM);
- }
-
- public void rotateByAngle(double angle) {
- angleMotor.getPIDController().setReference(getModuleAngle() + angle, ControlType.kPosition);
- }
-
-// public void resetEncoderByLamprey() {
-// angleMotor.setEncoderAng(getLampreyAngle());
-// }
-
- /** resetEncoderToValue - reset the angular encoder to RADIANS */
- public void resetEncoderToValue(double angle) {
- angleMotor.getEncoder().setPosition(angle);
- }
-
- public void resetEncoderToValue(){
- angleMotor.getEncoder().setPosition(0);
- }
-
- public void resetEncoderByLamprey(Dataset dataset){
- resetEncoderToValue(dataset.linearlyInterpolate(getLampreyVoltage())[0] * RobotMap.Pegasus.Swerve.NEO_PHYSICAL_TICKS_TO_RADIANS);
- }
-
- public void configLinPID(PIDObject pidObject) {
- linearMotor.getPIDController().setP(pidObject.getKp());
- linearMotor.getPIDController().setI(pidObject.getKi());
- linearMotor.getPIDController().setD(pidObject.getKd());
- linearMotor.getPIDController().setFF(pidObject.getKf());
- linearMotor.getPIDController().setIZone(pidObject.getIZone());
- linearMotor.getPIDController().setOutputRange(-pidObject.getMaxPower(), pidObject.getMaxPower());
- }
-
- public void configAnglePID(PIDObject pidObject) {
- angleMotor.getPIDController().setP(pidObject.getKp());
- angleMotor.getPIDController().setI(pidObject.getKi());
- angleMotor.getPIDController().setD(pidObject.getKd());
- angleMotor.getPIDController().setFF(pidObject.getKf());
- angleMotor.getPIDController().setIZone(pidObject.getIZone());
- angleMotor.getPIDController().setOutputRange(-pidObject.getMaxPower(), pidObject.getMaxPower());
- }
-
-
- private void setLinSpeed(double speed) {
- speed *= isReversed;
- linearMotor.getPIDController().setReference(speed,ControlType.kVelocity, 0, feedforward.calculate(speed));
- }
-
- public void stop (){
- angleMotor.set(0);
- linearMotor.set(0);
- }
-
- //only for debugging
-
- public double getTargetAngle() {
- return targetAngle;
- }
-
- public double getTargetVel() {
- return targetVel * isReversed;
- }
-
- public int getIsReversed() {
- return isReversed;
- }
-
- public SwerveModuleState getModuleState (){
- return new SwerveModuleState(getCurrentVelocity(),new Rotation2d(this.getModuleAngle()));
- }
-
- /** get the lamprey's angle raw units (analog to digital converter)*/
- public double getLampreyVoltage(){
- return lamprey.getVoltage();
- }
- public void setRotPowerOnlyForCalibrations(double power){
- angleMotor.set(power);
- }
+public interface SwerveModule {
+ void setModuleState(SwerveModuleState moduleState);
+ void rotateToAngle(double angle);
+ double getModuleAngle();
+
+ double getCurrentVelocity();
+
+
+ void resetEncoderToValue(double angle);
+
+ void resetEncoderToValue();
+
+ // reads the absolute angle from the absolute encoder and puts it into the relative encoder
+ void resetEncoderByAbsoluteEncoder(SwerveChassis.Module module);
+
+ void configLinPID(PIDObject pidObject);
+
+ void configAnglePID(PIDObject pidObject);
+
+ void setLinSpeed(double speed);
+
+ void stop();
+
+ double getTargetAngle();
+
+ double getTargetVel();
+
+ SwerveModuleState getModuleState();
+
+ double getAbsoluteEncoderValue();
+
+ void setRotPowerOnlyForCalibrations(double power);
+
+ void setLinPowerOnlyForCalibrations(double power);
}
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/Dataset.java b/src/main/java/edu/greenblitz/pegasus/utils/Dataset.java
index 0370d864..ad4d8ad2 100644
--- a/src/main/java/edu/greenblitz/pegasus/utils/Dataset.java
+++ b/src/main/java/edu/greenblitz/pegasus/utils/Dataset.java
@@ -14,7 +14,7 @@
public class Dataset {
/**
- * The function is from R to R^n, then the dimension is n + 1. This is because a function as described is a n+1
+ * The function is from R to R^n, then the dimension is n + 1. This is because a function as described is an n+1
* dimensional curve.
*/
private int dimension;
@@ -106,8 +106,24 @@ public TwoTuple, TwoTuple> getAdjes
* known sample.
*/
public double[] linearlyInterpolate(double x){
+ if(Double.isNaN(x)){
+ throw new RuntimeException("x is NaN");
+ }
TwoTuple, TwoTuple> data = getAdjesent(x);
- double weight = (x - data.getFirst().getFirst()) / (data.getSecond().getFirst() - data.getFirst().getFirst());
+ double weight;
+ if(x == data.getFirst().getFirst()){
+ weight = 0;
+ }
+ else {
+ double denom = data.getSecond().getFirst() - data.getFirst().getFirst();
+ if (denom == 0) {
+ throw new RuntimeException("denom is 0" + x + " : " + data.getFirst().getFirst() + " : " + data.getSecond().getFirst());
+ }
+ weight = (x - data.getFirst().getFirst()) / (denom);
+ if (Double.isNaN(weight)) {
+ throw new RuntimeException("weight is NaN");
+ }
+ }
double[] ret = new double[dimension];
for (int i = 0; i < dimension - 1; i++){
ret[i] = data.getFirst().getSecond()[i] +
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/DigitalInputMap.java b/src/main/java/edu/greenblitz/pegasus/utils/DigitalInputMap.java
deleted file mode 100644
index f432a8c8..00000000
--- a/src/main/java/edu/greenblitz/pegasus/utils/DigitalInputMap.java
+++ /dev/null
@@ -1,43 +0,0 @@
-package edu.greenblitz.pegasus.utils;
-
-import edu.greenblitz.pegasus.subsystems.GBSubsystem;
-import edu.wpi.first.wpilibj.DigitalInput;
-
-// This is GBSubsystem for periodic and the dashboard. this isn't a real subsystem.
-public class DigitalInputMap extends GBSubsystem {
-
- private static final int NUMBER_OF_PORTS = 10;
-
- private static DigitalInputMap instance;
- private final DigitalInput[] digitalInputs;
-
- private DigitalInputMap() {
-
- digitalInputs = new DigitalInput[NUMBER_OF_PORTS];
- for (int i = 0; i < NUMBER_OF_PORTS; i++) {
- digitalInputs[i] = new DigitalInput(i);
- }
-
- }
-
- public static DigitalInputMap getInstance() {
- if (instance == null) {
- instance = new DigitalInputMap();
- instance.register();
- }
- return instance;
- }
-
- public DigitalInput getDigitalInput(int port) {
- return digitalInputs[port];
- }
-
- public boolean getValue(int port) {
- return digitalInputs[port].get();
- }
-
- @Override
- public void periodic() {
-
- }
-}
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/GBMath.java b/src/main/java/edu/greenblitz/pegasus/utils/GBMath.java
index 7e2fe32b..900a6308 100644
--- a/src/main/java/edu/greenblitz/pegasus/utils/GBMath.java
+++ b/src/main/java/edu/greenblitz/pegasus/utils/GBMath.java
@@ -15,7 +15,7 @@ public class GBMath {
* @return the correct modulo result
*
*/
- public static double modulo(double x, double y) {
+ public static double modulo(double x, double y) {
return ((x % y) + y) % y;
}
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/LogTime.java b/src/main/java/edu/greenblitz/pegasus/utils/LogTime.java
deleted file mode 100644
index c70b2e2a..00000000
--- a/src/main/java/edu/greenblitz/pegasus/utils/LogTime.java
+++ /dev/null
@@ -1,29 +0,0 @@
-package edu.greenblitz.pegasus.utils;
-
-import edu.greenblitz.pegasus.utils.commands.GBCommand;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-public class LogTime extends GBCommand {
-
- private static long timeStartedLast = 0;
- private final String name;
-
- public LogTime(String n) {
- name = n;
- }
-
- @Override
- public void initialize() {
- if (timeStartedLast == 0) {
- timeStartedLast = System.currentTimeMillis();
- return;
- }
- SmartDashboard.putNumber("Time took " + name, System.currentTimeMillis() - timeStartedLast);
- timeStartedLast = System.currentTimeMillis();
- }
-
- @Override
- public boolean isFinished() {
- return true;
- }
-}
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/PIDObject.java b/src/main/java/edu/greenblitz/pegasus/utils/PIDObject.java
index 09016b17..ea28f2c9 100644
--- a/src/main/java/edu/greenblitz/pegasus/utils/PIDObject.java
+++ b/src/main/java/edu/greenblitz/pegasus/utils/PIDObject.java
@@ -28,13 +28,16 @@ public PIDObject(double kp, double ki, double kd, double kf, int inv, double iZo
setInverted(inv);
}
- public PIDObject(PIDObject p) {
- this.kp = p.getKp();
- this.kd = p.getKd();
- this.ki = p.getKi();
- this.ff = p.getKf();
- this.iZone = p.getIZone();
- setInverted(p.getInverted());
+
+ public PIDObject(PIDObject other) {
+ this.kp = other.kp;
+ this.kd = other.kd;
+ this.ki = other.ki;
+ this.ff = other.ff;
+ this.iZone = other.iZone;
+ this.tolerance = other.tolerance;
+ this.inverted = other.inverted;
+ this.maxPower = other.maxPower;
}
public PIDObject(double kp, double ki, double kd) {
@@ -65,6 +68,8 @@ public PIDObject(double kp, int inv) {
this(kp, 0.0, inv);
}
+
+
// -----
@Override
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/PigeonGyro.java b/src/main/java/edu/greenblitz/pegasus/utils/PigeonGyro.java
index ef7fef23..fbd575ea 100644
--- a/src/main/java/edu/greenblitz/pegasus/utils/PigeonGyro.java
+++ b/src/main/java/edu/greenblitz/pegasus/utils/PigeonGyro.java
@@ -43,17 +43,17 @@ public void setRollOffset (double offset){
@Override
public double getYaw(){
- return GBMath.modulo(Math.toRadians(super.getYaw()) - yawOffset, 2 * Math.PI);
+ return GBMath.modulo((Math.toRadians(super.getYaw()) - yawOffset), 2 * Math.PI);
}
@Override
public double getPitch(){
- return GBMath.modulo(Math.toRadians(super.getYaw()) - pitchOffset, 2 * Math.PI);
+ return GBMath.modulo((Math.toRadians(super.getYaw()) - pitchOffset) , 2 * Math.PI);
}
@Override
public double getRoll(){
- return GBMath.modulo(Math.toRadians(super.getYaw()) - rollOffset, 2 * Math.PI);
+ return GBMath.modulo((Math.toRadians(super.getYaw()) - rollOffset) , 2 * Math.PI);
}
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/hid/SmartJoystick.java b/src/main/java/edu/greenblitz/pegasus/utils/hid/SmartJoystick.java
index 156e332d..acaba108 100644
--- a/src/main/java/edu/greenblitz/pegasus/utils/hid/SmartJoystick.java
+++ b/src/main/java/edu/greenblitz/pegasus/utils/hid/SmartJoystick.java
@@ -137,7 +137,6 @@ public double getAxisValue(Axis axis) {
*/
public void rumble(boolean left, double power) {
joystick.setRumble(left ? GenericHID.RumbleType.kLeftRumble : GenericHID.RumbleType.kRightRumble, power);
- SmartDashboard.putNumber((left ? "left" : "right") + " rumble", power);
}
public enum Axis {
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/motors/GBFalcon.java b/src/main/java/edu/greenblitz/pegasus/utils/motors/GBFalcon.java
new file mode 100644
index 00000000..717f9484
--- /dev/null
+++ b/src/main/java/edu/greenblitz/pegasus/utils/motors/GBFalcon.java
@@ -0,0 +1,138 @@
+package edu.greenblitz.pegasus.utils.motors;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.TalonFXPIDSetConfiguration;
+import edu.greenblitz.pegasus.utils.PIDObject;
+
+public class GBFalcon extends TalonFX {
+
+ /**
+ * Constructor
+ *
+ * @param deviceNumber [0,62]
+ */
+ public GBFalcon(int deviceNumber) {
+ super(deviceNumber);
+ }
+
+ /**
+ * configs the motor settings using FalconConfObject
+ *
+ * @param conf configObject, uses builder
+ */
+ public void config (GBFalcon.FalconConfObject conf){
+ configFactoryDefault();
+ super.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, conf.getCurrentLimit(), conf.getCurrentLimit(), 0));
+ super.configClosedloopRamp(conf.getRampRate());
+ super.configOpenloopRamp(conf.getRampRate());
+ super.setInverted(conf.isInverted());
+ super.configSelectedFeedbackCoefficient(conf.getConversionFactor());
+ super.configVoltageCompSaturation(conf.getVoltageCompSaturation());
+ super.setNeutralMode(conf.getNeutralMode());
+ configPID(conf.pidObject);
+ }
+
+ public void configPID(PIDObject pidObject){
+ super.config_kP(0, pidObject.getKp());
+ super.config_kI(0, pidObject.getKi());
+ super.config_kD(0, pidObject.getKd());
+ super.config_kF(0, pidObject.getKf());
+ super.config_IntegralZone(0, pidObject.getIZone());
+ super.configClosedLoopPeakOutput(0,pidObject.getMaxPower());
+ }
+
+ /**
+ * @see GBSparkMax.SparkMaxConfObject
+ */
+ public static class FalconConfObject {
+ private PIDObject pidObject = new PIDObject(0, 0, 0);
+ private int currentLimit = 0;
+ private double rampRate = 0;
+ private boolean inverted = false;
+ private double ConversionFactor = 1;
+ private double voltageCompSaturation = 0;
+ private NeutralMode neutralMode = NeutralMode.Brake;
+
+ public FalconConfObject(FalconConfObject other){
+ this.pidObject = new PIDObject(other.pidObject);
+ this.currentLimit = other.currentLimit;
+ this.rampRate = other.rampRate;
+ this.inverted = other.inverted;
+ this.neutralMode = other.neutralMode;
+ this.voltageCompSaturation = other.voltageCompSaturation;
+ }
+
+ public FalconConfObject() {
+
+ }
+
+ public int getCurrentLimit() {
+ return currentLimit;
+ }
+
+ public double getRampRate() {
+ return rampRate;
+ }
+
+ public boolean isInverted() {
+ return inverted;
+ }
+
+ public double getConversionFactor() {
+ return ConversionFactor;
+ }
+
+ public double getVoltageCompSaturation() {
+ return voltageCompSaturation;
+ }
+
+ public NeutralMode getNeutralMode() {
+ return neutralMode;
+ }
+
+ public FalconConfObject withNeutralMode(NeutralMode neutralMode) {
+ this.neutralMode = neutralMode;
+ return this;
+ }
+
+ public FalconConfObject withConversionFactor (double velocityConversionFactor){
+ this.ConversionFactor = velocityConversionFactor;
+ return this;
+ }
+
+ public FalconConfObject withPID (PIDObject pidObject){
+ this.pidObject = pidObject;
+ return this;
+ }
+
+ public FalconConfObject withCurrentLimit(int currentLimit){
+ this.currentLimit = currentLimit;
+ return this;
+ }
+
+ public FalconConfObject withRampRate(double rampRate){
+ this.rampRate = rampRate;
+ return this;
+ }
+
+ public FalconConfObject withInverted(Boolean inverted){
+ this.inverted = inverted;
+ return this;
+ }
+
+
+ public FalconConfObject withVoltageCompSaturation(double voltageCompSaturation) {
+ this.voltageCompSaturation = voltageCompSaturation;
+ return this;
+ }
+
+ public PIDObject getPidObject() {
+ return pidObject;
+ }
+
+
+ }
+}
diff --git a/src/main/java/edu/greenblitz/pegasus/utils/motors/GBSparkMax.java b/src/main/java/edu/greenblitz/pegasus/utils/motors/GBSparkMax.java
new file mode 100644
index 00000000..4ab8a6a3
--- /dev/null
+++ b/src/main/java/edu/greenblitz/pegasus/utils/motors/GBSparkMax.java
@@ -0,0 +1,175 @@
+package edu.greenblitz.pegasus.utils.motors;
+
+import com.revrobotics.*;
+import edu.greenblitz.pegasus.RobotMap;
+import edu.greenblitz.pegasus.utils.PIDObject;
+
+public class GBSparkMax extends CANSparkMax {
+
+
+ /**
+ * Create a new object to control a SPARK MAX motor Controller
+ *
+ * @param deviceId The device ID.
+ * @param type The motor type connected to the controller. Brushless motor wires must be connected
+ * to their matching colors and the hall sensor must be plugged in. Brushed motors must be
+ * connected to the Red and Black terminals only.
+ */
+ public GBSparkMax(int deviceId, MotorType type) {
+ super(deviceId, type);
+ }
+
+
+ /**
+ * configs the motor settings using SparkMaxConfObject
+ *
+ * @param conf configObject, uses builder
+ */
+ public void config (SparkMaxConfObject conf){
+ super.restoreFactoryDefaults();
+ super.setSmartCurrentLimit(conf.getCurrentLimit());
+ configPID(conf.getPidObject());
+ super.getEncoder().setPositionConversionFactor(conf.getPositionConversionFactor());
+ super.getEncoder().setVelocityConversionFactor(conf.getVelocityConversionFactor());
+ super.setClosedLoopRampRate(conf.getRampRate());
+ super.setOpenLoopRampRate(conf.getRampRate());
+ super.setInverted(conf.isInverted());
+ super.setIdleMode(conf.getIdleMode());
+ enableVoltageCompensation(conf.getVoltageComp());
+
+ }
+
+
+ public void configPID (PIDObject pidObject){
+ super.getPIDController().setP(pidObject.getKp());
+ super.getPIDController().setI(pidObject.getKi());
+ super.getPIDController().setD(pidObject.getKd());
+ super.getPIDController().setFF(pidObject.getKf());
+ super.getPIDController().setIZone(pidObject.getIZone());
+ super.getPIDController().setOutputRange(-pidObject.getMaxPower(), pidObject.getMaxPower());
+ }
+
+
+
+ /**
+ * inner conf class
+ * usage example:
+ * this.motor = new GBSparkMax(id, CANSparkMaxLowLevel.MotorType.kBrushless);
+ * motor.config(new GBSparkMax.SparkMaxConfObject()
+ * .withInverted(true) //whether the motor should be flipped
+ * .withCurrentLimit(40) // the max current to allow should be inline with the fuse
+ * .withIdleMode(CANSparkMax.IdleMode.kCoast) // trying to force brake is harmful for the motor
+ * .withRampRate(General.RAMP_RATE_VAL) // prevents the motor from drawing to much when rapidly changing speeds
+ * .withVoltageComp(General.VOLTAGE_COMP_VAL) // makes for more reproducible results
+ * .withPositionConversionFactor(1)
+ * .withVelocityConversionFactor(1)
+ * .withPID(new PIDObject(0.0003, 0.0000003, 0).withIZone(300)
+ * );
+ * */
+
+ public static class SparkMaxConfObject{
+
+ private PIDObject pidObject = new PIDObject(0,0,0);
+
+ private int currentLimit = 0;
+ private double rampRate = 0;
+ private boolean inverted = false;
+ private IdleMode idleMode = IdleMode.kBrake;
+
+ private double positionConversionFactor = 1;
+
+ private double velocityConversionFactor = 1;
+
+ private double voltageComp = 0;
+
+ //no parameters, usage for external config functions
+ public SparkMaxConfObject (){
+
+ }
+
+ public SparkMaxConfObject(SparkMaxConfObject other) {
+ this.pidObject = new PIDObject(other.pidObject);
+ this.currentLimit = other.currentLimit;
+ this.rampRate = other.rampRate;
+ this.inverted = other.inverted;
+ this.idleMode = other.idleMode;
+ this.positionConversionFactor = other.positionConversionFactor;
+ this.velocityConversionFactor = other.velocityConversionFactor;
+ this.voltageComp = other.voltageComp;
+ }
+
+ public boolean isInverted() {
+ return inverted;
+ }
+
+ public int getCurrentLimit() {
+ return currentLimit;
+ }
+
+ public double getRampRate() {
+ return rampRate;
+ }
+
+ public IdleMode getIdleMode() {
+ return idleMode;
+ }
+
+ public double getPositionConversionFactor() {
+ return positionConversionFactor;
+ }
+
+ public double getVelocityConversionFactor() {
+ return velocityConversionFactor;
+ }
+
+ public double getVoltageComp() {
+ return voltageComp;
+ }
+
+ public SparkMaxConfObject withVelocityConversionFactor (double velocityConversionFactor){
+ this.velocityConversionFactor = velocityConversionFactor;
+ return this;
+ }
+ public SparkMaxConfObject withPositionConversionFactor (double positionConversionFactor){
+ this.positionConversionFactor = positionConversionFactor;
+ return this;
+ }
+
+ public SparkMaxConfObject withPID (PIDObject pidObject){
+ this.pidObject = pidObject;
+ return this;
+ }
+
+ public SparkMaxConfObject withCurrentLimit(int currentLimit){
+ this.currentLimit = currentLimit;
+ return this;
+ }
+
+ public SparkMaxConfObject withRampRate(double rampRate){
+ this.rampRate = rampRate;
+ return this;
+ }
+
+ public SparkMaxConfObject withInverted(Boolean inverted){
+ this.inverted = inverted;
+ return this;
+ }
+
+ public SparkMaxConfObject withIdleMode(IdleMode idleMode){
+ this.idleMode = idleMode;
+ return this;
+ }
+
+ public SparkMaxConfObject withVoltageComp(double voltageComp) {
+ this.voltageComp = voltageComp;
+ return this;
+ }
+
+ public PIDObject getPidObject() {
+ return pidObject;
+ }
+
+
+ }
+
+}
diff --git a/vendordeps/PhotonLib-json-1.0.json b/vendordeps/PhotonLib-json-1.0.json
new file mode 100644
index 00000000..59bbc0c5
--- /dev/null
+++ b/vendordeps/PhotonLib-json-1.0.json
@@ -0,0 +1,41 @@
+{
+ "fileName": "photonlib.json",
+ "name": "photonlib",
+ "version": "v2023.1.1-beta-6",
+ "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
+ "mavenUrls": [
+ "https://maven.photonvision.org/repository/internal",
+ "https://maven.photonvision.org/repository/snapshots"
+ ],
+ "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonLib-cpp",
+ "version": "v2023.1.1-beta-6",
+ "libName": "Photon",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxx86-64"
+ ]
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonLib-java",
+ "version": "v2023.1.1-beta-6"
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonTargeting-java",
+ "version": "v2023.1.1-beta-6"
+ }
+ ]
+}