Make code to move the arms (#24)

* Co-authored-by: BookSerpent <BookSerpent@users.noreply.github.com>

* declared left and right climb motors, and made it so we only call them "leftClimb" and "rightClimb" respectively

* making sure prettier is happy

* made the extention and retraction speed of the arm constants

* added explanations with units to the constants for arm extension

src/main/java/frc/robot/Constants.java

@@ -8,9 +8,24 @@ public class Constants {
     public static final int DRIVE_RIGHT_PARENT_ID = 11;
     public static final int DRIVE_RIGHT_CHILD_ID = 3;
 
+    /*CLIMB MOTOR IDS */
+
+    public static final int LEFT_CLIMB_ID = 31;
+    public static final int RIGHT_CLIMB_ID = 21;
+
     /*SHOOTER CONFIG*/
+    /**the amount of time we let the wheels rev up when shooting in seconds */
     public static final double REV_TIME = 1.0;
 
+    /*CLIMB CONFIG */
+    /**the time we give the arms to extend in seconds */
+    public static final double ARM_EXTENSION_TIME = 3;
+    /**the time we give the arms to retract in seconds */
+    public static final double ARM_RETRACTION_TIME = 3;
+    /**the % of max power we give to the arm motors while extending */
+    public static final double ARM_EXTENSION_SPEED = .25;
+    /**the % of max power we give to the arm motors while retracting */
+    public static final double ARM_RETRACTION_SPEED = .25;
     /*DRIVE TRAIN CONFIG */
     public static final double CONTROLLER_DEADZONE = 0.06;
 

src/main/java/frc/robot/InputtedCoDriverControls.java

@@ -2,6 +2,8 @@
 
 import edu.wpi.first.wpilibj.XboxController;
 import frc.robot.auton.AutonShoot;
+import frc.robot.auton.ExtendArms;
+import frc.robot.auton.RetractArms;
 
 public class InputtedCoDriverControls {
 
@@ -21,20 +23,20 @@ public static void onEveryFrame() {
             shootIntoSpeaker();
         }
         doShooterIntake();
-        if (controller.getBackButton()) {
-            // doClimb();
-        }
         if (controller.getStartButton()) {
-            // extendArms();
+            extendArms();
+        }
+        if (controller.getBackButton()) {
+            retractArms();
         }
     }
 
     private static void extendArms() {
-        throw new UnsupportedOperationException("Unimplemented method 'extendArms'");
+        Robot.getTeleopActionRunner().addActionToRun(new ExtendArms());
     }
 
     private static void retractArms() {
-        throw new UnsupportedOperationException("Unimplemented method 'doClimb'");
+        Robot.getTeleopActionRunner().addActionToRun(new RetractArms());
     }
 
     private static void doShooterIntake() {

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

@@ -42,6 +42,8 @@ public class Robot extends TimedRobot {
     MotorController leftFlywheel = MotorControllerFactory.create(6, MotorController.Type.SparkMax);
     MotorController rightFlywheel = MotorControllerFactory.create(7, MotorController.Type.SparkMax);
     MotorController feederMotor = MotorControllerFactory.create(9, MotorController.Type.SparkMax);
+    MotorController leftClimb = MotorControllerFactory.create(Constants.LEFT_CLIMB_ID, MotorController.Type.Talon);
+    MotorController rightClimb = MotorControllerFactory.create(Constants.RIGHT_CLIMB_ID, MotorController.Type.Talon);
     XboxController driverController = new XboxController(Constants.DRIVER_CONTROLLER_ID);
     XboxController coDriverController = new XboxController(1);
     static AHRS navX = new AHRS(SPI.Port.kMXP);
@@ -95,7 +97,9 @@ public void robotInit() {
                 .driveRightChild(driveRightChild)
                 .feeder(feederMotor)
                 .leftFlywheel(leftFlywheel)
-                .rightFlywheel(rightFlywheel);
+                .rightFlywheel(rightFlywheel)
+                .leftClimb(leftClimb)
+                .rightClimb(rightClimb);
         getGyroscope().reset();
         System.out.println(Constants.APRIL_TAG_CONFIDENCE_FRAMES);
         SmartDashboard.putNumber("rotationGainsP", Constants.ROTATION_GAINS.P);

src/main/java/frc/robot/RobotMotors.java

@@ -92,13 +92,13 @@ public RobotMotors intakeMotor(MotorController intakeMotor) {
         return this;
     }
 
-    public RobotMotors leftClimbMotor(MotorController leftClimbMotor, String arg) {
-        this.leftClimb = leftClimbMotor;
+    public RobotMotors leftClimb(MotorController leftClimb) {
+        this.leftClimb = leftClimb;
         return this;
     }
 
-    public RobotMotors rightClimbMotor(MotorController rightClimbMotor) {
-        this.rightClimb = rightClimbMotor;
+    public RobotMotors rightClimb(MotorController rightClimb) {
+        this.rightClimb = rightClimb;
         return this;
     }
 }

src/main/java/frc/robot/auton/ExtendArms.java

@@ -0,0 +1,27 @@
+package frc.robot.auton;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.Robot;
+
+public class ExtendArms extends AutonAction {
+
+    private double extensionTime;
+
+    @Override
+    public boolean isDone() {
+        if (Timer.getFPGATimestamp() >= extensionTime) {
+            Robot.motors.getLeftClimb().set(0);
+            Robot.motors.getRightClimb().set(0);
+            return true;
+        }
+        return false;
+    }
+
+    @Override
+    public void initiate() {
+        Robot.motors.getLeftClimb().set(Constants.ARM_EXTENSION_SPEED);
+        Robot.motors.getRightClimb().set(Constants.ARM_EXTENSION_SPEED);
+        extensionTime = Timer.getFPGATimestamp() + Constants.ARM_EXTENSION_TIME;
+    }
+}

src/main/java/frc/robot/auton/RetractArms.java

@@ -0,0 +1,27 @@
+package frc.robot.auton;
+
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants;
+import frc.robot.Robot;
+
+public class RetractArms extends AutonAction {
+
+    private double extensionTime;
+
+    @Override
+    public boolean isDone() {
+        if (Timer.getFPGATimestamp() >= extensionTime) {
+            Robot.motors.getLeftClimb().set(0);
+            Robot.motors.getRightClimb().set(0);
+            return true;
+        }
+        return false;
+    }
+
+    @Override
+    public void initiate() {
+        Robot.motors.getLeftClimb().set(Constants.ARM_RETRACTION_SPEED);
+        Robot.motors.getRightClimb().set(Constants.ARM_RETRACTION_SPEED);
+        extensionTime = Timer.getFPGATimestamp() + Constants.ARM_RETRACTION_TIME;
+    }
+}