Merge pull request #21 from Eva-Unit-02/manip+wrist-inverted

Wrist changes
Team3128 · Sep 16, 2023 · 2c7058f · 2c7058f
2 parents 3ee9d5f + bf2c7a0
commit 2c7058f
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 27 deletions.
diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java
@@ -514,7 +514,7 @@ public static class WristConstants {
         public static final double kV = 0;
         public static final double kG = 0;
 
-        public static final double GEAR_RATIO = 1.0;
+        public static final double GEAR_RATIO = 108.0;
 
         public static final double ROTATION_TO_DEGREES = 360;
 
@@ -530,14 +530,14 @@ public static class WristConstants {
         public static final double INTAKE_TOLERANCE = 7.5;
 
         //Motor ID
-        public static final int WRIST_ID = 10;
+        public static final int WRIST_ID = 25;
 
         //Sensor IDs
 
     }
 
     public static class ManipulatorConstants{
-        public static final int ROLLER_MOTOR_ID = 4;
+        public static final int ROLLER_MOTOR_ID = 0;
         public static final double ROLLER_POWER = 0.9;
         public static final double STALL_POWER_CONE = 0.2;
         public static final double STALL_POWER_CUBE = 0.0;

diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java
@@ -19,26 +19,30 @@
 
 public class CmdManager {
     private static Led led = Led.getInstance();
-    // private static Wrist wrist = Wrist.getInstance();
+    private static Wrist wrist = Wrist.getInstance();
     private static Manipulator manipulator = Manipulator.getInstance();
     private static NAR_XboxController controller = RobotContainer.controller;
 
 
     private CmdManager() {}
 
-    // public static CommandBase CmdWrist(double setpoint) {
-    //     return sequence(
-    //         runOnce(()-> wrist.startPID(setpoint), wrist),
-    //         waitUntil(()-> wrist.atSetpoint())
-    //     );
-    // }
+    public static CommandBase CmdWrist(double setpoint) {
+        return sequence(
+            runOnce(()-> wrist.startPID(setpoint), wrist),
+            waitUntil(()-> wrist.atSetpoint())
+        );
+    }
+
+    public static CommandBase CmdWrist(WristPosition position) {
+        return sequence(
+            runOnce(()-> wrist.startPID(position.wristAngle), wrist),
+            waitUntil(()-> wrist.atSetpoint())
+        );
+    }
 
-    // public static CommandBase CmdWrist(WristPosition position) {
-    //     return sequence(
-    //         runOnce(()-> wrist.startPID(position.wristAngle), wrist),
-    //         waitUntil(()-> wrist.atSetpoint())
-    //     );
-    // }
+    public static CommandBase CmdMoveWrist(double power) {
+        return new InstantCommand(() -> wrist.set(power), wrist);
+    }
 
     public static CommandBase CmdManipIntake(Boolean cone) {
         return sequence(

diff --git a/src/main/java/frc/team3128/subsystems/Wrist.java b/src/main/java/frc/team3128/subsystems/Wrist.java
@@ -1,21 +1,21 @@
 package frc.team3128.subsystems;
 
 import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
 
 import static frc.team3128.Constants.WristConstants.*;
 
 import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax;
+import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax.EncoderType;
+import frc.team3128.common.utility.NAR_Shuffleboard;
 
 public class Wrist extends NAR_PIDSubsystem {
     public NAR_CANSparkMax m_wrist;
 
-    private DutyCycleEncoder m_encoder;
-
     private static Wrist instance;
 
     public enum WristPosition {
@@ -50,34 +50,40 @@ public Wrist() {
         setkG_Function(()-> Math.cos(Units.degreesToRadians(getMeasurement())));
         setConstraints(MIN_ANGLE, MAX_ANGLE);
         configMotor();
-        configEncoders();
     }
 
     private void configMotor() {
-        m_wrist = new NAR_CANSparkMax(WRIST_ID);
+        m_wrist = new NAR_CANSparkMax(WRIST_ID, EncoderType.Absolute, MotorType.kBrushless);
         m_wrist.setInverted(false);
         m_wrist.setIdleMode(IdleMode.kBrake);
         m_wrist.setSmartCurrentLimit(40);
         resetEncoder();
     }
 
-    private void configEncoders() {
-        m_encoder = new DutyCycleEncoder(ENCODER_DIO_ID);
-    }
-
     @Override
     protected void useOutput(double output, double setpoint) {
         m_wrist.set(MathUtil.clamp(output / 12.0, -1, 1));
     }
 
     @Override
     public double getMeasurement() {
-        // return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO;
-        return MathUtil.inputModulus(-m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180);
+        // return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO + ANGLE_OFFSET;
+        return 12.3;
+        // return MathUtil.inputModulus(-m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180);
     }
 
     public void resetEncoder() {
         m_wrist.setSelectedSensorPosition(0);
     }
 
+    public void set(double power) {
+        m_wrist.set(power);
+    }
+
+    public void initShuffleboard() {
+        // NAR_Shuffleboard.addData("Manipulator", "Manip current", () -> getCurrent(), 0, 1);
+        // NAR_Shuffleboard.addData("Manipulator", "get", () -> m_roller.getMotorOutputPercent(), 0, 3);
+        NAR_Shuffleboard.addData("Wrist", "angle", ()-> m_wrist.getSelectedSensorPosition(), 1, 1);
+    }
+
 }