Added code for robot to rotate precise angle and drive forward precis…

6238_prototype/src/main/java/frc/robot/IOConstants.java

@@ -10,4 +10,6 @@ public final class IOConstants {
     public static final int ROTATE_DOWN = 8;
     public static final int CLIMBER_AUTO_EXTEND = 9;
     public static final int CLIMBER_LIMIT_TOGGLE = 11;
+    public static final int TARGET_ANGLE_DEGREES = 3;
+    public static final int OVERRIDE_SHOOTER_SENSOR = 10;
 }

6238_prototype/src/main/java/frc/robot/Robot.java

@@ -64,7 +64,9 @@ public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
+
     if (m_autonomousCommand != null) {
+      System.out.println("autonomousSchedule ");
       m_autonomousCommand.schedule();
     }
   }

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

@@ -4,10 +4,13 @@
 
 package frc.robot;
 
+import java.sql.Driver;
+
 import edu.wpi.first.wpilibj.Joystick;
 import frc.robot.commands.ClimberCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.commands.DriveCommand;
+import frc.robot.commands.RotateCommand;
 import frc.robot.commands.StopShooterCommand;
 import frc.robot.commands.TuningShooterCommand;
 import frc.robot.commands.Intake.ExtendIntakeCommand;
@@ -30,6 +33,7 @@ public class RobotContainer {
     private BallSubsystem ballSubsystem;
     private Joystick joystick;
     private CameraSubsystem cameraSubsystem;
+    private final double targetAngleDegrees = 180;
 
     private void addClimber() {
         joystick = new Joystick(IOConstants.JOYSTICK_PORT);
@@ -57,12 +61,14 @@ private void addClimber() {
 
         new JoystickButton(joystick, IOConstants.CLIMBER_LIMIT_TOGGLE)
                 .whenPressed(() -> climberSubsystem.toggleLimitSwitchEnabled());
+
     }
 
     private void addDrive() {
         driveSubsystem = new DriveSubsystem();
         driveCommand = new DriveCommand(driveSubsystem, joystick);
         driveSubsystem.setDefaultCommand(driveCommand);
+
     }
 
     private void addBall() {
@@ -99,6 +105,8 @@ public RobotContainer() {
     }
 
     public Command getAutonomousCommand() {
+        System.out.println("getAutononmousCommand ");
+        // return new RotateCommand(targetAngleDegrees, driveSubsystem);
         return new AutonomousComand(driveSubsystem, ballSubsystem);
     }
 }

6238_prototype/src/main/java/frc/robot/commands/AutonomousComand.java

@@ -15,7 +15,7 @@ public AutonomousComand(DriveSubsystem drive, BallSubsystem ball) {
         this.drive = drive;
         this.ball = ball;
         timeLimit = 10000;
-        speed = 0.5;
+        speed = 0.4;
         drive.setDrive(0,0);
         ball.setSpeed (0,0);
         addRequirements(drive, ball);
@@ -28,21 +28,22 @@ public void initialize() {
     @Override
     public void execute() {
         long timeElapsed = System.currentTimeMillis() - startTime;
-        if (2000 < timeElapsed && timeElapsed < 7000 ){
+        if (500 < timeElapsed && timeElapsed < 6000 ){
             drive.setDrive(0,0);
-            double bottomMotorSpeed = 0;
             double upperMotorRPM = BallTunedConstants.AUTONOMOUS_MODE_UPPER_MOTOR_RPM;
-            double upperMotorRPMCurrent = ball.getRPMUpperMotor();
+            ball.setSpeed(0, upperMotorRPM);
 
-            if (BallTunedConstants.AUTONOMOUS_MODE_UPPER_MOTOR_RPM - 50 < upperMotorRPMCurrent
-                && upperMotorRPMCurrent< BallTunedConstants.AUTONOMOUS_MODE_UPPER_MOTOR_RPM + 50) {
-                bottomMotorSpeed = 1;
+            if (3000 < timeElapsed && timeElapsed < 6000) {
+                ball.activateLeftKicker(true);
+                ball.activateRightKicker(true);
+            } else {
+                ball.activateLeftKicker(false);
+                ball.activateRightKicker(false);
             }
-            ball.setSpeed(bottomMotorSpeed, upperMotorRPM);
+
         } else {
             drive.setDrive(speed,0);
             ball.setSpeed(0,0);
-
         }
     }
 
@@ -56,4 +57,5 @@ public boolean isFinished() {
     public void end(boolean interrupted) {
         drive.setDrive(0,0);
     }
+
 }

6238_prototype/src/main/java/frc/robot/commands/BallTunedConstants.java

@@ -2,6 +2,6 @@
 
 public class BallTunedConstants {
     public final static double INTAKE_MODE_UPPER_MOTOR_RPM = 0;
-    public final static double INTAKE_MODE_LOWER_MOTOR_SPEED = 0.6;
-    public final static double AUTONOMOUS_MODE_UPPER_MOTOR_RPM = 5000;
+    public final static double INTAKE_MODE_LOWER_MOTOR_SPEED = 0.75;
+    public final static double AUTONOMOUS_MODE_UPPER_MOTOR_RPM = 4600;
 }

6238_prototype/src/main/java/frc/robot/commands/DriveCommand.java

@@ -14,8 +14,8 @@ public class DriveCommand extends CommandBase {
     private double maximumAcceleration;
     // Positive Value
 
-    private final SmartDashboardParam accelerationSlewRate = new SmartDashboardParam("accelerationSlewRate", 0.05);
-    private final SmartDashboardParam deccelerationSlewRate = new SmartDashboardParam("deccelerationSlewRate", 1);
+    private final SmartDashboardParam accelerationSlewRate = new SmartDashboardParam("accelerationSlewRate", 0.03);
+    private final SmartDashboardParam deccelerationSlewRate = new SmartDashboardParam("deccelerationSlewRate", 0.1);
 
     private double previousSpeed;
 
@@ -71,9 +71,9 @@ public void execute() {
         speedRot *= Math.abs(speedRot) < 0.05 ? 0 : 1;
 
         if (speedRot > .05){
-            speedRot = speedRot * .65 + .25;
+            speedRot = speedRot * .50 + .25;
         } else if (speedRot < -.05) {
-            speedRot = speedRot * .65 - .25;
+            speedRot = speedRot * .50 - .25;
         } else {
             speedRot = 0;
         }

6238_prototype/src/main/java/frc/robot/commands/DriveDistanceCommand.java

@@ -0,0 +1,65 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc.robot.SmartDashboardParam;
+import frc.robot.subsystems.DriveSubsystem;
+
+public class DriveDistanceCommand extends PIDCommand{
+    private final double target;
+    private final DriveSubsystem driveSubsystem;
+
+    SmartDashboardParam kPSlider = new SmartDashboardParam("kPAutonomousDrive", 0);
+    SmartDashboardParam kISlider = new SmartDashboardParam("kIAutonomousDrive", 0);
+    SmartDashboardParam kDSlider = new SmartDashboardParam("kDAutonomousDrive", 0);
+
+    private double kP;
+    private double kI;
+    private double kD;
+
+    static private final double kSpeedToleranceMetersPerS = 2.0;
+    static private final double kDistanceToleranceMeters = 2.0;
+
+    public DriveDistanceCommand(double targetDistanceMeters, DriveSubsystem driveSubsystem) {
+        super(
+            new PIDController(0, 0, 0),
+            driveSubsystem::getPosition,
+            targetDistanceMeters,
+            output -> driveSubsystem.setDrive(output > 0.3 ? 0.3 : (output < -0.3 ? -0.3 : output), 0),
+            driveSubsystem
+            );
+        this.target = targetDistanceMeters;
+        this.driveSubsystem = driveSubsystem;
+        driveSubsystem.resetEncoders();
+
+        // kSppedToleranceMetersPerS ensures the robot is stationary at the
+        // setpoint before it is considered as having reached the reference
+        getController()
+            .setTolerance(kDistanceToleranceMeters, kSpeedToleranceMetersPerS);
+
+        // addRequirements(driveSubsystem) is called in the parent constructor
+    }
+
+    @Override
+    public void execute() {
+        if (kPSlider.get() != kP) {
+            kP = kPSlider.get();
+            getController().setP(kP);
+        }
+        if (kISlider.get() != kI) {
+            kI = kISlider.get();
+            getController().setI(kI);
+        }
+        if (kDSlider.get() != kD) {
+            kD = kDSlider.get();
+            getController().setD(kD);
+        }
+        SmartDashboard.putNumber("Distance Error", driveSubsystem.getPosition() - target);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return getController().atSetpoint();
+    }
+}

6238_prototype/src/main/java/frc/robot/commands/RotateCommand.java

@@ -0,0 +1,74 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
+import frc.robot.SmartDashboardParam;
+import frc.robot.subsystems.DriveSubsystem;
+
+public class RotateCommand extends PIDCommand{
+    private final double target;
+    private final DriveSubsystem driveSubsystem;
+
+    SmartDashboardParam kPSlider = new SmartDashboardParam("kPAutonomousDrive", 0.03);
+    SmartDashboardParam kISlider = new SmartDashboardParam("kIAutonomousDrive", 0);
+    SmartDashboardParam kDSlider = new SmartDashboardParam("kDAutonomousDrive", 0);
+
+    private double kP;
+    private double kI;
+    private double kD;
+
+    static private final double kTurnRateToleranceDegPerS = 2.0;
+    static private final double kTurnToleranceDeg = 2.0;
+
+
+    public RotateCommand(double targetAngleDegrees, DriveSubsystem driveSubsystem) {
+        super(
+            new PIDController(0, 0, 0),
+            driveSubsystem::getAngle,
+            targetAngleDegrees,
+            driveSubsystem::setPIDRotateOutput
+            ); 
+
+        target = targetAngleDegrees;
+        this.driveSubsystem = driveSubsystem;
+        System.out.println ("RotateCommandConstruct" + targetAngleDegrees);
+        getController().enableContinuousInput(-180, 180);
+
+        // kTurnRateToleranceDegPerS ensures the robot is stationary at the
+        // setpoint before it is considered as having reached the reference
+        getController()
+            .setTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS);
+
+        addRequirements(driveSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        driveSubsystem.resetAngle();
+    }
+
+    @Override
+    public void execute() {
+        super.execute();
+        if (kPSlider.get() != kP) {
+            kP = kPSlider.get();
+            getController().setP(kP);
+        }
+        if (kISlider.get() != kI) {
+            kI = kISlider.get();
+            getController().setI(kI);
+        }
+        if (kDSlider.get() != kD) {
+            kD = kDSlider.get();
+            getController().setD(kD);
+        }
+        SmartDashboard.putNumber("Rotate Error", driveSubsystem.getAngle() - target);
+        System.out.println ("RotateCommandExecute" + driveSubsystem.getAngle() + " " + target);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return getController().atSetpoint();
+    }
+}

6238_prototype/src/main/java/frc/robot/commands/ShooterCommand.java

@@ -1,7 +1,9 @@
 package frc.robot.commands;
 
 
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.IOConstants;
 import frc.robot.subsystems.BallSubsystem;
 
 public class ShooterCommand extends CommandBase{
@@ -14,7 +16,12 @@ public static enum PneumaticKickers {LEFT_KICKER, RIGHT_KICKER, BOTH};
     private final long timeLimit;
     private final double rpmDiffTolerance;
     private long countdownStart;
-
+
+
+    private Joystick joystick;
+    enum OverridePreviousKickerEnum {LEFT, RIGHT};
+    OverridePreviousKickerEnum overridePreviousKicker;
+
 
     private void printStuff(String location){
   /* dont run debugging in comp
@@ -34,11 +41,13 @@ private void printStuff(String location){
 
 
     public ShooterCommand(BallSubsystem ball, PneumaticKickers kicker, double rpmTarget) {
+        overridePreviousKicker = OverridePreviousKickerEnum.RIGHT;
         this.ball = ball;
         upperShooterRPMTarget = rpmTarget;
         timeLimit = 300;
-        rpmDiffTolerance = 50;
+        rpmDiffTolerance = 100;
         this.kicker = kicker;
+        this.joystick = new Joystick(IOConstants.JOYSTICK_PORT);
         addRequirements(ball);
     }
 
@@ -55,11 +64,13 @@ public void initialize(){
     @Override
     public void execute() {
         printStuff("Execute");
-        if (Math.abs(ball.getRPMUpperMotor() - upperShooterRPMTarget) > rpmDiffTolerance) {
+        if (Math.abs(ball.getRPMUpperMotor() - upperShooterRPMTarget) > rpmDiffTolerance && !joystick.getRawButton(IOConstants.OVERRIDE_SHOOTER_SENSOR)) {
 
             restartTimer();
         }
+
         if (System.currentTimeMillis() - timerStart > timeLimit) {
+
             printStuff("Shoot");
 
             switch (kicker) {

6238_prototype/src/main/java/frc/robot/commands/TuningShooterCommand.java

@@ -7,7 +7,7 @@ public class TuningShooterCommand extends ShooterCommand {
     SmartDashboardParam upperShooterRPMTargetSlider;
     public TuningShooterCommand(BallSubsystem ball, PneumaticKickers kicker) {
         super(ball, kicker, 0);
-        upperShooterRPMTargetSlider = new SmartDashboardParam("upperShooterRPMTarget", 4600);
+        upperShooterRPMTargetSlider = new SmartDashboardParam("upperShooterRPMTarget", 4400);
     }
 
     @Override

6238_prototype/src/main/java/frc/robot/subsystems/BallSubsystem.java

@@ -2,6 +2,7 @@
 
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.Solenoid;
@@ -14,6 +15,7 @@
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import frc.robot.Constants;
+import frc.robot.IOConstants;
 import frc.robot.SmartDashboardParam;
 
 public class BallSubsystem extends SubsystemBase {
@@ -31,7 +33,7 @@ public class BallSubsystem extends SubsystemBase {
     SmartDashboardParam shooterPGainSlider = new SmartDashboardParam("shooterPGain", 0.00016);
     SmartDashboardParam shooterIGainSlider = new SmartDashboardParam("shooterIGain",0.000001);
     SmartDashboardParam shooterDGainSlider = new SmartDashboardParam("shooterDGain",0);
-    SmartDashboardParam shooterFFGainSlider = new SmartDashboardParam("shooterFFGain",0.000205);
+    SmartDashboardParam shooterFFGainSlider = new SmartDashboardParam("shooterFFGain",0.000208);
 
     double kP;
     double kI;
@@ -71,7 +73,6 @@ public BallSubsystem() {
         pidController.setFF(kFF); // 0.00018  | .00018 | 0.0000058  ==> 0.000205
         pidController.setOutputRange(-1, 1);
 
-
     }
 
     public void activateLeftKicker(boolean activate){
@@ -118,6 +119,8 @@ public void periodic() {
             pidController.setFF(kFF);
         }
 
+
+
         if (isExtended) {
             doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
         } else {