Small edits, fully functioning arm subsystem

Author: RushilGhosh

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

@@ -172,7 +172,7 @@ public static class ArmConstants {
     public static final double LIFT_HEIGHT_2 = 2;
     public static final double LIFT_HEIGHT_3 = 3;
 
-    public static final double LIFT_SPEED = 0.05;
+    public static final double LIFT_SPEED = 0.1;
     public static final double CORAL_INTAKE_SPEED = 0.03;
   }
 

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

@@ -83,6 +83,6 @@ private void configureBindings() {
    */
   public Command getAutonomousCommand() {
     // An example command will be run in autonomous
-    return Autos.driveStraight(m_driveSubsystem, 3);
+    return Autos.driveStraight(m_driveSubsystem, m_armSubsystem);
   }
 }

src/main/java/frc/robot/commands/Autos.java

@@ -4,6 +4,8 @@
 
 package frc.robot.commands;
 
+import frc.robot.Constants;
+import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -18,15 +20,17 @@ public static Command exampleAuto(ExampleSubsystem subsystem) {
   }
 
   /** Drives for a certain number of seconds. */
-  public static Command driveStraight(DriveSubsystem drive, double seconds) {
+  public static Command driveStraight(DriveSubsystem drive, ArmSubsystem arm) {
     return new InstantCommand(() -> drive.drive(0.0, 0.5, 0.0))
-               .andThen(new WaitCommand(seconds))
-               .andThen(() -> drive.drive(0.0, 0.0, 0.0));
+               .andThen(new WaitCommand(1.5))
+               .andThen(() -> drive.drive(0.0, 0.0, 0.0))
+               .andThen(() -> arm.setArmSpeed(Constants.ArmConstants.LIFT_SPEED))
+               .andThen(new WaitCommand(0.5))
+               .andThen(() -> arm.setArmSpeed(0));
   }
 
   private Autos() {
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
-
 }

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

@@ -74,7 +74,7 @@ public ArmSubsystem(InputSubsystem inputSubsystem) {
         liftConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
 
         SparkMaxConfig followLiftConfig = new SparkMaxConfig();
-        followLiftConfig.follow(Constants.ArmConstants.LEFT_LIFT_CAN_ID).apply(liftConfig);
+        followLiftConfig.apply(liftConfig);
 
 
         SparkMaxConfig coralConfig = new SparkMaxConfig();
@@ -83,7 +83,6 @@ public ArmSubsystem(InputSubsystem inputSubsystem) {
         SparkMaxConfig followCoralConfig = new SparkMaxConfig();
         followCoralConfig.follow(Constants.ArmConstants.LEFT_CORAL_CAN_ID).apply(coralConfig);
 
-
         followLiftConfig.follow(Constants.ArmConstants.LEFT_LIFT_CAN_ID, true);
         followCoralConfig.follow(Constants.ArmConstants.LEFT_CORAL_CAN_ID, true);
 
@@ -123,7 +122,7 @@ public void periodic() {
        down) and +1.0 (full speed up.) 0 stops the arm from moving.
     */
     public void setArmSpeed(double speed) {
-        //armSpeed = speed;
+        armSpeed = speed;
         System.out.println("Moving arm at " + (speed * 100) + "%");
     }