Better codriver controls (#28)

* Rename stopAll to stopAllDriveMotors * remove import * Start more OOP approach to codriver controls * implement OOP based codriver controls
WTMCRobotics · Mar 2, 2024 · a6d7c63 · a6d7c63
1 parent 2fa899e
commit a6d7c63
Show file tree

Hide file tree

Showing 11 changed files with 133 additions and 66 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -16,7 +16,9 @@ public class Constants {
 
     /*SHOOTER CONFIG*/
     /**the amount of time we let the wheels rev up when shooting in seconds */
-    public static final double REV_TIME = 1.0;
+    public static final double SHOOTER_FLYWHEEL_STARTUP_TIME = 1.0;
+    /*The amount of seconds that the feeder should wait before turning off after shooting */
+    public static final double FEEDER_MOTOR_SHUTOFF_TIME = 0;
 
     /*CLIMB CONFIG */
     public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5;
@@ -60,4 +62,5 @@ public class Constants {
     /* ROTATION PID CONSTANTS */
 
     public static final double ROTATION_ERROR_DEGREES = 5.;
+    public static final double SHOOTING_VELOCITY = 4000;
 }
diff --git a/src/main/java/frc/robot/InputtedCoDriverControls.java b/src/main/java/frc/robot/InputtedCoDriverControls.java
@@ -1,9 +1,9 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.XboxController;
-import frc.robot.auton.AutonShoot;
-import frc.robot.auton.ExtendArms;
-import frc.robot.auton.RetractArms;
+import frc.robot.RobotComponents.Climber;
+import frc.robot.RobotComponents.Intake;
+import frc.robot.RobotComponents.Shooter;
 
 public class InputtedCoDriverControls {
 
@@ -14,62 +14,27 @@ public static void setCoDriverController(XboxController xboxController) {
     }
 
     public static void onEveryFrame() {
-        Robot.motors.getFeeder().stopMotor();
-        if (controller.getAButton()) {
-            Robot.motors.getFeeder().set(30);
+        if (controller.getAButtonPressed()) {
+            Shooter.startFeedMotors();
         }
-        if (controller.getXButtonPressed()) {
-            shootIntoSpeaker();
+        if (controller.getXButton()) {
+            Shooter.startShooterMotors();
+        } else {
+            Shooter.stopShooterMotors();
         }
-        doShooterIntake();
-        Robot.motors.getIntake().stopMotor();
+
         if (controller.getYButton()) {
-            doFloorIntake();
-        }
-        Robot.motors.getLeftClimb().stopMotor();
-        Robot.motors.getRightClimb().stopMotor();
-        if (controller.getStartButton()) {
-            //extendArms();
-            Robot.motors.getLeftClimb().set(Constants.CLIMB_EXTENSION_SPEED);
-            Robot.motors.getRightClimb().set(Constants.CLIMB_EXTENSION_SPEED);
-        }
-        if (controller.getBackButton()) {
-            //retractArms();
-            Robot.motors.getLeftClimb().set(Constants.CLIMB_RETRACTION_SPEED);
-            Robot.motors.getRightClimb().set(Constants.CLIMB_RETRACTION_SPEED);
+            Intake.startFloorIntake();
+        } else {
+            Intake.stopFloorIntake();
         }
-    }
-
-    private static void doFloorIntake() {
-        Robot.motors.getIntake().set(.5);
-        //Robot.motors.getFeeder().set(.5);
-    }
-
-    private static void extendArms() {
-        Robot.getTeleopActionRunner().addActionToRun(new ExtendArms());
-        Robot.getTeleopActionRunner().removeActionsOfType(RetractArms.class);
-    }
-
-    private static void retractArms() {
-        Robot.getTeleopActionRunner().addActionToRun(new RetractArms());
-        Robot.getTeleopActionRunner().removeActionsOfType(ExtendArms.class);
-    }
 
-    private static void doShooterIntake() {
-        if (controller.getPOV() == 0) {
-            Robot.motors.getRightFlywheel().set(0.3);
-            Robot.motors.getLeftFlywheel().set(-0.3);
-        } else if (controller.getPOV() == 180) {
-            Robot.motors.getRightFlywheel().set(0);
-            Robot.motors.getLeftFlywheel().set(0);
+        if (controller.getStartButton()) {
+            Climber.manualExtendArms();
+        } else if (controller.getBackButton()) {
+            Climber.manualRetractArms();
+        } else {
+            Climber.stopClimbMotors();
         }
     }
-
-    private static void shootIntoSpeaker() {
-        Robot.getTeleopActionRunner().addActionToRun(new AutonShoot());
-    }
-
-    private static void dropIntoAmp() {
-        throw new UnsupportedOperationException("Unimplemented method 'dropIntoAmp'");
-    }
 }
diff --git a/src/main/java/frc/robot/QuickActions.java b/src/main/java/frc/robot/QuickActions.java
@@ -2,12 +2,12 @@
 
 public class QuickActions {
 
-    public static void setAll(double percent) {
+    public static void setDriveMotors(double percent) {
         Robot.motors.getDriveLeftParent().set(percent);
         Robot.motors.getDriveRightParent().set(percent);
     }
 
-    public static void stopAll() {
+    public static void stopDriveMotors() {
         Robot.motors.getDriveLeftParent().set(0.0);
         Robot.motors.getDriveRightParent().set(0.0);
     }

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -5,7 +5,6 @@
 package frc.robot;
 
 import com.kauailabs.navx.frc.AHRS;
-import com.revrobotics.CANSparkLowLevel.MotorType;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -211,7 +210,7 @@ public void teleopPeriodic() {
         // TODO: make slow mode
         double leftY = driverController.getLeftY();
         double rightY = driverController.getRightY();
-        QuickActions.stopAll();
+        QuickActions.stopDriveMotors();
         if (Math.abs(leftY) > Constants.CONTROLLER_DEADZONE) {
             driveLeftParent.set(leftY);
         }

diff --git a/src/main/java/frc/robot/RobotComponents/Climber.java b/src/main/java/frc/robot/RobotComponents/Climber.java
@@ -0,0 +1,34 @@
+package frc.robot.RobotComponents;
+
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.auton.ExtendArms;
+import frc.robot.auton.RetractArms;
+
+public class Climber {
+
+    private static void extendArms() {
+        Robot.getTeleopActionRunner().addActionToRun(new ExtendArms());
+        Robot.getTeleopActionRunner().removeActionsOfType(RetractArms.class);
+    }
+
+    private static void retractArms() {
+        Robot.getTeleopActionRunner().addActionToRun(new RetractArms());
+        Robot.getTeleopActionRunner().removeActionsOfType(ExtendArms.class);
+    }
+
+    public static void manualExtendArms() {
+        Robot.motors.getLeftClimb().set(Constants.CLIMB_EXTENSION_SPEED);
+        Robot.motors.getRightClimb().set(Constants.CLIMB_EXTENSION_SPEED);
+    }
+
+    public static void manualRetractArms() {
+        Robot.motors.getLeftClimb().set(Constants.CLIMB_RETRACTION_SPEED);
+        Robot.motors.getRightClimb().set(Constants.CLIMB_RETRACTION_SPEED);
+    }
+
+    public static void stopClimbMotors() {
+        Robot.motors.getLeftClimb().stopMotor();
+        Robot.motors.getRightClimb().stopMotor();
+    }
+}
diff --git a/src/main/java/frc/robot/RobotComponents/Intake.java b/src/main/java/frc/robot/RobotComponents/Intake.java
@@ -0,0 +1,14 @@
+package frc.robot.RobotComponents;
+
+import frc.robot.Robot;
+
+public class Intake {
+
+    public static void startFloorIntake() {
+        Robot.motors.getIntake().set(.5);
+    }
+
+    public static void stopFloorIntake() {
+        Robot.motors.getIntake().stopMotor();
+    }
+}
diff --git a/src/main/java/frc/robot/RobotComponents/Shooter.java b/src/main/java/frc/robot/RobotComponents/Shooter.java
@@ -0,0 +1,28 @@
+package frc.robot.RobotComponents;
+
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.auton.AutonShoot;
+import frc.robot.auton.AutonShooterFeed;
+
+public class Shooter {
+
+    public static void startShooterMotors() {
+        Robot.motors.getLeftFlywheel().setVelocity(Constants.SHOOTING_VELOCITY);
+        Robot.motors.getRightFlywheel().setVelocity(Constants.SHOOTING_VELOCITY);
+    }
+
+    public static void stopShooterMotors() {
+        Robot.motors.getLeftFlywheel().stopMotor();
+        Robot.motors.getRightFlywheel().stopMotor();
+    }
+
+    public static void startFeedMotors() {
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonShooterFeed.class);
+        Robot.getTeleopActionRunner().addActionToRun(new AutonShooterFeed());
+    }
+
+    public static void autoShootIntoSpeaker() {
+        Robot.getTeleopActionRunner().addActionToRun(new AutonShoot());
+    }
+}
diff --git a/src/main/java/frc/robot/auton/AutonMoveInches.java b/src/main/java/frc/robot/auton/AutonMoveInches.java
@@ -53,6 +53,6 @@ public AutonMoveInches(double distance) {
 
     @Override
     public void shutdown() {
-        QuickActions.stopAll();
+        QuickActions.stopDriveMotors();
     }
 }
diff --git a/src/main/java/frc/robot/auton/AutonRotate.java b/src/main/java/frc/robot/auton/AutonRotate.java
@@ -64,6 +64,6 @@ public void initiate() {
 
     @Override
     public void shutdown() {
-        QuickActions.stopAll();
+        QuickActions.stopDriveMotors();
     }
 }
diff --git a/src/main/java/frc/robot/auton/AutonShoot.java b/src/main/java/frc/robot/auton/AutonShoot.java
@@ -14,17 +14,15 @@ public boolean isDone() {
             Robot.motors.getFeeder().set(0.2);
             Robot.motors.getIntake().set(.2);
         }
-        if (Timer.getFPGATimestamp() >= revFinishedTime + 1.0) {
-            return true;
-        }
-        return false;
+
+        return Timer.getFPGATimestamp() >= revFinishedTime + 1.0;
     }
 
     @Override
     public void initiate() {
         Robot.motors.getLeftFlywheel().set(1.0);
         Robot.motors.getRightFlywheel().set(-1.0);
-        revFinishedTime = Timer.getFPGATimestamp() + Constants.REV_TIME;
+        revFinishedTime = Timer.getFPGATimestamp() + Constants.SHOOTER_FLYWHEEL_STARTUP_TIME;
     }
 
     @Override

diff --git a/src/main/java/frc/robot/auton/AutonShooterFeed.java b/src/main/java/frc/robot/auton/AutonShooterFeed.java
@@ -0,0 +1,26 @@
+package frc.robot.auton;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.Robot;
+
+public class AutonShooterFeed extends AutonAction {
+
+    private double revFinishedTime;
+
+    @Override
+    public boolean isDone() {
+        return Timer.getFPGATimestamp() > revFinishedTime;
+    }
+
+    @Override
+    public void initiate() {
+        Robot.motors.getFeeder().set(0.75);
+        revFinishedTime = Timer.getFPGATimestamp() + Constants.FEEDER_MOTOR_SHUTOFF_TIME;
+    }
+
+    @Override
+    public void shutdown() {
+        Robot.motors.getFeeder().stopMotor();
+    }
+}