Merge branch 'main' of https://github.com/eshsrobotics/swervebot

Author: RushilGhosh

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

@@ -161,11 +161,20 @@ public static class ArmConstants {
     // CAN IDs; this way, we can avoid any conflicts.
     public static final int LEFT_LIFT_CAN_ID = 20; 
     public static final int RIGHT_LIFT_CAN_ID = 21; 
-    public static final int LEFT_ALGAE_CAN_ID = 22; 
-    public static final int RIGHT_ALGAE_CAN_ID = 23; 
-    public static final int CORAL_INTAKE_CAN_ID = 24;
+    public static final int LEFT_CORAL_CAN_ID = 22; 
+    public static final int RIGHT_CORAL_CAN_ID = 23; 
+    public static final int ALGAE_INTAKE_CAN_ID = 24;
+    
+    /**
+     * The heights of each stage will be in terms of rotations, which need to updated with testing.
+     */
+    public static final double DEFAULT_HEIGHT = 0;
+    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 = 0;
+    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

@@ -1,10 +1,15 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.RelativeEncoder;
 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;
 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.util.sendable.SendableBuilder;
@@ -14,6 +19,8 @@
 
 public class ArmSubsystem extends SubsystemBase {
 
+    private InputSubsystem input; 
+
     /**
      * Controls the left motor of the 4 bar lift.
      */
@@ -26,46 +33,51 @@ public class ArmSubsystem extends SubsystemBase {
     private SparkMax RightLift;
 
     /**
-     * Left algae intake motor.
+     * Left coral intake motor.
      */
-    private SparkMax LeftAlgae;
+    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 RightAlgae;
+    private SparkMax RightCoral;
 
-    /**
-     * Coral intake motor
-     */
-    private SparkMax CoralIntake;
 
-    public ArmSubsystem() {
+    private RelativeEncoder leftLiftEncoder;
+
+    private SparkClosedLoopController pidController;
+
+    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);
-        LeftAlgae = new SparkMax(Constants.ArmConstants.LEFT_ALGAE_CAN_ID, MotorType.kBrushless);
-        RightAlgae = new SparkMax(Constants.ArmConstants.RIGHT_ALGAE_CAN_ID, MotorType.kBrushless);
-        CoralIntake = new SparkMax(Constants.ArmConstants.CORAL_INTAKE_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);
+
+        leftLiftEncoder = LeftLift.getEncoder();
+
+        pidController = LeftLift.getClosedLoopController();
 
         // The idle mode of the coral intake and lift motors are set to brake to prevent them from
         // continuing to move after the movement is halted. The algae config idle mode is set to 
         // coast due to the motors being in control of flywheels, meaning that the flywheels will
         // slowly spin to a halt rather than fully stopping immediately.
         SparkMaxConfig liftConfig = new SparkMaxConfig();
         liftConfig.idleMode(IdleMode.kBrake);
-        SparkMaxConfig algaeConfig = new SparkMaxConfig();
-        algaeConfig.idleMode(IdleMode.kCoast);
+        liftConfig.closedLoop.pid(0.1, 0, 0);
+        liftConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+
         SparkMaxConfig coralConfig = new SparkMaxConfig();
         coralConfig.idleMode(IdleMode.kBrake);
-        liftConfig.follow(Constants.ArmConstants.LEFT_LIFT_CAN_ID);
-        algaeConfig.follow(Constants.ArmConstants.LEFT_ALGAE_CAN_ID);
+
+        liftConfig.follow(Constants.ArmConstants.LEFT_LIFT_CAN_ID, true);
+        coralConfig.follow(Constants.ArmConstants.LEFT_CORAL_CAN_ID, true);
 
         LeftLift.configure(liftConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
         RightLift.configure(liftConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        LeftAlgae.configure(algaeConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        RightAlgae.configure(algaeConfig, com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
-        CoralIntake.configure(coralConfig, 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);
     }
 
     @Override
@@ -75,15 +87,17 @@ public void initSendable(SendableBuilder builder) {
         // as they are following them. As such, we only get the speed of the
         // 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("Algae", () -> LeftAlgae.get(), (intakeSpeed) -> LeftAlgae.set(intakeSpeed));
-        builder.addDoubleProperty("CoralIntake", () -> CoralIntake.get(), (intakeSpeed) -> CoralIntake.set(intakeSpeed));
+        builder.addDoubleProperty("Coral", () -> LeftCoral.get(), (intakeSpeed) -> LeftCoral.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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import com.revrobotics.RelativeEncoder;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.util.sendable.SendableBuilder;
@@ -70,11 +72,14 @@ public class InputSubsystem extends SubsystemBase {
      */
     private double lastCheckTimeSeconds;
 
+    int currentHeight;
+
     public InputSubsystem() {
         xboxController = null;
         mainJoystick = null;
         secondaryJoystick = null;
         lastCheckTimeSeconds = Timer.getFPGATimestamp() - JOYSTICK_POLLING_INTERVAL_SECONDS; // Force initial check
+        currentHeight = 0;
     }
 
     /**
@@ -176,11 +181,105 @@ public double getTurn() {
         return m_rotLimiter.calculate(turn);
     }
 
-    
-    // public double getLiftSpeed() {
-    //     double liftOut = (xboxController != null && xboxController.isConnected() && xboxController.getBButton() ? Constants.ArmConstants.LIFT_SPEED : 0);
-    //     return liftOut;
-    // } 
+    /**
+     * A function to return the number of desired rotations of the lift motors based on user input.
+     * - Stage 0 represents base height of the lift when idle, which is equal to the height of the trough.
+     * - Each additional stage (up to the top) represents the next level of the coral branch. 
+     *   - Lift stage 1 -> coral level 2
+     *   - Lift stage 2 ->  coral level 3
+     *   - Lift stage 3 -> coral level 4
+     * 
+     * - Each time the user presses one of the lift triggering buttons, 
+     *   the lift will automatically transition between the stages.
+     */
+    public double getDesiredPosition() {
+        double[] liftHeights = {
+            Constants.ArmConstants.DEFAULT_HEIGHT,
+            Constants.ArmConstants.LIFT_HEIGHT_1, 
+            Constants.ArmConstants.LIFT_HEIGHT_2,
+            Constants.ArmConstants.LIFT_HEIGHT_3};
+        
+
+        if(xboxController != null &&xboxController.isConnected() == true) {
+            if(xboxController.getBButtonPressed() && currentHeight <= liftHeights.length) {
+                currentHeight++;
+            } else if(xboxController.getAButtonPressed() && currentHeight >= 0) {
+                currentHeight--;
+            }
+        }
+
+        if(mainJoystick != null && mainJoystick.isConnected() && secondaryJoystick != null && secondaryJoystick.isConnected()) {
+            if(secondaryJoystick.getRawButtonPressed(6) && currentHeight <= liftHeights.length) {
+                currentHeight++;
+            } else if(secondaryJoystick.getRawButtonPressed(4) && currentHeight >= 0) {
+                currentHeight--;
+            }
+        } else if(mainJoystick != null && mainJoystick.isConnected()) {
+            if(mainJoystick.getRawButtonPressed(6) && currentHeight <= liftHeights.length) {
+                currentHeight++;
+            } 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;
+            }
+        }
+
+        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;
+    }
 
     /**
      * A function called every few seconds meant to detect when xbox controllers