Updated ArmSubsystem control scheme, finished periodic.

Added 2 new functions to InputSubsystem:
- Added a control scheme for the lift to move the
  motors while holding down button (most likely
  what we'll use at the competition)
- Made a function for the coral intake

Finished implementing the PID controller and state
machine control scheme. Also updated the motor
configurations based on recent changes to the
robot design.

* All values for the ArmSubsystem and DriveSubsystem
movement need to be tested.

Author: FRC1

README.md

@@ -5,5 +5,4 @@ Code for the ESHS P.O.T.A.T.O.E.S. swerve drive test platform.
 
 ## To-do list
 1. Temporarily disable feedForward mechanism (example code) to see what PID controller does, and vice versa. Test to see if we like how the feedForward mechanism works with the PID controller
-2. Socialize with multiple teams at competitions in order to learn more about robotics in general (buisness cards included)
-3. Measure the height of each coral branch and associate number of rotations to each height. Then set each of those values to the corresponding variables in Constants.java.
+2. Socialize with multiple teams at competitions in order to learn more about robotics in general (buisness cards included)

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

@@ -172,6 +172,9 @@ public static class ArmConstants {
     public static final double LIFT_HEIGHT_1 = 1;
     public static final double LIFT_HEIGHT_2 = 2;
     public static final double LIFT_HEIGHT_3 = 3;
+
+    public static final double LIFT_SPEED = 1.0;
+    public static final double CORAL_INTAKE_SPEED = 0.3;
   }
 
 }

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

@@ -9,6 +9,7 @@
 import frc.robot.Constants.DriveConstants.DriveType;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ExampleCommand;
