Aishah/sysid

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

@@ -6,6 +6,7 @@
 
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.Units;
 
 
 /**
@@ -50,15 +51,19 @@ public static class ElevatorConstants {
     public static final int COUNTS_PER_REV = 42; // may need to be updated
 
     public static final double INITIAL_POSITION = 0;
-    public static final double METERS_PER_ROTATION = 0.0; 
+    public static final double METERS_PER_ROTATION = Units.Meters.convertFrom(1.751 * Math.PI/5, Units.Inches); // temp
     public static final double MAX_SPEED = 0.0; 
     public static final double SPEED_LOWER_LIMIT = 0.0;
     public static final double SPEED_UPPER_LIMIT = 0.0;
+
+    public static final double POSITION_LOWER_LIMIT = 0.01;
+    public static final double POSITION_UPPER_LIMIT = 0.71;
+
     public static final int MOTOR_STATOR_LIMIT = 60; // needs to be tuned
 
     // not tuned
     public static final double kS = 0;
-    public static final double kG = 0;
+    public static final double kG = 0.2;
     public static final double kV = 12/MAX_SPEED; // may need to be updated
     public static final double kA = 0;
 
@@ -69,6 +74,7 @@ public static class ElevatorConstants {
     public static final double kMaxV = 0;
     public static final double kMaxA = 0; 
 
+    public static final double MANUAL_MOVE_MOTOR_SPEED = 2.0;
   }
 
   public static class ArmConstants {
@@ -78,7 +84,7 @@ public static class ArmConstants {
 
     public static final double INITIAL_POSITION = 0; // facing down
     public static final double POSITION_OFFSET = Math.PI/2; // difference between pid 0 (horizontal) and our 0 (down)
-    public static final double DEGREES_PER_ROTATION = 0.0; 
+    public static final double DEGREES_PER_ROTATION = 1;  // temp
     public static final double MAX_SPEED = 0.0; 
     public static final double SPEED_LOWER_LIMIT = 0.0;
     public static final double SPEED_UPPER_LIMIT = 0.0;
@@ -95,6 +101,8 @@ public static class ArmConstants {
 
     public static final double kMaxV = 0;
     public static final double kMaxA = 0; 
+
+    public static final double MANUAL_MOVE_MOTOR_SPEED = 3.0;
   }
 
   public static class CollarConstants {
@@ -107,6 +115,8 @@ public static class CollarConstants {
     public static final double MAX_SPEED = 0.0; 
     public static final double SPEED_LOWER_LIMIT = 0.0;
     public static final double SPEED_UPPER_LIMIT = 0.0;
+
+    public static final double OUTTAKE_MOTOR_SPEED = 4.0;
   }
 
   public static class RampConstants {

src/main/java/frc/robot/Controls.java

@@ -7,7 +7,7 @@
 public class Controls {
 
     public static JoystickVals adjustInputs(double x, double y, boolean slowmode) {
-        return adjustSlowmode(inputShape(x, y), slowmode);
+        return adjustSlowmode(inputShape(x, y), slowmode); 
     }
 
 

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

@@ -43,25 +43,29 @@
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.Autos;
 import frc.robot.commands.RotateToFaceReefCommand;
-import frc.robot.generated.TunerConstants;
+import frc.robot.generated.TunerConstantsDynamene;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
 import frc.robot.subsystems.collar.CollarSubsystem;
+import frc.robot.subsystems.lifter.ArmSubsystem;
+import frc.robot.subsystems.lifter.ElevatorIOHardware;
+import frc.robot.subsystems.lifter.ElevatorSubsystem;
 import frc.robot.subsystems.lifter.LifterCommandFactory;
 import frc.robot.subsystems.ramp.RampSubsystem;
 import frc.robot.commands.AutoAlignCommand;
 
 
+
 public class RobotContainer {
 
   record JoystickVals(double x, double y) { }
 
   private RobotState currentState = RobotState.INTAKE;
   private RobotState nextState = RobotState.INTAKE;
 
-  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12VoltsMps desired top speed *0.3 for pid tuning 9/15
+  private double MaxSpeed = TunerConstantsDynamene.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12VoltsMps desired top speed *0.3 for pid tuning 9/15
   private double MaxAngularRate = 1.5 * Math.PI; // 3/4 of a rotation per second max angular velocity
 
   /* Setting up bindings for necessary control of the swerve drive platform */
@@ -70,14 +74,17 @@ record JoystickVals(double x, double y) { }
   private final CommandXboxController testJoystick = new CommandXboxController(OperatorConstants.kTestControllerPort); // test joystick
 
 
-  private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); // My drivetrain
+  private final CommandSwerveDrivetrain drivetrain = TunerConstantsDynamene.createDrivetrain(); // My drivetrain
   private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); 
   private final VisionSubsystem m_vision = new VisionSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); 
   private final CollarSubsystem m_collar = new CollarSubsystem();
   private final RampSubsystem m_ramp = new RampSubsystem();
 
   private final LifterCommandFactory m_lifter = new LifterCommandFactory();
