From b1e4309e5fa9a1533c5aa4aca18f9388c19bef83 Mon Sep 17 00:00:00 2001 From: nmp2965 <115129873+nmp2965@users.noreply.github.com> Date: Tue, 23 Jan 2024 20:15:03 -0800 Subject: [PATCH] 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 <39502367+amb2127@users.noreply.github.com> --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/Robot.java | 7 ++ src/main/java/frc/subsystem/wrist/Wrist.java | 39 ++++++++ .../java/frc/subsystem/wrist/WristIO.java | 23 +++++ .../frc/subsystem/wrist/WristIOTalonFX.java | 98 +++++++++++++++++++ 5 files changed, 169 insertions(+) create mode 100644 src/main/java/frc/subsystem/wrist/Wrist.java create mode 100644 src/main/java/frc/subsystem/wrist/WristIO.java create mode 100644 src/main/java/frc/subsystem/wrist/WristIOTalonFX.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b12816f..9d2aecf 100644 --- 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 index 99cd529..147158d 100644 --- 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 new file mode 100644 index 0000000..7e0eaa9 --- /dev/null +++ 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(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/subsystem/wrist/WristIO.java b/src/main/java/frc/subsystem/wrist/WristIO.java new file mode 100644 index 0000000..e6a98af --- /dev/null +++ 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 new file mode 100644 index 0000000..25511b9 --- /dev/null +++ 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 wristAbsolutePosition; + private final StatusSignal wristRelativePosition; + private final StatusSignal wristVelocity; + private final StatusSignal wristCurrent; + private final StatusSignal wristTemp; + private final StatusSignal 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); + } +} \ No newline at end of file