Incrmenenting stop at sensor

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

@@ -151,8 +151,8 @@ public void robotPeriodic() {
         CommandScheduler.getInstance().run();
 
         //The following 2 lines run Drivebase methods that tell shuffleboard what the motor current draw is and if motor current limiting is active.
-        DriveBase2020.getInstance().getMotorCurrent();
-        DriveBase2020.getInstance().isMotorLimitActive();
+        DriveBaseHolder.getInstance().getMotorCurrent();
+        DriveBaseHolder.getInstance().isMotorLimitActive();
     }
 
     /**

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

@@ -12,6 +12,8 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.commands.*;
 import frc.robot.config.Config;
@@ -105,13 +107,14 @@ private void configureButtonBindings() {
         positionPowercell = new PositionPowercellCommand();
         new JoystickButton(controlStick, XboxController.Button.kBumperRight.value).toggleWhenActive(positionPowercell, true);
 
-        moveToOuterPort = new TurnToOuterPortCommand(true, 3.0, 2.0);
+      //  moveToOuterPort = new TurnToOuterPortCommand(true, 3.0, 2.0);
+        moveToOuterPort = new TurnToOuterPortCommand(true, 1.0, 2.0);
         new JoystickButton(driverStick, XboxController.Button.kA.value).whenHeld(moveToOuterPort, true);
 
-        reverseArmManually = new MoveArmManuallyCommand(-0.35);
+        reverseArmManually = new MoveArmManuallyCommand(-0.15);
         new JoystickButton(driverStick, XboxController.Button.kX.value).whenHeld(reverseArmManually);
 
-        moveArm = new MoveArmManuallyCommand(10);
+        moveArm = new MoveArmManuallyCommand(0.5);
         new JoystickButton(driverStick, XboxController.Button.kY.value).whenHeld(moveArm);
 
         sensitiveDriving = new SensitiveDriverControl(driverStick);
@@ -136,10 +139,20 @@ public Command getAutonomousCommand() {
             // This is our 'do nothing' selector
             return null;
         } else if (selectorOne == 1) {
-            return new SpinUpShooterWithTime(Config.RPM.get(), 7).alongWith(new RunFeederCommandWithTime(-0.7, 7)).andThen(new DriveWithTime(0.5, 0.5, 0.5));
+            int time = 7;
+
+            Command spinUpAndShoot = new SpinUpShooterWithTime(Config.RPM.get(), time)
+                    .alongWith(new RunFeederCommandWithTime(-0.4, time));
+
+
+            Command driveWithTime = new ArcadeDriveWithTime(0.5, 0.5, 0, true);
+
+            SequentialCommandGroup sequentialCommandGroup = new SequentialCommandGroup(spinUpAndShoot, new PrintCommand("Hello!!"), driveWithTime);
+
+            return sequentialCommandGroup;
            // return new DriveWithTime(AUTO_DRIVE_TIME,  AUTO_LEFT_MOTOR_SPEED,  AUTO_RIGHT_MOTOR_SPEED);
         } else if(selectorOne == 2) {
-            return new DriveWithTime(0.5, 0.5, 0.5);
+            return new ArcadeDriveWithTime(0.5, 0.5, 0, true);
         }
 
 

src/main/java/frc/robot/commands/ArcadeDriveWithTime.java

@@ -0,0 +1,87 @@
+package frc.robot.commands;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.DriveBase;
+import frc.robot.subsystems.DriveBase2020;
+import frc.robot.subsystems.DriveBaseHolder;
+
+public class ArcadeDriveWithTime extends CommandBase {
+
+    // Initialization of the variables which will allow imported variables to be public(making usable throughout the DriveWithTime command).
+    private double seconds = 0;
+    private double forwardSpeed = 0;
+    private double rotateSpeed = 0;
+    private boolean initBrake;
+
+    /**
+     * This command will take in speeds (left and right) and time, for which is will set the motors to the x speed for y seconds.
+     *
+     * @param seconds        amount of seconds the robot motors will be activated for
+     * @param forwardSpeed     when the motor is activated, this is the speed at which the LEFT motors will be. speed can be inbetween -1 to 1.
+     * @param rotateSpeed    when the motor is activated, this is the speed at which the RIGHT motors will be. speed can be inbetween -1 to 1.
+     */
+
+    public ArcadeDriveWithTime(double seconds, double forwardSpeed, double rotateSpeed, boolean initBrake) {
+
+        // Sets the variables to public to allow their usage throughout the DriveWithTime command.
+        this.seconds = seconds;
+        this.forwardSpeed = forwardSpeed;
+        this.rotateSpeed = rotateSpeed;
+        this.initBrake = initBrake;
+        addRequirements(DriveBaseHolder.getInstance());
+
+
+
+    }
+
+    //Initializes a timer
+    private Timer timer = new Timer();
+
+
+    // Called  one when the command is initially scheduled.
+    @Override
+    public void initialize() {
+
+      //  timer = new Timer();
+
+        DriveBaseHolder.getInstance().setDriveMode(DriveBase.DriveMode.OpenLoopVoltage);
+        DriveBaseHolder.getInstance().setNeutralMode(initBrake ? NeutralMode.Brake : NeutralMode.Coast);
+
+        // Timer is started and will start counting UP from 0
+        timer.start();
+
+
+
+
+    }
+
+    // Called every time the scheduler runs while the command is scheduled. (run every 20ms)
+    @Override
+    public void execute() {
+
+        // Sets the motor speeds to those of which the user inputed
+        // DriveBaseHolder.getInstance().tankDrive(leftSpeeds,rightSpeeds,false);
+        DriveBaseHolder.getInstance().arcadeDrive(forwardSpeed, rotateSpeed,false);
+
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+        // If using the withTimeout(double seconds); as the timer, return here.
+        DriveBaseHolder.getInstance().stopMotors();
+    }
+
+
+    @Override
+    public boolean isFinished() {
+        /*
+         *Will return true when the timer's count reaches the inputed seconds, this will stop the command, disabling the motors.
+         *when returning false, the command will continue to run, and the motors will continue to spin at thei inputed rate.
+         */
+        return timer.get() >= seconds;
+    }
+
+}

