shooter method real value conversions on pivots, drivetrain slew gone

src/main/java/frc/robot/RobotContainer.java

@@ -36,8 +36,8 @@ public class RobotContainer {
 
   private Drivetrain drivetrain;
 
-  private CommandXboxController primary;
+  private CommandXboxController driver;
-  //private CommandXboxController secondary;
+  private CommandXboxController operator;
 
   private Trigger isTeleopTrigger;
 
@@ -75,7 +75,7 @@ public class RobotContainer {
     // TODO Specify a default auto string once we have one
     autoChooser = AutoBuilder.buildAutoChooser();
 
-    primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
+    driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
     //secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
     isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
@@ -107,64 +107,64 @@ public class RobotContainer {
 
     drivetrain.setDefaultCommand(
       drivetrain.teleopCommand(
-        primary::getLeftY, 
+        driver::getLeftY, 
-        primary::getLeftX, 
+        driver::getLeftX, 
-        primary::getRightX, 
+        driver::getRightX, 
         OIConstants.kTeleopDriveDeadband
       )
     );
 
-    primary.povCenter().onFalse(
+    driver.povCenter().onFalse(
       drivetrain.driveCardinal(
-        primary::getLeftY, 
+        driver::getLeftY, 
-        primary::getLeftX, 
+        driver::getLeftX, 
-        primary.getHID()::getPOV,
+        driver.getHID()::getPOV,
         OIConstants.kTeleopDriveDeadband).until(
-          () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+          () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
         )
     );
 
     // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
-    primary.a().onTrue(
+    driver.a().onTrue(
       drivetrain.driveAprilTagLock(
-        primary::getLeftY, 
+        driver::getLeftY, 
-        primary::getLeftX, 
+        driver::getLeftX, 
         OIConstants.kTeleopDriveDeadband, 
         ampTagID
       ).until(
-        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+        () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
       )
     );
 
     // Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
-    primary.b().onTrue(
+    driver.b().onTrue(
       drivetrain.driveAprilTagLock(
-        primary::getLeftY, 
+        driver::getLeftY, 
-        primary::getLeftX,  
+        driver::getLeftX,  
         OIConstants.kTeleopDriveDeadband, 
         sourceTagID
       ).until(
-        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+        () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
       )
     );
 
     // Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
-    primary.x().onTrue(
+    driver.x().onTrue(
       drivetrain.driveAprilTagLock(
-        primary::getLeftY, 
+        driver::getLeftY, 
-        primary::getLeftX, 
+        driver::getLeftX, 
         OIConstants.kTeleopDriveDeadband, 
         speakerTag
       ).until(
-        () -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
+        () -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
       )
     );
 
     // This was originally a run while held, not sure that's really necessary, change it if need be
-    primary.y().onTrue(drivetrain.zeroHeadingCommand());
+    driver.y().onTrue(drivetrain.zeroHeadingCommand());
 
     // This was originally a run while held, not sure that's really necessary, change it if need be
-    primary.rightBumper().onTrue(drivetrain.setXCommand());
+    driver.rightBumper().onTrue(drivetrain.setXCommand());
 
     /*
      * This has been added because interest has been expressed in trying field relative vs
@@ -175,7 +175,7 @@ public class RobotContainer {
      * If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
      * mechanism will need to be devised, this will work for now. 
      */
-    primary.start().onTrue(drivetrain.toggleFieldRelativeControl());
+    driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
   }
 
   private void shuffleboardSetup() {

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -4,7 +4,7 @@ public class IntakeConstants {
     public static final int kIntakePivotID = 9;
     public static final int kIntakeRollerID = 10;
 
-    public static final double kIntakePivotConversionFactor = 0;
+    public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0));
 
     public static final int kPivotCurrentLimit = 40;
 

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -17,6 +17,8 @@ public class ShooterConstants {
     public static final double kShooterPivotI = 0.0;
     public static final double kShooterPivotD = 0.0;
 
+    public static final double kPivotConversion = 1/(40.0*(28.0/15.0));
+
     public static final double kSShooterPivotFF = 0.0;
     public static final double kGShooterPivotFF = 0.0;
     public static final double kVShooterPivotFF = 0.0;

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -301,7 +301,7 @@ public class Drivetrain extends SubsystemBase {
                 -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
                 -MathUtil.applyDeadband(rotation.getAsDouble(), deadband), 
                 () -> fieldRelativeControl, 
-                true
+                false
             );
         });
     }

src/main/java/frc/robot/subsystems/Shooter.java

@@ -6,9 +6,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -31,7 +29,7 @@ public class Shooter extends SubsystemBase{
     private PIDController bottomShooterPID;
     private SimpleMotorFeedforward bottomShooterFF;
 
-    private ProfiledPIDController shooterPivotPID;
+    private PIDController shooterPivotPID;
     private ArmFeedforward shooterPivotFF;
 
     public Shooter(){
@@ -44,6 +42,7 @@ public class Shooter extends SubsystemBase{
         shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
 
         pivotEncoder = new Encoder(0, 1);
+        pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
 
         //TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed
         topShooterPID = new PIDController(
@@ -60,14 +59,10 @@ public class Shooter extends SubsystemBase{
         topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
         topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
 
-        shooterPivotPID = new ProfiledPIDController(
+        shooterPivotPID = new PIDController(
             ShooterConstants.kShooterPivotP,
             ShooterConstants.kShooterPivotI,
-            ShooterConstants.kShooterPivotD,
+            ShooterConstants.kShooterPivotD
-            new TrapezoidProfile.Constraints(
-                ShooterConstants.kMaxPivotSpeed, 
-                ShooterConstants.kMaxPivotAcceleration
-            )
         );
 
         shooterPivotFF = new ArmFeedforward(
@@ -77,11 +72,15 @@ public class Shooter extends SubsystemBase{
         );
     }
 
-    public Command setShooterAngle(double setpointAngle){
+    public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
         return run(()-> {
             shooterPivot.setVoltage(
                 shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + 
-                shooterPivotFF.calculate(setpointAngle, 0));
+                shooterPivotFF.calculate(setpointAngle, 0.0));
+
+            topShooter.setVoltage(topShooterPID.calculate(topEncoder.getVelocity(), topRPM)+topShooterFF.calculate(topRPM));
+            
+            bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM));
         });
     }
 }