revert arm code to week1

team4909 · Mar 9, 2024 · 1daa219 · 1daa219
1 parent 717fcde
commit 1daa219
Show file tree

Hide file tree

Showing 10 changed files with 397 additions and 290 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -11,7 +11,7 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.arm.Arm;
-import frc.robot.arm.Arm.ArmSetpoints;
+import frc.robot.arm.ArmSetpoints;
 import frc.robot.climber.Climber;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.feeder.Feeder;
@@ -99,15 +99,15 @@ public Robot() {
         break;
     }
     m_lights = new Lights();
-    NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier);
+    // NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier);
     NoteVisualizer.resetNotes();
     NoteVisualizer.showStagedNotes();
     Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake);
     NamedCommands.registerCommand("intake", m_intake.intake());
     NamedCommands.registerCommand("intakeOff", m_intake.idle());
     NamedCommands.registerCommand("enableShooter", new ScheduleCommand(m_shooter.runShooter()));
     NamedCommands.registerCommand("runShooter", m_shooter.runShooter().withTimeout(0.1));
-    NamedCommands.registerCommand("subShot", m_arm.aimWrist(Arm.kSubwooferWristAngleRad));
+    NamedCommands.registerCommand("subShot", m_arm.goToSetpoint(-0.52, 2.083, 0.0, 0.0));
     NamedCommands.registerCommand("feederOn", m_feeder.feed().withTimeout(.3));
     NamedCommands.registerCommand("feederOnTest", m_feeder.feed());
     NamedCommands.registerCommand("feederOff", m_feeder.idle().withTimeout(1));
@@ -150,26 +150,6 @@ public Robot() {
         "Shooter SysId (Dynamic Forward)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kForward));
     m_autoChooser.addOption(
         "Shooter SysId (Dynamic Reverse)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));
-    m_autoChooser.addOption(
-        "Elbow SysId (Quasistatic Forward)",
-        m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kForward));
-    m_autoChooser.addOption(
-        "Elbow SysId (Quasistatic Reverse)",
-        m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kReverse));
-    m_autoChooser.addOption(
-        "Elbow SysId (Dynamic Forward)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kForward));
-    m_autoChooser.addOption(
-        "Elbow SysId (Dynamic Reverse)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kReverse));
-    m_autoChooser.addOption(
-        "Wrist SysId (Quasistatic Forward)",
-        m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kForward));
-    m_autoChooser.addOption(
-        "Wrist SysId (Quasistatic Reverse)",
-        m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kReverse));
-    m_autoChooser.addOption(
-        "Wrist SysId (Dynamic Forward)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kForward));
-    m_autoChooser.addOption(
-        "Wrist SysId (Dynamic Reverse)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kReverse));
     m_autoChooser.addOption("3 Piece Centerline", autos.centerlineTwoPiece());
     m_drivetrain.setDefaultCommand(
         m_drivetrain.joystickDrive(
@@ -193,12 +173,10 @@ public Robot() {
         .whileTrue(Superstructure.aimAtGoal(m_drivetrain, m_shooter, m_lights));
 
     m_driverController.start().onTrue(m_drivetrain.zeroGyro());
-    m_driverController.leftStick().toggleOnTrue(m_arm.aimElbowForTuning());
-    m_driverController.rightStick().toggleOnTrue(m_arm.aimWristForTuning());
     // m_driverController.a().whileTrue(m_climber.unwindWinch());
     // m_driverController.y().whileTrue(m_climber.windWinch());
     m_driverController.rightBumper().whileTrue(Superstructure.spit(m_shooter, m_feeder, m_intake));
-    m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb));
+    m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(1.633, -2.371, 0.0, 0.0));
     m_driverController.b().whileTrue(Commands.parallel(m_arm.idleCoast(), m_climber.windWinch()));
     m_driverController.leftBumper().whileTrue(Superstructure.sensorIntake(m_feeder, m_intake));
 
@@ -212,7 +190,7 @@ public Robot() {
     m_operatorController
         .povUp()
         .onTrue(
-            Commands.parallel(m_arm.aimWrist(Arm.kSubwooferWristAngleRad), m_shooter.runShooter()))
+            Commands.parallel(m_arm.goToSetpoint(-0.52, 2.083, 0.0, 0.0), m_shooter.runShooter()))
         .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed));
 
     m_operatorController.y().whileTrue(m_shooter.runShooter());

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
@@ -1,10 +1,10 @@
 package frc.robot;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.arm.Arm;
-import frc.robot.arm.Arm.ArmSetpoints;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.feeder.Feeder;
 import frc.robot.intake.Intake;
@@ -27,14 +27,17 @@ public static Command sensorIntake(Feeder feeder, Intake intake) {
 
   public static Command sensorCatch(Shooter shooter, Feeder feeder, Intake intake, Arm arm) {
     return Commands.parallel(
-            arm.aimWrist(Arm.kCatchWristAngleRad), feeder.enterCoast(), shooter.catchNote())
+            arm.goToSetpoint(-0.558, 2.264 - Units.degreesToRadians(5.0), 0, 0),
+            feeder.enterCoast(),
+            shooter.catchNote())
         .andThen(feeder.pullBack().until(feeder.hasNote))
         .withName("Sensor Catch");
   }
 
   public static Command ampShot(Arm arm, Shooter shooter) {
-    return Commands.parallel(arm.goToSetpoint(ArmSetpoints.kAmp), shooter.runShooter())
-        .finallyDo(() -> arm.goToSetpoint(ArmSetpoints.kStowed));
+    return Commands.parallel(
+            arm.goToSetpoint(1.49 + 0.0873, -2.307, 0.0, 0.0), shooter.runShooter())
+        .finallyDo(() -> arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0));
   }
 
   public static Command spit(Shooter shooter, Feeder feeder, Intake intake) {