Ankle Subsystem

src/main/deploy/pathplanner/paths/Example Path.path

@@ -8,36 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 8.564640410958912,
-        "y": 2.6351241438356174
+        "x": 7.789895007209807,
+        "y": -1.860191735760635
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 3.9517551369863013,
-        "y": 5.42990154109589
+        "x": 4.612885273979555,
+        "y": 6.076005993145134
       },
       "prevControl": {
-        "x": 7.7935855263157885,
-        "y": 6.622861842105262
-      },
-      "nextControl": {
-        "x": 2.2635141527995155,
-        "y": 4.905671091395238
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.753253424657535,
-        "y": 2.66517551369863
-      },
-      "prevControl": {
-        "x": 7.716611842105262,
-        "y": 7.190542763157895
+        "x": 7.152226027404213,
+        "y": 5.685338184925955
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/swerve/modules/backleft.json

@@ -3,10 +3,10 @@
     "front": -11.5,
     "left": 11.5
   },
-  "absoluteEncoderOffset": 0,
+  "absoluteEncoderOffset": 164.5,
   "drive": {
     "type": "sparkflex_vortex",
-    "id": 63,
+    "id": 53,
     "canbus": "rio"
   },
   "inverted": {
@@ -15,7 +15,7 @@
   },
   "angle": {
     "type": "sparkmax_neo",
-    "id": 73,
+    "id": 43,
     "canbus": "rio"
   },
   "encoder": {

src/main/deploy/swerve/modules/backright.json

@@ -3,10 +3,10 @@
     "front": -11.5,
     "left": -11.5
   },
-  "absoluteEncoderOffset": 0,
+  "absoluteEncoderOffset": 118,
   "drive": {
     "type": "sparkflex_vortex",
-    "id": 64,
+    "id": 54,
     "canbus": "rio"
   },
   "inverted": {
@@ -15,7 +15,7 @@
   },
   "angle": {
     "type": "sparkmax_neo",
-    "id": 74,
+    "id": 44,
     "canbus": "rio"
   },
   "encoder": {

src/main/deploy/swerve/modules/frontleft.json

@@ -3,10 +3,10 @@
     "front": 11.5,
     "left": 11.5
   },
-  "absoluteEncoderOffset": 0,
+  "absoluteEncoderOffset": 211.2,
   "drive": {
     "type": "sparkflex_vortex",
-    "id": 61,
+    "id": 51,
     "canbus": "rio"
   },
   "inverted": {
@@ -15,12 +15,12 @@
   },
   "angle": {
     "type": "sparkmax_neo",
-    "id": 71,
+    "id": 41,
     "canbus": "rio"
   },
   "encoder": {
     "type": "thrifty",
-    "id": 1,
+    "id": 2,
     "canbus": null
   },
   "absoluteEncoderInverted": false

src/main/deploy/swerve/modules/frontright.json

@@ -3,10 +3,10 @@
     "front": 11.5,
     "left": -11.5
   },
-  "absoluteEncoderOffset": 0,
+  "absoluteEncoderOffset": 42.6,
   "drive": {
     "type": "sparkflex_vortex",
-    "id": 62,
+    "id": 52,
     "canbus": "rio"
   },
   "inverted": {
@@ -15,12 +15,12 @@
   },
   "angle": {
     "type": "sparkmax_neo",
-    "id": 72,
+    "id": 42,
     "canbus": "rio"
   },
   "encoder": {
     "type": "thrifty",
-    "id": 2,
+    "id": 1,
     "canbus": null
   },
   "absoluteEncoderInverted": false

src/main/deploy/swerve/modules/pidfproperties.json

@@ -1,16 +1,16 @@
 {
   "drive": {
-    "p": 0.0020645,
+    "p": 0.04,
     "i": 0,
-    "d": 0,
-    "f": 0,
+    "d": 0.000125,
+    "f": 0.01,
     "iz": 0
   },
   "angle": {
-    "p": 0.0020645,
+    "p": 0,  
     "i": 0,
     "d": 0,
     "f": 0,
     "iz": 0
   }
-}
+}

src/main/deploy/swerve/swervedrive.json

@@ -1,7 +1,7 @@
 {
   "imu": {
     "type": "pigeon2",
-    "id": 51,
+    "id": 31,
     "canbus": "rio"
   },
   "invertedIMU": false,

src/main/java/frc/robot/Constants.java

@@ -11,7 +11,7 @@ public static class OperatorConstants {
   }
 
   public static class GeneralConstants {
-    public static final int LED_LENGTH = 30;
-    public static final int LED_PWM_PORT = 1;
+    public static final int LED_LENGTH = 73;
+    public static final int LED_PWM_PORT = 8;
   }
 }

src/main/java/frc/robot/PIDDebugger.java

@@ -0,0 +1,22 @@
+package frc.robot;
+
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class PIDDebugger {
+
+    public PIDDebugger() {
+
+    }
+
+    public void setSparkPIDControllerToDashboard(String prefix, double kP, double kI, double kD, double kMinOutput,
+            double kIz, double kMaxOutput) {
+        SmartDashboard.putNumber(prefix + "P", kP);
+        SmartDashboard.putNumber(prefix + "I", kI);
+        SmartDashboard.putNumber(prefix + "D", kD);
+        SmartDashboard.putNumber(prefix + "MinOutput", kMinOutput);
+        SmartDashboard.putNumber(prefix + "MaxOutput", kMaxOutput);
+        SmartDashboard.putNumber(prefix + "Iz", kIz);
+    }
+
+}

