i'm cooked

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 4f1ce7cf2bff8b794472a32e7b87e80f877f9cd9..ec744f4b0f2446251695568872517b9499bb20b6 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -207,7 +207,7 @@ public class AutoShootCommand extends Command {
 
         turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
         //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
-        shooter.setShooter(goalState.exitVel());
+        //shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
         SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
         SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);

diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index b029e1cef02998b8a37d065271cb21cfd09254cb..d12c69d33af2a8688b4daaec0ad942125e124efc 100644 (file)
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -48,10 +48,10 @@ public class Shooter extends SubsystemBase implements ShooterIO {
         updateInputs();
         
         TalonFXConfiguration config = new TalonFXConfiguration();
-        config.Slot0.kP = 0.1; //tune p value
+        config.Slot0.kP = 0.15; //tune p value
         config.Slot0.kI = 0;
-        config.Slot0.kD = 0;
-        config.Slot0.kV = 0.12; //Maximum rps = 100 --> 12V/100rps
+        config.Slot0.kD = 0.0;
+        config.Slot0.kV = 0.125; //Maximum rps = 100 --> 12V/100rps
         shooterMotorLeft.getConfigurator().apply(config);
         shooterMotorRight.getConfigurator().apply(config);
         
@@ -64,7 +64,7 @@ public class Shooter extends SubsystemBase implements ShooterIO {
             new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
         );
 
-        SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(20.0)));
+        SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0)));
     }
 
     @Override
@@ -73,8 +73,10 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
         powerModifier = SmartDashboard.getNumber("shooter power modifier", powerModifier);
         SmartDashboard.putNumber("shooter power modifier", powerModifier);
-        shooterMotorLeft.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier));
-        shooterMotorRight.setControl(voltageRequest.withVelocity(shooterTargetSpeed * powerModifier));          
+        shooterMotorLeft.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2))));
+        shooterMotorRight.setControl(voltageRequest.withVelocity(Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2))));   
+        
+        Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
     }
 
     public void setShooter(double linearVelocityMps) {
@@ -88,8 +90,8 @@ public class Shooter extends SubsystemBase implements ShooterIO {
 
     @Override
     public void updateInputs(){
-        inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble());
-        inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble());
+        inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
+        inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())* ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
         Logger.processInputs("Shooter", inputs);
     }
 

diff --git a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java
index c265cec05d01dd60c91b48d8c31652c7748f79e3..67486bd39c51b196d3cc956d6fe23411e41b5a66 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java
+++ b/src/main/java/frc/robot/subsystems/turret/ShotInterpolation.java
@@ -7,6 +7,8 @@ public class ShotInterpolation {
     public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap();
     public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap();
     public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap();
+    
+    public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap();
 
     static{
         timeOfFlightMap.put(0.0, 0.67);
@@ -17,5 +19,8 @@ public class ShotInterpolation {
 
         hoodAngleMap.put(0.0, Units.degreesToRadians(90));
         hoodAngleMap.put(1.0, Units.degreesToRadians(90));
+
+        exitVelocityMap.put(1.0, 2.0);
+        exitVelocityMap.put(2.0, 4.0);
     }
 }

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 2a7721dbd1a520cde2a03ab93bd4e02015509166..f856892ebeaf8fe8f7fc97b990f298e848e47df0 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -56,12 +56,13 @@ public class Turret extends SubsystemBase implements TurretIO{
        private static final double MAX_VEL_RAD_PER_SEC = 15;
        //private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now
        // private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
-       private static final double MAX_ACCEL_RAD_PER_SEC2 = 60.0;
+       private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0;
 
        private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO;
 
-       private static final PIDController positionPID = new PIDController(15, 0, 0.25);
-       private static final PIDController velocityPID = new PIDController(0.0, 0.0, 0.0);
+       private static final PIDController positionPID = new PIDController(15.0, 0, 0.0);
+       // private static final PIDController positionPID = new PIDController(15, 0, 0.25);
+       private static final PIDController velocityPID = new PIDController(0.2, 0.0, 0.0);
 
     private final CANcoder encoderLeft = new CANcoder(0, Constants.SUBSYSTEM_CANIVORE_CAN);
     private final CANcoder encoderRight = new CANcoder(1, Constants.SUBSYSTEM_CANIVORE_CAN);
@@ -105,7 +106,7 @@ public class Turret extends SubsystemBase implements TurretIO{
        private final MechanismLigament2d ligament = root.append(new MechanismLigament2d("barrel", 30, 0));
 
     // private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0.010);
-    private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, 1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt, 0);
+    private final SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(0.1, (1. / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt) * 0.8, 0);
        private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
 
        /* ---------------- Constructor ---------------- */
@@ -113,10 +114,10 @@ public class Turret extends SubsystemBase implements TurretIO{
        public Turret() {
                motor.setNeutralMode(NeutralModeValue.Coast);
 
-               motor.getConfigurator().apply(
-                               new Slot0Configs()
-                                               .withKP(kP)
-                                               .withKD(kD));
+               // motor.getConfigurator().apply(
+               //              new Slot0Configs()
+               //                              .withKP(kP)
+               //                              .withKD(kD));
 
                TalonFXConfiguration config = new TalonFXConfiguration();
                var motionMagicConfigs = config.MotionMagic;
@@ -228,17 +229,17 @@ public class Turret extends SubsystemBase implements TurretIO{
 
                double motorSetpointPosition = (setpoint.position) * GEAR_RATIO;
 
-               targetVelocity = positionPID.calculate(
+               targetVelocity =
+               positionPID.calculate(
                                motor.getPosition().getValue().in(edu.wpi.first.units.Units.Radians),
                                motorSetpointPosition);
                        
-               //targetVelocity += Units.rotationsToRadians(motorVelRotPerSec); // TODO: Change this back after full rotation
-               targetVelocity += Math.abs(setpoint.position) != Math.PI/2 ? Units.rotationsToRadians(motorVelRotPerSec) : 0;
+               targetVelocity += Units.rotationsToRadians(motorVelRotPerSec) * 1.0;
 
                double voltage = feedForward.calculate(targetVelocity);
 
-               double velocityCorrectionVoltage = velocityPID.calculate(Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()), targetVelocity);
-               voltage += velocityCorrectionVoltage;
+               // double velocityCorrectionVoltage = velocityPID.calculate(Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()), targetVelocity);
+               // voltage += velocityCorrectionVoltage;
 
                motor.setVoltage(voltage);
 
@@ -279,6 +280,7 @@ public class Turret extends SubsystemBase implements TurretIO{
                inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
         inputs.encoderLeftRot = encoderLeft.getAbsolutePosition().getValueAsDouble();
         inputs.encoderRightRot = encoderRight.getAbsolutePosition().getValueAsDouble();
+               inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
        }
 
        private double wrapUnit(double value) {

diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java
index 44c1be3b11136ab7cc96a19170ccf68eec0b578d..a7bd928366220e9cd9d7600234c91516ebd04cc9 100644 (file)
--- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java
@@ -10,6 +10,7 @@ public interface TurretIO {
         public double motorCurrent = 0;
         public double encoderLeftRot = 0;
         public double encoderRightRot = 0;
+        public double motorVoltage = 0;
     }
 
     public void updateInputs();