src/main/java/frc/robot/commands/DriveWithTime.java

@@ -38,13 +38,15 @@ public DriveWithTime(double seconds, double leftSpeed, double rightSpeed) {
   }
 
   //Initializes a timer
-  private Timer timer = new Timer();
+  private Timer timer;
 
 
   // Called  one when the command is initially scheduled.
   @Override
   public void initialize() {
 
+    timer = new Timer();
+
     // Timer is started and will start counting UP from 0
     timer.start();
     addRequirements(DriveBaseHolder.getInstance());
@@ -56,15 +58,16 @@ public void initialize() {
   public void execute() {
 
     // Sets the motor speeds to those of which the user inputed
-    DriveBaseHolder.getInstance().tankDrive(leftSpeeds,rightSpeeds,false);
+   // DriveBaseHolder.getInstance().tankDrive(leftSpeeds,rightSpeeds,false);
+    DriveBaseHolder.getInstance().tankDrive(leftSpeeds, rightSpeeds,false);
 
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     // If using the withTimeout(double seconds); as the timer, return here.
-    DriveBase2020.getInstance().stopMotors();
+    DriveBaseHolder.getInstance().stopMotors();
   }
 
 

src/main/java/frc/robot/commands/DrivetrainPIDTurnDelta.java

@@ -52,6 +52,9 @@ public class DrivetrainPIDTurnDelta extends CommandBase {
     // A timer to ensure the command doesn't get stuck and the robot cannot drive
     private Timer timer;
 
+    private double volatileP;
+    private double volatileD;
+
 
     /**
      * Allows the robot to turn and move forward or back by itself
@@ -86,6 +89,11 @@ public boolean isFinished() {
         return pigeonIMU == null || isDone || (maxTime != null && timer.get() >= maxTime);
     }
 
+    @Override
+    public void end(boolean interrupted) {
+        drivebase.setCoastMode();
+    }
+
     @Override
     public void initialize() {
 
@@ -118,9 +126,28 @@ public void execute() {
             if (Math.abs(targetAngle - currentAngle) < acceptableError) {
                 isDone = true;
             }
+            if(Math.abs(deltaDegree) > 10 ) {
+                // Works for 20 degrees, less good for 45
+                volatileP = 0.0182;
+                volatileD = 0.001;
+
+            } else {
+                volatileP = 0.04382;
+                volatileD = 0.001;
+
+//                volatileP = 0.037;
+//                volatileD = 0.0023;
+
+                //Do PD
+
+                //Run motors according to the output of PD
+            }
+
+            turnThrottle = (targetAngle - currentAngle) * volatileP - (currentAngularRate) * volatileD;
+            drivebase.tankDrive(-turnThrottle + forwardSpeed, turnThrottle + forwardSpeed, false);
+
+            drivebase.setBrakeMode();
 
-            //Do PD
-            turnThrottle = (targetAngle - currentAngle) * pGain.get() - (currentAngularRate) * dGain.get();
 
             //Run motors according to the output of PD
             drivebase.tankDrive(-turnThrottle + forwardSpeed, turnThrottle + forwardSpeed, false);

src/main/java/frc/robot/commands/MoveArmToSetpointCommand.java

@@ -20,7 +20,8 @@ public void initialize() {
 
     @Override
     public void execute() {
-        ArmSubsystem.getInstance().setpoint(setPointIndex);
+      //  ArmSubsystem.getInstance().setpoint(setPointIndex);
+        ArmSubsystem.getInstance().setEncoderTicks(3850);
     }
 
     @Override

src/main/java/frc/robot/commands/OperatorIntakeCommand.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.AgitatorSubsystem;
+import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.ConditionalSubsystemBase;
 import frc.robot.subsystems.IntakeSubsystem;
 
@@ -17,6 +18,7 @@ public class OperatorIntakeCommand extends CommandBase {
     public OperatorIntakeCommand() {
         intakeSubsystem = IntakeSubsystem.getInstance();
         addRequirements(intakeSubsystem);
+     //   addRequirements(ArmSubsystem.getInstance());
 
         // Initialize the condition
         condition = intakeSubsystem.getCondition("operatorActivated");
@@ -26,11 +28,13 @@ public OperatorIntakeCommand() {
     public void initialize() {
         // When the command starts, tell the intake it can go
         condition.setState(true);
+        ArmSubsystem.getInstance().armLowerLimitOverride = true;
 
     }
 
     @Override
     public void execute() {
+        ArmSubsystem.getInstance().moveArm(-0.3);
 
     }
 
@@ -43,6 +47,7 @@ public boolean isFinished() {
     public void end(boolean interrupted) {
         // When the command stops, tell the intake to not go
         AgitatorSubsystem.INSTANCE.stopAgitator();
+        ArmSubsystem.getInstance().armLowerLimitOverride = false;
         condition.setState(false);
     }
 }

src/main/java/frc/robot/commands/TurnToOuterPortCommand.java

@@ -1,5 +1,6 @@
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.config.Config;
 import frc.robot.nettables.VisionCtrlNetTable;
@@ -43,6 +44,8 @@ public void initialize() {
 
         // Get the target angle from NetworkTables
         currentTarget = VisionCtrlNetTable.yawToOuterPort.get();
+
+        System.out.println("Running turn to outer port command");
     }
 
     /**
@@ -58,9 +61,9 @@ public void execute() {
             if(currentTarget <= 30 && currentTarget >= -30) {
                 // Check if the yaw should be inverted (Shooter is on the back so we may need to)
                 if (invert) {
-                    drivetrainPIDTurnDelta = new DrivetrainPIDTurnDelta(-currentTarget, 0, acceptableError, maxTime);
+                    drivetrainPIDTurnDelta = new DrivetrainPIDTurnDelta(-currentTarget, -0.2, acceptableError, maxTime);
                 } else {
-                    drivetrainPIDTurnDelta = new DrivetrainPIDTurnDelta(currentTarget, 0, acceptableError, maxTime);
+                    drivetrainPIDTurnDelta = new DrivetrainPIDTurnDelta(currentTarget, 0.2, acceptableError, maxTime);
                 }
                 drivetrainPIDTurnDelta.schedule();
             }
@@ -73,6 +76,7 @@ public void execute() {
             // Ensure no movement on faulty values
             drivetrainPIDTurnDelta = new DrivetrainPIDTurnDelta(0, 0, 0d, 0d);
             logger.log(Level.WARNING, "Invalid Current Target (Value Not Read): " + VisionCtrlNetTable.yawToOuterPort.get());
+            System.out.println("Invalid Current Target (Value Not Read): ");
         }
 
     }

src/main/java/frc/robot/config/Config.java

@@ -74,7 +74,7 @@ private Config() {
     // Static Constants
     private static Class<? extends DriveBase> Pre2020DriveBase = DriveBasePre2020.class.asSubclass(DriveBase.class);
     private static Class<? extends DriveBase> Post2020DriveBase = DriveBase2020.class.asSubclass(DriveBase.class);
-    public static Class<? extends DriveBase> DRIVEBASE_CLASS = robotSpecific(Post2020DriveBase, Post2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase);
+    public static Class<? extends DriveBase> DRIVEBASE_CLASS = robotSpecific(Post2020DriveBase, Post2020DriveBase, Post2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase, Pre2020DriveBase);
     public static int RIGHT_FRONT_MOTOR = robotSpecific(2, 2, 3, 2, 2);
     public static int RIGHT_REAR_MOTOR = robotSpecific(4, 4, 4, 4, 4);
     public static int LEFT_FRONT_MOTOR = robotSpecific(1, 1, 1, 1, 1);
@@ -131,10 +131,10 @@ private Config() {
     public static final boolean TELEOP_SQUARE_JOYSTICK_INPUTS = true;
 
     // PIDF values for the arm
-    public static double ARM_PID_P = robotSpecific(5);
+    public static double ARM_PID_P = robotSpecific(10);
     public static double ARM_PID_I = robotSpecific(0.0);
     public static double ARM_PID_D = robotSpecific(0.0);
-    public static double ARM_PID_F = robotSpecific(0.05);
+    public static double ARM_PID_F = robotSpecific(0.07);
 
 
     // Define a global constants table for subsystems to use
@@ -149,10 +149,10 @@ private Config() {
     public static String YAW_OUTER_PORT         = "YawToTarget";
 
     // Drivetrain PID values
-    public static double DRIVETRAIN_P_SPECIFIC = robotSpecific(0.037, 0.0, 0.0, 0.018d, 0.0, 0.25);
-    public static double DRIVETRAIN_D_SPECIFIC = robotSpecific(0.0023, 0.0, 0.0, 0.0016d, 0.0, 0.03);
+    public static double DRIVETRAIN_P_SPECIFIC = robotSpecific(0.037, 0.037, 0.0, 0.018d, 0.0, 0.25);
+    public static double DRIVETRAIN_D_SPECIFIC = robotSpecific(0.0023, 0.0023, 0.0, 0.0016d, 0.0, 0.03);
 
-    public static final FluidConstant<Integer> RPM = new FluidConstant<>("Shooter RPM", 1700)
+    public static final FluidConstant<Integer> RPM = new FluidConstant<>("Shooter RPM", 2000)
             .registerToTable(Config.constantsTable);
 
     public static FluidConstant<Double> DRIVETRAIN_P = new FluidConstant<>("DrivetrainP", DRIVETRAIN_P_SPECIFIC)

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

@@ -14,8 +14,8 @@
 public class ArmSubsystem extends ConditionalSubsystemBase {
 
     // TODO Change placeholder values to actual limits
-    private static final int FORWARD_LIMIT_TICKS = Config.robotSpecific(4150, 2200);
-    private static final int REVERSE_LIMIT_TICKS = Config.robotSpecific(3500, 1300);
+    private static final int FORWARD_LIMIT_TICKS = Config.robotSpecific(4300, 2100);
+    private static final int REVERSE_LIMIT_TICKS = Config.robotSpecific(3490, 1300);
 
     private static final int acceptableError = 50;
 
@@ -125,6 +125,10 @@ public void zeroTalonEncoder() {
         armTalon.setSelectedSensorPosition(0);
     }
 
+    public void setEncoderTicks(int ticks) {
+        currentPosition = ticks;
+    }
+
     public void setpoint(int setpointIndex) {
         if(setpointIndex < setpoints.length) {
             currentPosition = setpoints[setpointIndex];
@@ -137,6 +141,8 @@ public void setpoint(int setpointIndex) {
     public void moveArm(double speed) {
         armTalon.set(speed);
     }
+
+    public boolean armLowerLimitOverride = false;
 
     @Override
     public void periodic() {
@@ -151,7 +157,7 @@ public void periodic() {
 
         armTalon.set(ControlMode.Position, currentPosition);
 
-        if(armTalon.getSelectedSensorPosition() <= REVERSE_LIMIT_TICKS + 25) {
+        if(armTalon.getSelectedSensorPosition() <= REVERSE_LIMIT_TICKS + 15 ) {
             armTalon.set(0.0);
         }
     }