src/main/java/frc/robot/RobotContainer.java

@@ -38,8 +38,8 @@ public class RobotContainer {
 
 
     SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
-            () -> driverController.getLeftY() * -1,
-            () -> driverController.getLeftX() * -1)
+            () -> driverController.getLeftY(),
+            () -> driverController.getLeftX())
             .withControllerRotationAxis(driverController::getRightX)
             .deadband(OperatorConstants.DEADBAND)
             .scaleTranslation(0.8)
@@ -73,6 +73,11 @@ private void configureBindings() {
         driverController.button(5).whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
         driverController.button(10).onTrue((Commands.runOnce(drivebase::zeroGyro)));
         driverController.button(4).whileTrue(drivebase.centerModulesCommand());
+        driverController.button(2).whileTrue(
+          drivebase.driveToPose(
+              new Pose2d(new Translation2d(7, 4), Rotation2d.fromDegrees(0)))
+                              );
+        driverController.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
         //driverController.button(3).whileTrue(drivebase.pa); Go barge command
     }
 

src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.subsystems.AnkleSubsystem;
+
+public class AnkleConstants {
+    public final static int RedlineID = 1;
+    public final static int SparkID = 1;
+    public final static double maxAngle = 0/360;
+    public final static double minAngle = 0/360;
+    public static final double MaxOutput = 0;
+    public static final double MinOutput = 0;
+    public static double kP = 0.04;
+    public static double kI = 0;
+    public static double kD = 0;
+    public static double gearRatio = 4;
+
+}

src/main/java/frc/robot/subsystems/AnkleSubsystem/AnkleSubsystem.java

@@ -0,0 +1,111 @@
+package frc.robot.subsystems.AnkleSubsystem;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class AnkleSubsystem extends SubsystemBase {
+  private SparkMax m_motor;
+  private WPI_VictorSPX m_wheelMotor;
+  private SparkClosedLoopController closedLoopController;
+  private SparkMaxConfig motorConfig;
+  private RelativeEncoder encoder;
+  private double currentAngleSetpoint;
+  private double kP, kI, kD, kIz, kMaxOutput, kMinOutput;
+
+  public AnkleSubsystem() {
+    m_motor = new SparkMax(AnkleConstants.SparkID, MotorType.kBrushless);
+    m_wheelMotor = new WPI_VictorSPX(AnkleConstants.RedlineID);
+    encoder = m_motor.getEncoder();
+    closedLoopController = m_motor.getClosedLoopController();
+    motorConfig = new SparkMaxConfig();
+    m_motor.configure(configCreator(motorConfig), ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters);
+    encoder.setPosition(0);
+  }
+
+  private void setAngle(double angle) {
+    if (isAngleInRange(angle)) {
+      currentAngleSetpoint = angle;
+      closedLoopController.setReference(angle * AnkleConstants.gearRatio, ControlType.kMAXMotionPositionControl,
+          ClosedLoopSlot.kSlot0);
+    } else {
+      setDefault();
+    }
+  }
+
+  private void setAngleTest() {
+    double angle = SmartDashboard.getNumber("testAngleAnkle", 0);
+    if (isAngleInRange(angle)) {
+      closedLoopController.setReference(angle  * AnkleConstants.gearRatio, ControlType.kMAXMotionPositionControl,
+          ClosedLoopSlot.kSlot0);
+    } else {
+      setDefault();
+    }
+  }
+
+  private void setDefault() {
+    closedLoopController.setReference(0, ControlType.kMAXMotionPositionControl,
+        ClosedLoopSlot.kSlot0);
+        currentAngleSetpoint = 0;
+    if (encoder.getPosition() == 0) {
+      stopAngleMotor();
+    }
+  }
+
+  private boolean isAngleInRange(double angle) {
+    return angle <= AnkleConstants.maxAngle && angle >= AnkleConstants.minAngle;
+  }
+
+  private void stopAngleMotor() {
+    m_motor.set(0);
+  }
+
+  private void setWheelMotor(double velocity) {
+    m_wheelMotor.set(velocity);
+  }
+
+  @Override
+  public void periodic() {
+    SmartDashboard.putNumber("Current Angle (Ankle)", currentAngleSetpoint);
+    // This method will be called once per scheduler run
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+
+  private SparkMaxConfig configCreator(SparkMaxConfig motorConfig) {
+    String prefix = "ankle";
+    double kP = SmartDashboard.getNumber(prefix + "P", AnkleConstants.kP);
+    double kI = SmartDashboard.getNumber(prefix + "I", AnkleConstants.kI);
+    double kD = SmartDashboard.getNumber(prefix + "D", AnkleConstants.kD);
+    double kMinOutput = SmartDashboard.getNumber(prefix + "MinOutput", AnkleConstants.MinOutput);
+    double kMaxOutput = SmartDashboard.getNumber(prefix + "MaxOutput", AnkleConstants.MaxOutput);
+    motorConfig.closedLoop
+        .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+        .p(kP)
+        .i(kI)
+        .d(kD)
+        .outputRange(kMinOutput, kMaxOutput);
+
+    motorConfig.closedLoop.maxMotion
+        .maxVelocity(1000)
+        .maxAcceleration(1000)
+        .allowedClosedLoopError(1);
+        motorConfig.smartCurrentLimit(50);
+
+        return motorConfig;
+  }
+
+}