Merge remote-tracking branch 'origin/dev-2' into feat/elevator-2

Fixed some merge conflicts

src/main/java/frc/robot/Commands/SequentialCommands.java

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.WristSubsystem;
+
+public class SequentialCommands extends SequentialCommandGroup {
+  /** Creates a new TestCommand. */
+  public SequentialCommands(WristSubsystem wristSubsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addCommands(
+        new WristCommand(wristSubsystem, 0),
+        new WristCommand(wristSubsystem, 20),
+        new WristCommand(wristSubsystem, 40));
+  }
+
+  // // Called when the command is initially scheduled.
+  // @Override
+  // public void initialize() {}
+
+  // // Called every time the scheduler runs while the command is scheduled.
+  // @Override
+  // public void execute() {}
+
+  // // Called once the command ends or is interrupted.
+  // @Override
+  // public void end(boolean interrupted) {}
+
+  // // Returns true when the command should end.
+  // @Override
+  // public boolean isFinished() {
+  //   return false;
+  // }
+}

src/main/java/frc/robot/Commands/WristCommand.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.WristSubsystem;
+
+public class WristCommand extends CommandBase {
+  /** Creates a new WristCommand. */
+  private final WristSubsystem wristSubsystem;
+
+  private double targetAngle;
+
+  public WristCommand(WristSubsystem wristSubsystem, double targetAngle) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.wristSubsystem = wristSubsystem;
+    this.targetAngle = targetAngle;
+
+    addRequirements(wristSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    // sets target angle
+    wristSubsystem.setTargetAngle(targetAngle);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return wristSubsystem.nearTargetAngle();
+  }
+}

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

@@ -2,30 +2,35 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-
 package frc.robot;
 
-
 @SuppressWarnings("java:S1118")
 /**
-* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
-* constants. This class should not be used for any other purpose. All constants should be declared
-* globally (i.e. public static). Do not put anything functional in this class.
-*
-* <p>It is advised to statically import this class (or one of its inner classes) wherever the
-* constants are needed, to reduce verbosity.
-*/
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
 public final class Constants {
-   public static class OperatorConstants {
-       public static final int kDriverControllerPort = 0;
-     }
-     public static class Elevator{
-       /**the gear ratio of the motor to the final gear revolutions */
-       public static final double GEAR_RATIO = 0.1008;
-       /**The number of ticks per motor revolution */
-       public static final double TICKS_PER_REVOLUTION = 2048;
-       /**The gear circumferance for distance */
-       public static final double GEAR_CIRCUMFERENCE = 1.432*Math.PI;
-
+  public static class OperatorConstants {
+    public static final int kDriverControllerPort = 0;
+  }
+
+  public static class Elevator {
+    /** the gear ratio of the motor to the final gear revolutions */
+    public static final double GEAR_RATIO = 0.1008;
+    /** The number of ticks per motor revolution */
+    public static final double TICKS_PER_REVOLUTION = 2048;
+    /** The gear circumferance for distance */
+    public static final double GEAR_CIRCUMFERENCE = 1.432 * Math.PI;
+  }
+  public final class Wrist {
+    public static final int DRIVER_CONTROLLER_PORT = 0;
+    public static final int WRIST_MOTOR_DEVICE_NUMBER = 16;
+    public static final double TICKS = 2048;
+    public static final double DEGREES = 360;
+    public static final double GEAR_RATIO = 0.061;
+  }
 }
-}

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

@@ -2,111 +2,94 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-
 package frc.robot;
 
-
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
-
 /**
-* The VM is configured to automatically run this class, and to call the functions corresponding to
-* each mode, as described in the TimedRobot documentation. If you change the name of this class or
-* the package after creating this project, you must also update the build.gradle file in the
-* project.
-*/
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
 public class Robot extends TimedRobot {
- private Command m_autonomousCommand;
-
-
- private RobotContainer m_robotContainer;
-
-
- /**
-  * This function is run when the robot is first started up and should be used for any
-  * initialization code.
-  */
- @Override
- public void robotInit() {
-   // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-   // autonomous chooser on the dashboard.
-   m_robotContainer = new RobotContainer();
- }
-
-
- /**
-  * This function is called every robot packet, no matter the mode. Use this for items like
-  * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
-  *
-  * <p>This runs after the mode specific periodic functions, but before LiveWindow and
-  * SmartDashboard integrated updating.
-  */
- @Override
- public void robotPeriodic() {
-   // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-   // commands, running already-scheduled commands, removing finished or interrupted commands,
-   // and running subsystem periodic() methods.  This must be called from the robot's periodic
-   // block in order for anything in the Command-based framework to work.
-   CommandScheduler.getInstance().run();
- }
-
-
- /** This function is called once each time the robot enters Disabled mode. */
- @Override
- public void disabledInit() {}
-
-
- @Override
- public void disabledPeriodic() {}
-
-
- /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
- @Override
- public void autonomousInit() {
-   m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
-
-   // schedule the autonomous command (example)
-   if (m_autonomousCommand != null) {
-     m_autonomousCommand.schedule();
-   }
- }
-
-
- /** This function is called periodically during autonomous. */
- @Override
- public void autonomousPeriodic() {}
-
-
- @Override
- public void teleopInit() {
-   // This makes sure that the autonomous stops running when
-   // teleop starts running. If you want the autonomous to
-   // continue until interrupted by another command, remove
-   // this line or comment it out.
-   if (m_autonomousCommand != null) {
-     m_autonomousCommand.cancel();
-   }
- }
-
-
- /** This function is called periodically during operator control. */
- @Override
- public void teleopPeriodic() {}
-
-
- @Override
- public void testInit() {
-   // Cancels all running commands at the start of test mode.
-   CommandScheduler.getInstance().cancelAll();
- }
-
-
- /** This function is called periodically during test mode. */
- @Override
- public void testPeriodic() {}
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
 }
-
-

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

@@ -16,7 +16,8 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
 
-  private ElevatorBaseCommand ElevatorBaseCommand = new ElevatorBaseCommand(elevatorSubsystem, 0);
+  private final ElevatorBaseCommand ElevatorBaseCommand =
+      new ElevatorBaseCommand(elevatorSubsystem, 0);
 
   private final CommandXboxController driverA = new CommandXboxController(0);
   private double targetHeight;
@@ -45,7 +46,7 @@ private void configureButtonBindings() {}
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
     return new InstantCommand();
+    // An ExampleCommand will run in autonomous
   }
 }

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

@@ -19,7 +19,7 @@ public class ElevatorSubsystem extends SubsystemBase {
   private TalonFX left_motor;
   /** leader */
   private TalonFX right_motor;
-  /** leader */
+  /** follower */
   private TalonFX wrist_motor;
 
   private PIDController pidController;
@@ -67,6 +67,7 @@ public ElevatorSubsystem() {
     // pidController.setTolerance(0.7,0.001);
     ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
     ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
+    ElevatorTab.addNumber("Current Height", () -> currentHeight);
     ElevatorTab.add(pidController);
 
     ElevatorTab.addNumber("Left Motor Speed", left_motor::getSelectedSensorVelocity);
@@ -95,7 +96,6 @@ public static double ticksToInches(double ticks) {
 
   public void setTargetHeight(double targetHeight) {
     this.targetHeight = targetHeight;
-    pidController.setSetpoint(targetHeight);
   }
 
   public boolean nearTargetHeight() {
@@ -109,7 +109,7 @@ public boolean nearTargetHeight() {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    motorPower = pidController.calculate(currentHeight);
+    motorPower = pidController.calculate(currentHeight, targetHeight);
     left_motor.set(
         TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.5, 0.5)));
     currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition());