wrist code progress

team422 · Sep 16, 2023 · 1d7fa21 · 1d7fa21
1 parent ab9b957
commit 1d7fa21
Show file tree

Hide file tree

Showing 5 changed files with 52 additions and 27 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -57,6 +57,12 @@ public static final class Pigeon2AccelConstants {
         public static final double kMaxSpeedMetersPerSecond = 3.85;
     }
 
+    public static final class WristConstants{
+        public static final double kWristP = 5.5;
+        public static final double kWristI = 0.08;
+        public static final double kWristD = 0.3;
+    }
+
     public static final class ModuleConstants {
         public static final TunableNumber kDriveP = new TunableNumber("Drive P", 0.1);
         public static final TunableNumber kDriveI = new TunableNumber("Drive I", 0.0);

diff --git a/src/main/java/frc/robot/subsystems/wrist/Wrist.java b/src/main/java/frc/robot/subsystems/wrist/Wrist.java
@@ -1,26 +1,29 @@
 package frc.robot.subsystems.wrist;
 
+import org.littletonrobotics.junction.Logger;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
-import frc.robot.subsystems.wrist.WristIO.WristInputs;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class Wrist {
+public class Wrist extends SubsystemBase{
     private double kMinAngle;
     private double kMaxAngle;
 
     private WristIO m_IO;
     public WristInputsAutoLogged m_inputs;
 
-    private PIDController m_controller;
+    private ProfiledPIDController m_controller;
     private Rotation2d desRotation2d;
 
     public Wrist(WristIO io, ProfiledPIDController wristController, Rotation2d minAngle, Rotation2d maxAngle) {
         m_IO = io;
-        m_controller = new PIDController(0, 0, 0);
+        m_controller = wristController;
+        m_controller.setTolerance(Units.degreesToRadians(2));
         m_inputs = new WristInputsAutoLogged();
         kMinAngle = minAngle.getRadians();
         kMaxAngle = maxAngle.getRadians();
@@ -29,11 +32,17 @@ public Wrist(WristIO io, ProfiledPIDController wristController, Rotation2d minAn
 
     public void periodic() {
         m_IO.updateInputs(m_inputs);
+        Logger.getInstance().processInputs("Wrist", m_inputs);
+        double curAngle = m_inputs.angleRad;
+
+        double pidVoltage = m_controller.calculate(curAngle, desRotation2d.getRadians());
+
+        m_IO.setMotorVoltages(pidVoltage);
     }
 
     public void reset() {
         setAngle(Rotation2d.fromRadians(m_inputs.angleRad));
-        m_controller.reset();
+        m_controller.reset(m_inputs.angleRad);
     }
 
     public void setAngle(Rotation2d angle) {

diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIO.java b/src/main/java/frc/robot/subsystems/wrist/WristIO.java
@@ -16,8 +16,6 @@ public static class WristInputs{
 
     public double[] getSpeeds();
 
-    public void setMotorSpeeds(double speed);
-
     public void setMotorVoltages(double Voltage);
 
     public void setMotorBrake(boolean brake);

diff --git a/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java b/src/main/java/frc/robot/subsystems/wrist/WristIOSim.java
@@ -6,7 +6,7 @@
 
 public class WristIOSim implements WristIO{
 
-    DCMotor[] motors = new DCMotor[20];
+    DCMotor[] motors = new DCMotor[4];
     double m_voltage;
     boolean m_brakeEnabled;
 
@@ -30,11 +30,6 @@ public double[] getSpeeds() {
         return speeds;
     }
 
-    @Override
-    public void setMotorSpeeds(double speed) {
-
-    }
-
     @Override
     public void setMotorVoltages(double voltage) {
         m_voltage = voltage;       
@@ -43,5 +38,6 @@ public void setMotorVoltages(double voltage) {
     @Override
     public void setMotorBrake(boolean brake) {
         m_brakeEnabled = brake;
-    } 
+    }
+
 }
diff --git a/src/test/java/WristTesting/WristTest.java b/src/test/java/WristTesting/WristTest.java
@@ -9,19 +9,29 @@
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.subsystems.wrist.WristIOSim;
+import frc.robot.subsystems.wrist.Wrist;
+import frc.robot.Constants;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 
 public class WristTest {
-    static final double delta = 1e-2;
+    static final double delta = 2;
     Wrist m_wrist;
-    
+
     @BeforeEach
     void setup() {
-
+        new Rotation2d();
+        m_wrist = new Wrist(new WristIOSim(),
+                new ProfiledPIDController(Constants.WristConstants.kWristP, Constants.WristConstants.kWristI,
+                        Constants.WristConstants.kWristD, new Constraints(30, 25)),
+                Rotation2d.fromDegrees(-90), Rotation2d.fromDegrees(90));
     }
 
-    void sleepSeconds(int seconds){
+    void sleepSeconds(int seconds) {
         int i = 0;
-        while(i < seconds * 50){
+        while (i < seconds * 50) {
             sleep(1);
             i++;
         }
@@ -36,16 +46,22 @@ void sleep(int ticks) {
         }
     }
 
-    @Test 
-    void testSpeed() {
-
+    @Test
+    void testAngle() {
+        for (int i = -60; i < 60; i++) {
+            m_IO.setAngle(i);
+            sleepSeconds(3);
+            assertEquals(m_IO.getAngle(), i, delta);
+            m_IO.setAngle(0);
+            sleepSeconds(5);
+        }
     }
 
     // @Test
     // void testBrake(){
-    //     m_intake.brake(true);
-    //     sleep(1);
-    //     assertEquals(true, m_intake.isBrake());
+    // m_intake.brake(true);
+    // sleep(1);
+    // assertEquals(true, m_intake.isBrake());
     // }
 
 }