+import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.subsystems.InputSubsystem;
@@ -31,6 +32,7 @@ public class RobotContainer {
   private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
   private final InputSubsystem m_inputSubsystem = new InputSubsystem();
   private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(m_inputSubsystem, DriveType.DIFFERENTIAL_DRIVE);
+  private final ArmSubsystem m_armSubsystem = new ArmSubsystem(m_inputSubsystem);
 
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final CommandXboxController m_driverController = new CommandXboxController(

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

@@ -4,6 +4,7 @@
 import com.revrobotics.servohub.ServoHub.ResetMode;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.config.ClosedLoopConfig;
@@ -18,6 +19,8 @@
 
 public class ArmSubsystem extends SubsystemBase {
 
+    private InputSubsystem input; 
+
     /**
      * Controls the left motor of the 4 bar lift.
      */
@@ -30,31 +33,27 @@ public class ArmSubsystem extends SubsystemBase {
     private SparkMax RightLift;
 
     /**
-     * Left algae intake motor.
+     * Left coral intake motor.
      */
     private SparkMax LeftCoral;
 
     /**
-     * Right algae intake motor. Similar to the lift motors,
-     * this will be a follower of the left algae intake motor.
+     * Right coral intake motor. Similar to the lift motors,
+     * this will be a follower of the left coral intake motor.
      */
     private SparkMax RightCoral;
 
-    /**
-     * Coral intake motor
-     */
-    private SparkMax AlgaeIntake;
 
     private RelativeEncoder leftLiftEncoder;
 
     private SparkClosedLoopController pidController;
 
-    public ArmSubsystem() {
+    public ArmSubsystem(InputSubsystem inputSubsystem) {
+        input = inputSubsystem;
         LeftLift = new SparkMax(Constants.ArmConstants.LEFT_LIFT_CAN_ID, MotorType.kBrushless);
         RightLift = new SparkMax(Constants.ArmConstants.RIGHT_LIFT_CAN_ID, MotorType.kBrushless);
         LeftCoral = new SparkMax(Constants.ArmConstants.LEFT_CORAL_CAN_ID, MotorType.kBrushless);
         RightCoral = new SparkMax(Constants.ArmConstants.RIGHT_CORAL_CAN_ID, MotorType.kBrushless);
-        AlgaeIntake = new SparkMax(Constants.ArmConstants.ALGAE_INTAKE_CAN_ID, MotorType.kBrushless);
 
         leftLiftEncoder = LeftLift.getEncoder();
 
@@ -69,11 +68,8 @@ public ArmSubsystem() {
         liftConfig.closedLoop.pid(0.1, 0, 0);
         liftConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
-        SparkMaxConfig algaeConfig = new SparkMaxConfig();
-        algaeConfig.idleMode(IdleMode.kCoast);
-
         SparkMaxConfig coralConfig = new SparkMaxConfig();
-        coralConfig.idleMode(IdleMode.kCoast);
+        coralConfig.idleMode(IdleMode.kBrake);
 
         liftConfig.follow(Constants.ArmConstants.LEFT_LIFT_CAN_ID, true);
         coralConfig.follow(Constants.ArmConstants.LEFT_CORAL_CAN_ID, true);
@@ -82,7 +78,6 @@ public ArmSubsystem() {
         RightLift.configure(liftConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
         LeftCoral.configure(coralConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
         RightCoral.configure(coralConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        AlgaeIntake.configure(algaeConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
     }
 
     @Override
@@ -93,14 +88,16 @@ public void initSendable(SendableBuilder builder) {
         // leader, the LeftMotor. The right motor will be spinning in the opposite direction though.
         builder.addDoubleProperty("Lift", () -> LeftLift.get(), (armSpeed) -> LeftLift.set(armSpeed));
         builder.addDoubleProperty("Coral", () -> LeftCoral.get(), (intakeSpeed) -> LeftCoral.set(intakeSpeed));
-        builder.addDoubleProperty("AlgaeIntake", () -> AlgaeIntake.get(), (intakeSpeed) -> AlgaeIntake.set(intakeSpeed));
         initSendable(builder);
     }
 
     @Override
     public void periodic() {
-        
-        
+        //pidController.setReference(input.getDesiredPosition(), ControlType.kPosition); --> possible other control scheme
+        LeftLift.set(input.getArmMovement());
+        if (input.isCoralIntakeActivated()) {
+            LeftCoral.set(Constants.ArmConstants.CORAL_INTAKE_SPEED);
+        }
     }
 
 }

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

@@ -200,32 +200,85 @@ public double getDesiredPosition() {
             Constants.ArmConstants.LIFT_HEIGHT_3};
 
 
-
         if(xboxController != null &&xboxController.isConnected() == true) {
-            if(xboxController.getBButtonPressed() && currentHeight <= 3) {
+            if(xboxController.getBButtonPressed() && currentHeight <= liftHeights.length) {
                 currentHeight++;
-            } else if(xboxController.getXButtonPressed() && currentHeight >=0) {
+            } else if(xboxController.getAButtonPressed() && currentHeight >= 0) {
                 currentHeight--;
             }
         }
 
         if(mainJoystick != null && mainJoystick.isConnected() && secondaryJoystick != null && secondaryJoystick.isConnected()) {
-            if(secondaryJoystick.getRawButtonPressed(4) && currentHeight <= 3) {
+            if(secondaryJoystick.getRawButtonPressed(6) && currentHeight <= liftHeights.length) {
                 currentHeight++;
-            } else if(secondaryJoystick.getRawButtonPressed(3) && currentHeight >=0) {
+            } else if(secondaryJoystick.getRawButtonPressed(4) && currentHeight >= 0) {
                 currentHeight--;
             }
         } else if(mainJoystick != null && mainJoystick.isConnected()) {
-            if(mainJoystick.getRawButtonPressed(4) && currentHeight <= 3) {
+            if(mainJoystick.getRawButtonPressed(6) && currentHeight <= liftHeights.length) {
                 currentHeight++;
-            } else if(mainJoystick.getRawButtonPressed(3) && currentHeight >=0) {
+            } else if(mainJoystick.getRawButtonPressed(4) && currentHeight >= 0) {
                 currentHeight--;
             }
         }
 
+        return liftHeights[currentHeight];
+    }
 
+    /**
+     * This is a separte method of controlling the lift of the ArmSubsystem.
+     * It will move the lift at a constant speed as long as the player is holding down the desired movement button.
+     * The speed of the lift is dependent on the constant LIFT_SPEED in Constants.java.
+     */
+    public double getArmMovement() {
+        if(xboxController != null &&xboxController.isConnected() == true) {
+            if(xboxController.getPOV() > -10 && xboxController.getPOV() < 10) {
+                return Constants.ArmConstants.LIFT_SPEED;
+            } else if (xboxController.getPOV() > 170 && xboxController.getPOV() < 190) {
+                return Constants.ArmConstants.LIFT_SPEED * -1;
+            }
+        }
 
-        return liftHeights[currentHeight];
+        if(mainJoystick != null && mainJoystick.isConnected() && secondaryJoystick != null && secondaryJoystick.isConnected()) {
+            if(secondaryJoystick.getRawButtonPressed(5)) {
+                return Constants.ArmConstants.LIFT_SPEED;
+            } else if(secondaryJoystick.getRawButtonPressed(3)) {
+                return Constants.ArmConstants.LIFT_SPEED * -1;
+            }
+        } else if(mainJoystick != null && mainJoystick.isConnected()) {
+            if(mainJoystick.getRawButtonPressed(5)) {
+                return Constants.ArmConstants.LIFT_SPEED;
+            } else if(mainJoystick.getRawButtonPressed(3)) {
+                return Constants.ArmConstants.LIFT_SPEED * -1;
+            }
+        }
+
+        return 0;
+    }
+
+
+    /**
+     * Returns whether or not the coral flywheels should be moving based on whether or not the player
+     * is pressing the right trigger/right bumper button.
+     */
+    public boolean isCoralIntakeActivated() {
+        if(xboxController != null &&xboxController.isConnected() == true) {
+            if(xboxController.getRightBumperButton()) {
+                return true;
+            }
+        }
+
+        if(mainJoystick != null && mainJoystick.isConnected() && secondaryJoystick != null && secondaryJoystick.isConnected()) {
+            if(secondaryJoystick.getTrigger()) {
+                return true;
+            } 
+        } else if(mainJoystick != null && mainJoystick.isConnected()) {
+            if(mainJoystick.getTrigger()) {
+                return true;      
+            }
+        }
+
+        return false;
     }
 
     /**