fix: Turret gear rations, brake configs, motor inversion, and current…

… limits! Also converted everything to degrees to make Alvin happy.
flamingchickens1540 · Dec 15, 2023 · 42de6b1 · 42de6b1
1 parent a9f68d5
commit 42de6b1
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 16 deletions.
diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java
@@ -44,9 +44,10 @@ public static class DrivetrainConstants {
     }
 
     public static class TurretConstants{
-        public static final int MOTOR_ID = 0; //TODO: FIGURE OUT CAN ID!!!!!
-        public static final double kP = 20;
+        public static final int MOTOR_ID = 9; //TODO: MAKE SURE THIS CAN ID WORKS!!!
+        public static final double kP = 3.2;
         public static final double kI = 0;
         public static final double kD = 0.1;
+        public static final double gearRatio = 166.66666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666667;
     }
 }
diff --git a/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java b/src/main/java/org/team1540/bunnybotTank2023/commands/turret/Turret.java
@@ -12,8 +12,8 @@ public class Turret extends SubsystemBase {
     private final TurretIO io;
 
     //constructor
-    public Turret(TurretIO argIO){
-        io = argIO;
+    public Turret(TurretIO io){
+        this.io = io;
     }
 
     //periodic

diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIO.java b/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIO.java
@@ -8,11 +8,11 @@ public interface TurretIO {
     @AutoLog
     class TurretInputs{
         // fields:
-        public double currentPosition = 0;
-        public double setPointRotations = 0;
-        public double voltage = 0;
-        public double current = 0;
-        public double velocity = 0;
+        public double motorCurrentPositionDegrees = 0;
+        public double SetPointDegrees = 0;
+        public double motorVoltage = 0;
+        public double motorCurrentAmps = 0;
+        public double motorVelocityRPM = 0;
         public boolean forwardLimitSwitch = false;
         public boolean reverseLimitSwitch = false;
     }

diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/turret/TurretIOReal.java
@@ -1,18 +1,22 @@
 package org.team1540.bunnybotTank2023.io.turret;
 
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.ForwardLimitValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.ReverseLimitValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.team1540.bunnybotTank2023.Constants;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.signals.InvertedValue;
 
 public class TurretIOReal implements TurretIO {
 
     // fields ^-^
     private final TalonFX motor = new TalonFX(Constants.TurretConstants.MOTOR_ID);
-    final PositionVoltage positionVoltage = new PositionVoltage(0);
+    private final PositionVoltage positionVoltage = new PositionVoltage(0).withSlot(0);
     private Rotation2d setPosition = new Rotation2d();
 
     // constructor
@@ -26,6 +30,20 @@ public class TurretIOReal implements TurretIO {
         motor.getConfigurator().apply(slot0Configs, 0.050);
 
         positionVoltage.Slot = 0;
+
+        // Current Limits
+        CurrentLimitsConfigs turretCurrentLimit = new CurrentLimitsConfigs();
+        turretCurrentLimit.SupplyCurrentLimitEnable = true;
+        turretCurrentLimit.SupplyCurrentLimit = 20;
+        turretCurrentLimit.SupplyCurrentThreshold = 60;
+        turretCurrentLimit.SupplyTimeThreshold = 0.1;
+
+        // Motor Inversion
+        MotorOutputConfigs turretOutput = new MotorOutputConfigs();
+        turretOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+        // Brake Mode
+        turretOutput.NeutralMode = NeutralModeValue.Brake;
     }
 
     // sets the voltage
@@ -35,18 +53,20 @@ public void setVoltage(double volts) {
         motor.setVoltage(volts);
     }
 
+    @Override
     public void setTurretPosition(Rotation2d position) {
-        motor.setControl(positionVoltage.withPosition(position.getRotations()));
+        // using the gear ratio to convert from motor rotations to mechanism rotations
+        motor.setControl(positionVoltage.withPosition(position.getRotations()/Constants.TurretConstants.gearRatio));
         setPosition = position;
     }
 
     @Override
     public void updateInputs(TurretInputs inputs) {
-        inputs.velocity = motor.getVelocity().getValue();
-        inputs.currentPosition = motor.getPosition().getValue();
-        inputs.setPointRotations = setPosition.getRotations();
-        inputs.current = motor.getSupplyCurrent().getValue();
-        inputs.voltage = motor.getSupplyVoltage().getValue();
+        inputs.motorVelocityRPM = motor.getVelocity().getValue();
+        inputs.motorCurrentPositionDegrees = motor.getPosition().getValue() * 360;
+        inputs.SetPointDegrees = setPosition.getRotations() * 360;
+        inputs.motorCurrentAmps = motor.getSupplyCurrent().getValue();
+        inputs.motorVoltage = motor.getSupplyVoltage().getValue();
         inputs.forwardLimitSwitch = (motor.getForwardLimit().getValue() == ForwardLimitValue.ClosedToGround);
         inputs.reverseLimitSwitch = (motor.getReverseLimit().getValue() == ReverseLimitValue.ClosedToGround);
     }