+  private final ElevatorSubsystem m_elevator = new ElevatorSubsystem();
+
+  private final ArmSubsystem m_arm = new ArmSubsystem();
 
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
@@ -153,31 +160,61 @@ private void configureBindings() {
 
 
     // set next state, change LED colors accordingly 
-    copilotJoystick.leftTrigger().onTrue(
-      new ParallelCommandGroup(
-      new InstantCommand(() -> {nextState = RobotState.L4_CORAL;}),
-      m_ledSubsystem.runSolidRed())); 
-    copilotJoystick.rightTrigger().onTrue(
-      new ParallelCommandGroup(
-      new InstantCommand(() -> {nextState = RobotState.L3_CORAL;}),
-      m_ledSubsystem.runSolidOrange())); 
-    copilotJoystick.leftBumper().onTrue(
-      new ParallelCommandGroup(
-      new InstantCommand(() -> {nextState = RobotState.L2_CORAL;}),
-      m_ledSubsystem.runSolidYellow())); 
-    copilotJoystick.rightBumper().onTrue(
-      new ParallelCommandGroup(
-      new InstantCommand(() -> {nextState = RobotState.L1_CORAL;}),
-      m_ledSubsystem.runSolidWhite())); 
-    copilotJoystick.a().onTrue(
-      new ParallelCommandGroup(
-      new InstantCommand(() -> {nextState = RobotState.L3_ALGAE;}),
-      m_ledSubsystem.runSolidPurple())); 
-    copilotJoystick.y().onTrue(
-      new ParallelCommandGroup(
-      new InstantCommand(() -> {nextState = RobotState.L2_ALGAE;}),
-      m_ledSubsystem.runSolidPink())); 
+    //copilotJoystick.leftTrigger().onTrue(
+      //new ParallelCommandGroup(
+      //new InstantCommand(() -> {nextState = RobotState.L4_CORAL;}),
+      //m_ledSubsystem.runSolidRed())); 
+
+    //copilotJoystick.rightTrigger().onTrue(
+      //new ParallelCommandGroup(
+      //new InstantCommand(() -> {nextState = RobotState.L3_CORAL;}),
+      //m_ledSubsystem.runSolidOrange())); 
+
+    // copilotJoystick.leftBumper().onTrue(
+    //   new ParallelCommandGroup(
+    //   new InstantCommand(() -> {nextState = RobotState.L2_CORAL;}),
+    //   m_ledSubsystem.runSolidYellow())); 
+
+    // copilotJoystick.rightBumper().onTrue(
+    //   new ParallelCommandGroup(
+    //   new InstantCommand(() -> {nextState = RobotState.L1_CORAL;}),
+    //   m_ledSubsystem.runSolidWhite())); 
+
+    // copilotJoystick.a().onTrue(
+    //   new ParallelCommandGroup(
+    //   new InstantCommand(() -> {nextState = RobotState.L3_ALGAE;}),
+    //   m_ledSubsystem.runSolidPurple())); 
+
+    // copilotJoystick.y().onTrue(
+    //   new ParallelCommandGroup(
+    //   new InstantCommand(() -> {nextState = RobotState.L2_ALGAE;}),
+    //   m_ledSubsystem.runSolidPink())); 
+
+    //open loop control testing:
+    copilotJoystick.leftTrigger().whileTrue(
+      m_elevator.manualMoveCommand());
+
+    copilotJoystick.rightTrigger().whileTrue(
+      m_arm.manualMoveCommand());
 
+    copilotJoystick.leftBumper().whileTrue(
+      m_elevator.manualMoveBackwardCommand());
+
+    copilotJoystick.rightBumper().whileTrue(
+      m_arm.manualMoveBackwardCommand());
+
+    copilotJoystick.a().whileTrue(
+      m_collar.runCollar());
+
+    copilotJoystick.y().whileTrue(
+      m_collar.runCollarBackward());
+
+    m_elevator.setDefaultCommand(m_elevator.keepInPlaceCommand());
+    m_arm.setDefaultCommand(m_arm.keepInPlaceCommand());
+    m_collar.setDefaultCommand(m_collar.runCollarOff());
+
+
+
     // run command runSolidGreen continuously if robot isWithinTarget()
     m_vision.isWithinTargetTrigger().whileTrue(m_ledSubsystem.runSolidGreen());
 
@@ -194,6 +231,19 @@ private void configureBindings() {
     testJoystick.leftBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
     testJoystick.rightBumper().and(testJoystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
     testJoystick.rightBumper().and(testJoystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
+
+    //sysID routines for mechanisms 
+    //(comment out other mechanisms while running one of them since they are all bound
+    //to the same buttons )
+    //or change the bindings buttons haha
+    //run sysID routines for collar
+
+    //run sysID routines for elevator
+    testJoystick.a().whileTrue(m_elevator.sysIdDynamic(Direction.kForward));
+    testJoystick.b().whileTrue(m_elevator.sysIdDynamic(Direction.kReverse));
+    testJoystick.x().whileTrue(m_elevator.sysIdQuasistatic(Direction.kForward));
+    testJoystick.y().whileTrue(m_elevator.sysIdQuasistatic(Direction.kReverse));
+
     if (Utils.isSimulation()) {
       drivetrain.resetPose(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
     }

src/main/java/frc/robot/generated/TunerConstants.java → src/main/java/frc/robot/generated/TunerConstantsCeridwen.java

@@ -20,7 +20,7 @@
 
 // Generated by the Tuner X Swerve Project Generator
 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
-public class TunerConstants {
+public class TunerConstantsCeridwen {
     // Both sets of gains need to be tuned to your individual robot.
 
     // The steer motor uses any SwerveModule.SteerRequestType control request with the