feat: wrist initial code (#27)

* added wrist, wristio, and wristiotalonfx!! based on coco's arm code from yesterday. also added motor and encoder ports to the list * created wrist object in the robot class! * added io methods in wrist class so they can actually be called * fixed getWristDegrees method to actually get degrees instead of rotations * split position status signal into absolutePosition and relativePosition * made other changes amber suggested (thanks amber!) * deleted setWristVoltage bc it didn't do anything and wasn't used --------- Co-authored-by: amb2127 <[email protected]>
FRC3476 · Jan 24, 2024 · b1e4309 · b1e4309
1 parent 5f5dc94
commit b1e4309
Show file tree

Hide file tree

Showing 5 changed files with 169 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -55,6 +55,8 @@ public static class Ports {
         public static final int BR_CANCODER = 20;
 
         public static final int PIGEON = 30;
+        public static final int WRIST_MOTOR = 1;
+        public static final int WRIST_ENCODER = 2;
 
     }
 

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -8,6 +8,9 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import frc.subsystem.AbstractSubsystem;
 import frc.subsystem.drive.*;
+import frc.subsystem.wrist.Wrist;
+import frc.subsystem.wrist.WristIO;
+import frc.subsystem.wrist.WristIOTalonFX;
 import frc.utility.Controller;
 import frc.utility.Controller.XboxButtons;
 import frc.utility.ControllerDriveInputs;
@@ -46,6 +49,7 @@ public class Robot extends LoggedRobot {
 
 
     static Drive drive;
+    static Wrist wrist;
 
     /**
      * This function is run when the robot is first started up and should be used
@@ -117,6 +121,7 @@ public void robotInit() {
             powerDistribution = new PowerDistribution(1, PowerDistribution.ModuleType.kRev); // Enables power distribution logging
 
             drive = new Drive(new ModuleIOTalonFX(0), new ModuleIOTalonFX(1), new ModuleIOTalonFX(2), new ModuleIOTalonFX(3), new GyroIOPigeon2());
+            wrist = new Wrist(new WristIOTalonFX());
         } else {
             setUseTiming(false); // Run as fast as possible
             if(Objects.equals(VIRTUAL_MODE, "REPLAY")) {
@@ -128,6 +133,7 @@ public void robotInit() {
             }
 
             drive = new Drive(new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new GyroIO() {});
+            wrist = new Wrist(new WristIO() {});
         }
         // Initialize auto chooser
         chooser.addDefaultOption("Default Auto", defaultAuto);
@@ -139,6 +145,7 @@ public void robotInit() {
 
         Logger.start();
         drive.start();
+        wrist.start();
     }
 
     /** This function is called periodically during all modes. */

diff --git a/src/main/java/frc/subsystem/wrist/Wrist.java b/src/main/java/frc/subsystem/wrist/Wrist.java
@@ -0,0 +1,39 @@
+package frc.subsystem.wrist;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.subsystem.AbstractSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+public class Wrist extends AbstractSubsystem {
+
+    private final WristIO io;
+    private final WristInputsAutoLogged inputs = new WristInputsAutoLogged();
+
+    public Wrist(WristIO wristio) {
+        super();
+        this.io = wristio;
+    }
+
+
+    public synchronized void setWristPosition(double positionInDegrees) {
+        //does NOT set position relative to the ground, just relative to the arm
+        double currentTime = Timer.getFPGATimestamp();
+        io.setPosition(positionInDegrees);
+        Logger.recordOutput("Wrist target position", positionInDegrees);
+    }
+
+
+    @Override
+    public synchronized void update() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Wrist", inputs);
+    }
+
+    public double getWristAbsoluteDegrees() {
+        return (inputs.wristAbsolutePosition * 360.0) % 360;
+    }
+
+    public void zeroWristEncoder() {
+        io.zeroWristEncoder();
+    }
+}
diff --git a/src/main/java/frc/subsystem/wrist/WristIO.java b/src/main/java/frc/subsystem/wrist/WristIO.java
@@ -0,0 +1,23 @@
+package frc.subsystem.wrist;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface WristIO {
+    @AutoLog
+    class WristInputs {
+        double wristAbsolutePosition = 0.0;
+        double wristRelativePosition = 0.0;
+        double wristVelocity = 0.0;
+        double wristCurrent = 0.0;
+        double wristTemp = 0.0;
+        double wristVoltage = 0.0;
+
+    }
+
+    default void updateInputs(WristInputs inputs) {}
+
+    default void zeroWristEncoder() {}
+
+    default void setPosition(double position) {}
+
+}
diff --git a/src/main/java/frc/subsystem/wrist/WristIOTalonFX.java b/src/main/java/frc/subsystem/wrist/WristIOTalonFX.java
@@ -0,0 +1,98 @@
+package frc.subsystem.wrist;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+
+import static frc.robot.Constants.Ports.*;
+
+public class WristIOTalonFX implements WristIO {
+
+    private final TalonFX wristMotor;
+    private final CANcoder absoluteEncoder;
+
+
+    private final StatusSignal<Double> wristAbsolutePosition;
+    private final StatusSignal<Double> wristRelativePosition;
+    private final StatusSignal<Double> wristVelocity;
+    private final StatusSignal<Double> wristCurrent;
+    private final StatusSignal<Double> wristTemp;
+    private final StatusSignal<Double> wristVoltage;
+    private final MotionMagicVoltage motionMagicControl = new MotionMagicVoltage(0);
+
+
+
+    public WristIOTalonFX() {
+
+        wristMotor = new TalonFX(WRIST_MOTOR);
+        absoluteEncoder = new CANcoder(WRIST_ENCODER);
+
+        TalonFXConfiguration configs = new TalonFXConfiguration();
+
+        FeedbackConfigs wristFeedBackConfigs = configs.Feedback;
+        wristFeedBackConfigs.FeedbackRemoteSensorID = absoluteEncoder.getDeviceID();
+        wristFeedBackConfigs.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+        //ask amber about below
+        wristFeedBackConfigs.SensorToMechanismRatio = 1;
+        wristFeedBackConfigs.RotorToSensorRatio = 1.0 / 81;
+
+        MotionMagicConfigs wristMotionMagicConfig = configs.MotionMagic;
+        wristMotionMagicConfig.MotionMagicAcceleration = 1;     //TODO change motion magic values
+        wristMotionMagicConfig.MotionMagicCruiseVelocity = 10;
+        wristMotionMagicConfig.MotionMagicJerk = 50;
+
+
+        Slot0Configs slot0 = configs.Slot0;
+        slot0.kP = 0;
+        slot0.kI = 0;
+        slot0.kD = 0;
+        slot0.kV = 0;
+        slot0.kS = 0; // Approximately 0.25V to get the mechanism moving
+        slot0.kG = 0;
+        slot0.GravityType = GravityTypeValue.Arm_Cosine;
+
+        wristMotor.getConfigurator().apply(configs);
+
+        wristAbsolutePosition = absoluteEncoder.getAbsolutePosition();
+        wristRelativePosition = wristMotor.getPosition();
+        wristVelocity = wristMotor.getVelocity();
+        wristCurrent = wristMotor.getSupplyCurrent();
+        wristTemp = wristMotor.getDeviceTemp();
+        wristVoltage = wristMotor.getMotorVoltage();
+
+        BaseStatusSignal.setUpdateFrequencyForAll(50, wristAbsolutePosition, wristRelativePosition, wristVelocity, wristVoltage, wristCurrent, wristTemp);
+
+        wristMotor.optimizeBusUtilization();
+        absoluteEncoder.optimizeBusUtilization();
+    }
+
+    public void setPosition(double position){
+        wristMotor.setControl(motionMagicControl.withPosition(position).withSlot(0).withEnableFOC(true).withOverrideBrakeDurNeutral(true));
+    }
+
+    public void updateInputs(WristInputs inputs) {
+        BaseStatusSignal.refreshAll(wristAbsolutePosition, wristRelativePosition, wristVelocity, wristCurrent,
+                wristTemp, wristVoltage);
+        //Not sure if this is the fastest way to get encoder pos
+        //needs optimization!!!!!!
+        inputs.wristAbsolutePosition = absoluteEncoder.getPosition().getValue();
+        inputs.wristRelativePosition = wristRelativePosition.getValue();
+        inputs.wristVelocity = wristVelocity.getValue();
+        inputs.wristCurrent = wristCurrent.getValue();
+        inputs.wristTemp = wristTemp.getValue();
+        inputs.wristVoltage = wristVoltage.getValue();
+    }
+
+
+    public void zeroWristEncoder() {
+        absoluteEncoder.setPosition(0);
+    }
+}