Various improvements (#38)

* adjust ID's and shooter config * Improve controls * Add hood adjuster motor * Fix CME and add debug to test if working * Uncomment intake code * uncomment intake code (part 2) * idk
WTMCRobotics · Mar 7, 2024 · 529a06f · 529a06f
1 parent ee10acf
commit 529a06f
Show file tree

Hide file tree

Showing 10 changed files with 149 additions and 34 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -10,8 +10,9 @@ public class Constants {
     public static final int SHOOTER_LEFT_FLYWHEEL_ID = 9;
     public static final int SHOOTER_RIGHT_FLYWHEEL_ID = 7;
     public static final int SHOOTER_FEEDER_ID = 10;
+    public static final int SHOOTER_HOOD_ADJUSTERER_ID = 13;
 
-    public static final int INTAKE_ID = 6101;
+    public static final int INTAKE_ID = 12;
 
     /*CLIMB MOTOR IDS */
 
@@ -24,10 +25,18 @@ public class Constants {
     public static final double SHOOTER_LEFT_FLYWHEEL_SPEED = 1.0;
     public static final double SHOOTER_RIGHT_FLYWHEEL_SPEED = 1.0;
     /*The amount of seconds that the feeder should wait before turning off after shooting */
-
     public static final double FEEDER_MOTOR_SHUTOFF_TIME = 1.5;
 
+    //TODO configure speed based on thingys
+    /**The motor percentOutput that the shooter hood will move at */
+    public static final double SHOOTER_HOOD_ADJUSTMENT_SPEED = 0.6;
+    /**The amount of rotations of accuracy that the the hood needs to be to be considered in place */
+    public static final double SHOOTER_HOOD_ADJUSTMENT_ERROR_ROTATIONS = 0.5;
+
     public static final double SHOOTING_VELOCITY_RPM = 4000;
+    //TODO configure these correctly
+    public static final double HOOD_INTAKE_POSITION_ROTATIONS = 3.1415926;
+    public static final double HOOD_SHOOTING_POSITION_ROTATIONS = 2.7;
 
     /*CLIMB CONFIG */
     public static final double CLIMB_EXTENDED_ENCODER_POSITION = 5;
@@ -38,7 +47,7 @@ public class Constants {
     public static final double CLIMB_RETRACTION_SPEED = -.25;
 
     /*INTAKE CONFIG */
-    public static final double INTAKE_SPEED = 0;
+    public static final double INTAKE_SPEED = 1.00;
 
     public static final int INTAKE_BEAM_BREAK_ID = 9;
 

diff --git a/src/main/java/frc/robot/InputtedCoDriverControls.java b/src/main/java/frc/robot/InputtedCoDriverControls.java
@@ -21,22 +21,22 @@ public static void onTeleopInit() {
     static boolean hasGoneUp = false;
 
     public static void onEveryFrame() {
-        if (controller.getYButtonPressed()) {
+        if (controller.getXButtonPressed()) {
             Shooter.startFeedMotors();
         }
-        if (controller.getXButtonPressed()) {
+        if (controller.getLeftBumperPressed()) {
             Shooter.startShooterMotors();
-        } else if (controller.getBButtonPressed()) {
+        } else if (controller.getYButtonPressed()) {
             Shooter.autoShootIntoSpeaker();
         }
-        if (controller.getXButtonReleased()) {
+        if (controller.getLeftBumperReleased()) {
             Shooter.stopShooterMotors();
         }
 
-        if (controller.getLeftBumper()) {
+        if (controller.getBButton()) {
             Intake.startFloorIntake();
         }
-        if (controller.getLeftBumperReleased()) {
+        if (controller.getBButtonReleased()) {
             Intake.stopFloorIntake();
         }
 
@@ -49,6 +49,11 @@ public static void onEveryFrame() {
             Climber.stopClimbMotors();
         }
 
+        if (Double.compare(controller.getLeftTriggerAxis(), 0.90) == 0) {
+            Shooter.moveHoodToIntakePosition();
+        } else if (controller.getLeftTriggerAxis() >= 0.29 && controller.getLeftTriggerAxis() <= 0.71) {
+            Shooter.moveHoodToShootingPosition();
+        }
         if (controller.getAButtonPressed()) {
             Intake.backtrack();
             Shooter.backtrack();

diff --git a/src/main/java/frc/robot/InputtedDriverControls.java b/src/main/java/frc/robot/InputtedDriverControls.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.robotcomponents.DriveTrain;
+import frc.robot.robotcomponents.Intake;
 
 public class InputtedDriverControls {
 
@@ -22,6 +23,12 @@ public static void onEveryFrame() {
         if (controller.getXButtonPressed()) {
             stopAtShootingDistance();
         }
+        if (controller.getLeftBumper()) {
+            Intake.startFloorIntake();
+        }
+        if (controller.getLeftBumperReleased()) {
+            Intake.stopFloorIntake();
+        }
     }
 
     private static void stopAtShootingDistance() {}

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -71,8 +71,12 @@ public class Robot extends TimedRobot {
         Constants.RIGHT_CLIMB_ID,
         MotorController.Type.SparkMaxBrushed
     );
-
-    // MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushless);
+    MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushed);
+    MotorController hoodAdjuster = null;
+    // = MotorControllerFactory.create(
+    //     Constants.SHOOTER_HOOD_ADJUSTERER_ID,
+    //     MotorController.Type.SparkMaxBrushed
+    // );
 
     XboxController driverController = new XboxController(Constants.DRIVER_CONTROLLER_ID);
     XboxController coDriverController = new XboxController(Constants.CODRIVER_CONTROLLER_ID);
@@ -108,10 +112,10 @@ public void robotInit() {
         autonRouteChooser.addOption("backup turn backup", AutonRoutes.BACKUP_TURN_BACKUP);
         autonRouteChooser.addOption("shoot and back up", AutonRoutes.SHOOT_AND_BACK_UP);
 
-        autonRouteChooser.addOption("Test old rotation PID", AutonRoutes.TEST_ROTATION);
-        autonRouteChooser.addOption("Test new rotation PID", AutonRoutes.TEST_ROTATION_WITH_PID_COMMAND);
-        autonRouteChooser.addOption("Test SmartMotion movement", AutonRoutes.TEST_SMART_MOTION_MOVEMENT);
-        autonRouteChooser.addOption("Test Manual PID movement", AutonRoutes.TEST_PID_MOVEMENT);
+        // autonRouteChooser.addOption("Test old rotation PID", AutonRoutes.TEST_ROTATION);
+        // autonRouteChooser.addOption("Test new rotation PID", AutonRoutes.TEST_ROTATION_WITH_PID_COMMAND);
+        // autonRouteChooser.addOption("Test SmartMotion movement", AutonRoutes.TEST_SMART_MOTION_MOVEMENT);
+        // autonRouteChooser.addOption("Test Manual PID movement", AutonRoutes.TEST_PID_MOVEMENT);
 
         SmartDashboard.putData("Auton Routes", autonRouteChooser);
 
@@ -152,8 +156,9 @@ public void robotInit() {
                 .leftFlywheel(leftFlywheel)
                 .rightFlywheel(rightFlywheel)
                 .leftClimb(leftClimb)
-                .rightClimb(rightClimb);
-        // .intake(intake);
+                .rightClimb(rightClimb)
+                .intake(intake)
+                .hoodAdjuster(hoodAdjuster);
         getGyroscope().reset();
         System.out.println(Constants.APRIL_TAG_CONFIDENCE_FRAMES);
         SmartDashboard.putNumber("rotationGainsP", Constants.ROTATION_GAINS.P);

diff --git a/src/main/java/frc/robot/RobotComponents/Intake.java b/src/main/java/frc/robot/RobotComponents/Intake.java
@@ -1,17 +1,27 @@
 package frc.robot.robotcomponents;
 
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.auton.AutonIntake;
+
 public class Intake {
 
+    public static void autoIntake() {
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonIntake.class);
+        Robot.getTeleopActionRunner().addActionToRun(new AutonIntake());
+    }
+
     public static void startFloorIntake() {
-        // Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
+        Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
     }
 
     /* This will spin the motor backwards in an attempt to eject a stuck note*/
     public static void backtrack() {
-        // Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
+        Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
     }
 
     public static void stopFloorIntake() {
-        // Robot.motors.getIntake().stopMotor();
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonIntake.class);
+        Robot.motors.getIntake().stopMotor();
     }
 }
diff --git a/src/main/java/frc/robot/RobotComponents/Shooter.java b/src/main/java/frc/robot/RobotComponents/Shooter.java
@@ -2,6 +2,8 @@
 
 import frc.robot.Constants;
 import frc.robot.Robot;
+import frc.robot.auton.AutonMoveHoodToPosition;
+import frc.robot.auton.AutonMoveHoodToPosition.HoodPosition;
 import frc.robot.auton.AutonShoot;
 import frc.robot.auton.AutonShooterFeed;
 
@@ -35,4 +37,14 @@ public static void autoShootIntoSpeaker() {
         Robot.getTeleopActionRunner().removeActionsOfType(AutonShoot.class);
         Robot.getTeleopActionRunner().addActionToRun(new AutonShoot());
     }
+
+    public static void moveHoodToIntakePosition() {
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonMoveHoodToPosition.class);
+        Robot.getTeleopActionRunner().addActionToRun(new AutonMoveHoodToPosition(HoodPosition.INTAKING));
+    }
+
+    public static void moveHoodToShootingPosition() {
+        Robot.getTeleopActionRunner().removeActionsOfType(AutonMoveHoodToPosition.class);
+        Robot.getTeleopActionRunner().addActionToRun(new AutonMoveHoodToPosition(HoodPosition.SHOOTING_DEFAULT));
+    }
 }
diff --git a/src/main/java/frc/robot/RobotMotors.java b/src/main/java/frc/robot/RobotMotors.java
@@ -4,16 +4,21 @@
 
 public class RobotMotors {
 
-    MotorController driveLeftParent;
-    MotorController driveLeftChild;
-    MotorController driveRightParent;
-    MotorController driveRightChild;
-    MotorController feeder;
-    MotorController leftFlywheel;
-    MotorController rightFlywheel;
-    MotorController leftClimb;
-    MotorController rightClimb;
-    MotorController intake;
+    private MotorController driveLeftChild;
+    private MotorController driveLeftParent;
+    private MotorController driveRightParent;
+    private MotorController driveRightChild;
+    private MotorController feeder;
+    private MotorController leftFlywheel;
+    private MotorController rightFlywheel;
+    private MotorController leftClimb;
+    private MotorController rightClimb;
+    private MotorController intake;
+    private MotorController hoodAdjuster;
+
+    public MotorController getHoodAdjuster() {
+        return hoodAdjuster;
+    }
 
     public MotorController getIntake() {
         return intake;
@@ -106,4 +111,9 @@ public RobotMotors rightClimb(MotorController rightClimb) {
         this.rightClimb = rightClimb;
         return this;
     }
+
+    public RobotMotors hoodAdjuster(MotorController hoodAdjuster) {
+        this.hoodAdjuster = hoodAdjuster;
+        return this;
+    }
 }
diff --git a/src/main/java/frc/robot/auton/AutonMoveHoodToPosition.java b/src/main/java/frc/robot/auton/AutonMoveHoodToPosition.java
@@ -0,0 +1,53 @@
+package frc.robot.auton;
+
+import frc.robot.Constants;
+import frc.robot.Robot;
+
+public class AutonMoveHoodToPosition extends AutonAction {
+
+    public HoodPosition targetPosition;
+
+    public AutonMoveHoodToPosition(HoodPosition targetPosition) {
+        this.targetPosition = targetPosition;
+    }
+
+    @Override
+    public boolean isDone() {
+        return true;
+        // double error = Math.abs(
+        //     Robot.motors.getHoodAdjuster().getEncoderPosition() - targetPosition.getTargetRotations()
+        // );
+        // return error < Constants.SHOOTER_HOOD_ADJUSTMENT_ERROR_ROTATIONS;
+    }
+
+    @Override
+    public void initiate() {
+        // int multiplier = 1;
+        // if (Robot.motors.getHoodAdjuster().getEncoderPosition() > targetPosition.getTargetRotations()) {
+        //     multiplier = -1;
+        // } else {
+        //     multiplier = 1;
+        // }
+        // Robot.motors.getHoodAdjuster().set(Constants.SHOOTER_HOOD_ADJUSTMENT_SPEED * multiplier);
+    }
+
+    @Override
+    public void shutdown() {
+        // Robot.motors.getHoodAdjuster().stopMotor();
+    }
+
+    public enum HoodPosition {
+        INTAKING(Constants.HOOD_INTAKE_POSITION_ROTATIONS),
+        SHOOTING_DEFAULT(Constants.HOOD_SHOOTING_POSITION_ROTATIONS);
+
+        private double encoderRotations;
+
+        private HoodPosition(double encoderRotation) {
+            this.encoderRotations = encoderRotation;
+        }
+
+        public double getTargetRotations() {
+            return encoderRotations;
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/auton/AutonShoot.java b/src/main/java/frc/robot/auton/AutonShoot.java
@@ -12,7 +12,7 @@ public class AutonShoot extends AutonAction {
     public boolean isDone() {
         if (Timer.getFPGATimestamp() >= revFinishedTime) {
             Robot.motors.getFeeder().set(0.2);
-            //Robot.motors.getIntake().set(.2);
+            Robot.motors.getIntake().set(0.2);
         }
 
         return Timer.getFPGATimestamp() >= revFinishedTime + 1.0;

diff --git a/src/main/java/frc/robot/auton/ParallelActionRunner.java b/src/main/java/frc/robot/auton/ParallelActionRunner.java
@@ -24,15 +24,19 @@ public void onEveryFrame() {
         }
     }
 
-    // This method is important if you have code that could add two of the same action with opposite goals.
-    // That would mean they would fight each other and never finish, which is bad news bears if you ask me.
+    /**This method is important if you have code that could add two of the same action with opposite goals.
+     *That would mean they would fight each other and never finish, which is bad news bears if you ask me.
+     * @param clazzez A singular class or a array of classes*/
+
     public void removeActionsOfType(Class<?>... clazzez) {
         Iterator<AutonAction> it = actions.iterator();
         while (it.hasNext()) {
             AutonAction auction = it.next();
             for (Class<?> clazz : clazzez) {
                 if (auction.getClass().equals(clazz)) {
-                    actions.remove(auction);
+                    System.out.println("Removing " + clazz.getName() + " from parallel action queue");
+                    auction.shutdown();
+                    it.remove();
                 }
             }
         }