fixed a sim issue. pid is WAY out of tune though

SciBorgs · Jan 11, 2025 · 17cebc0 · 17cebc0
1 parent a0c1414
commit 17cebc0
Show file tree

Hide file tree

Showing 8 changed files with 142 additions and 28 deletions.
diff --git a/simgui.json b/simgui.json
@@ -80,6 +80,9 @@
     },
     "transitory": {
       "Robot": {
+        "arm": {
+          "open": true
+        },
         "autos": {
           "open": true
         },
@@ -167,5 +170,40 @@
   },
   "NetworkTables Info": {
     "visible": true
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 338,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/Robot/arm/position"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/Robot/arm/velocity"
+            }
+          ]
+        }
+      ]
+    }
   }
 }
diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java
@@ -33,8 +33,10 @@
 import org.sciborgs1155.lib.Test;
 import org.sciborgs1155.robot.Ports.OI;
 import org.sciborgs1155.robot.arm.Arm;
+import org.sciborgs1155.robot.arm.ArmConstants;
 import org.sciborgs1155.robot.commands.Autos;
 import org.sciborgs1155.robot.drive.Drive;
+import org.sciborgs1155.robot.roller.Roller;
 import org.sciborgs1155.robot.vision.Vision;
 
 /**
@@ -54,6 +56,7 @@ public class Robot extends CommandRobot implements Logged {
   private final Drive drive = Drive.create();
   private final Vision vision = Vision.create();
   private final Arm arm = Arm.create();
+  private final Roller roller = Roller.create();
 
   // COMMANDS
   @Log.NT private final SendableChooser<Command> autos = Autos.configureAutos(drive);
@@ -135,6 +138,10 @@ private void configureBindings() {
 
     drive.setDefaultCommand(drive.drive(x, y, omega));
 
+    arm.setDefaultCommand(arm.goTowards(ArmConstants.DEFAULT_ANGLE));
+
+    roller.setDefaultCommand(roller.halt());
+
     autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy());
 
     test().whileTrue(systemsCheck());
@@ -146,9 +153,10 @@ private void configureBindings() {
         .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER))
         .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER));
 
-    operator.a().onTrue(arm.moveArmTo(MAX_ANGLE));
-    operator.b().onTrue(arm.moveArmTo(MIN_ANGLE));
+    operator.a().whileTrue(arm.goTowards(MAX_ANGLE));
+    operator.b().whileTrue(arm.goTowards(MIN_ANGLE));
     // TODO: Add any additional bindings.
+
   }
 
   /**

diff --git a/src/main/java/org/sciborgs1155/robot/arm/Arm.java b/src/main/java/org/sciborgs1155/robot/arm/Arm.java
@@ -1,10 +1,13 @@
 package org.sciborgs1155.robot.arm;
 
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
 import static org.sciborgs1155.robot.arm.ArmConstants.*;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -18,7 +21,13 @@ public class Arm extends SubsystemBase implements Logged, AutoCloseable {
   /** Interface for interacting with the motor itself. */
   @Log.NT private final ArmIO hardware;
 
-  private final PIDController feedbackController = new PIDController(kP, kI, kD);
+  private final ProfiledPIDController feedbackController =
+      new ProfiledPIDController(
+          kP,
+          kI,
+          kD,
+          new TrapezoidProfile.Constraints(
+              MAX_VELOCITY.in(RadiansPerSecond), MAX_ACCEl.in(RadiansPerSecondPerSecond)));
   private final ArmFeedforward feedforwardController = new ArmFeedforward(kS, kG, kV, kA);
 
   /**
@@ -67,17 +76,27 @@ public double getVoltage() {
     return hardware.voltage();
   }
 
-  /** Moves the arm to a specified goal angle. */
-  public Command moveArmTo(Angle goal) {
-    double feedForward = feedforwardController.calculate(hardware.position(), hardware.velocity());
-    double feedBack = feedbackController.calculate(hardware.position(), goal.in(Radians));
-    return run(() -> hardware.setVoltage(feedBack + feedForward))
+  /** Moves the arm towards a specified goal angle. */
+  public Command goTowards(Angle goal) {
+    return run(() -> {
+          double feedForward = feedforwardController.calculate(goal.in(Radians), 0);
+          double feedBack = feedbackController.calculate(hardware.position(), goal.in(Radians));
+          hardware.setVoltage(feedBack + feedForward);
+        })
         .withName("Moving Arm To: " + goal.toString() + " radians")
         .andThen(Commands.print("Yippee"));
   }
 
+  public Command goTo(Angle goal) {
+    return goTowards(goal)
+        .until(() -> (Math.abs(goal.in(Radians) - position()) < GOTO_TOLERANCE.in(Radians)));
+  }
+
   @Override
   public void close() throws Exception {
     hardware.close();
   }
+
+  @Override
+  public void simulationPeriodic() {}
 }
diff --git a/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java b/src/main/java/org/sciborgs1155/robot/arm/ArmConstants.java
@@ -1,40 +1,52 @@
 package org.sciborgs1155.robot.arm;
 
 import static edu.wpi.first.units.Units.Centimeters;
+import static edu.wpi.first.units.Units.Degrees;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
 
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
 
 /** Constants for the {@link Arm} subsystem. */
 public class ArmConstants {
   // TODO: not tuned (at all)
-  public static final double kP = 1;
+  public static final double kP = 10;
   public static final double kI = 0;
-  public static final double kD = 0.01;
+  public static final double kD = 0.1;
 
-  public static final double kS = 1;
+  public static final double kS = 0;
   public static final double kV = 0;
-  public static final double kA = 0.01;
-  public static final double kG = 0.01;
+  public static final double kA = 0;
+  public static final double kG = 0;
 
   // TODO: figure these out
-  public static final Rotation2d INTAKE_ANGLE = Rotation2d.fromRadians(0);
-  public static final Rotation2d OUTTAKE_ANGLE = Rotation2d.fromRadians(0);
-  public static final Rotation2d STARTING_ANGLE = Rotation2d.fromRadians(0);
+  public static final Angle INTAKE_ANGLE = Radians.of(-Math.PI / 4);
+  public static final Angle OUTTAKE_ANGLE = Radians.of(Math.PI * 3 / 4);
+  public static final Angle STARTING_ANGLE = Radians.of(Math.PI / 2);
+  public static final Angle DEFAULT_ANGLE = Radians.of(Math.PI * 5 / 8);
+  public static final Angle CLIMB_INTAKE_ANGLE = Radians.of(0);
+  public static final Angle CLIMB_FINAL_ANGLE = Radians.of(Math.PI * 3 / 4);
 
   // TODO: and these...
-  public static final DCMotor GEARBOX = DCMotor.getNEO(1);
+  public static final DCMotor GEARBOX = DCMotor.getKrakenX60(2);
   public static final double GEARING = 8.21;
-  public static final double MOI = 1;
+  public static final double MOI = 0.7765;
 
   public static final Distance ARM_LENGTH = Centimeters.of(10);
 
   /** Fully extended. */
-  public static final Angle MIN_ANGLE = Radians.of(0.5);
+  public static final Angle MIN_ANGLE = Radians.of(-Math.PI / 4 - 0.2);
 
   /** Fully retracted. */
-  public static final Angle MAX_ANGLE = Radians.of(3);
+  public static final Angle MAX_ANGLE = Radians.of(Math.PI * 3 / 4 + 0.1);
+
+  public static final Angle GOTO_TOLERANCE = Degrees.of(2);
+
+  public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(4);
+  public static final AngularAcceleration MAX_ACCEl = RadiansPerSecondPerSecond.of(10);
 }
diff --git a/src/main/java/org/sciborgs1155/robot/arm/SimArm.java b/src/main/java/org/sciborgs1155/robot/arm/SimArm.java
@@ -2,9 +2,11 @@
 
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.Seconds;
 import static org.sciborgs1155.robot.arm.ArmConstants.*;
 
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import org.sciborgs1155.robot.Constants;
 
 /** Simulated {@link ArmIO} hardware interface. */
 public class SimArm implements ArmIO {
@@ -17,7 +19,7 @@ public class SimArm implements ArmIO {
           MIN_ANGLE.in(Radians),
           MAX_ANGLE.in(Radians),
           true,
-          STARTING_ANGLE.getRadians());
+          STARTING_ANGLE.in(Radians));
 
   @Override
   public double position() {
@@ -32,6 +34,7 @@ public double velocity() {
   @Override
   public void setVoltage(double voltage) {
     simulation.setInputVoltage(voltage);
+    simulation.update(Constants.PERIOD.in(Seconds));
   }
 
   @Override

diff --git a/src/main/java/org/sciborgs1155/robot/commands/GroundIntake.java b/src/main/java/org/sciborgs1155/robot/commands/GroundIntake.java
@@ -0,0 +1,29 @@
+package org.sciborgs1155.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.sciborgs1155.robot.arm.Arm;
+import org.sciborgs1155.robot.arm.ArmConstants;
+import org.sciborgs1155.robot.roller.Roller;
+
+public class GroundIntake {
+
+  private Arm arm;
+  private Roller roller;
+
+  public GroundIntake(Arm arm, Roller roller) {
+    this.arm = arm;
+    this.roller = roller;
+  }
+
+  public Command intake() {
+    return arm.goTo(ArmConstants.INTAKE_ANGLE).alongWith(roller.intake());
+  }
+
+  public Command outtake() {
+    return arm.goTo(ArmConstants.OUTTAKE_ANGLE).andThen(roller.outtake());
+  }
+
+  public Command climb() {
+    return arm.goTo(ArmConstants.CLIMB_INTAKE_ANGLE);
+  }
+}
diff --git a/src/main/java/org/sciborgs1155/robot/roller/Roller.java b/src/main/java/org/sciborgs1155/robot/roller/Roller.java
@@ -16,15 +16,19 @@ public class Roller extends SubsystemBase implements Logged, AutoCloseable {
    * Returns a new {@link Roller} subsystem, which will have real hardware if the robot is real, and
    * no hardware connection if it isn't.
    */
-  public Roller create() {
+  public static Roller create() {
     return Robot.isReal() ? new Roller(new RealRoller()) : new Roller(new NoRoller());
   }
 
   /** Creates a new {@link Roller} with no hardware interface(does nothing). */
-  public Roller none() {
+  public static Roller none() {
     return new Roller(new NoRoller());
   }
 
+  private Roller(RollerIO hardware) {
+    this.hardware = hardware;
+  }
+
   /** Makes the roller spin inwards(towards robot). */
   public Command intake() {
     return run(() -> hardware.setPower(INTAKE_POWER)).withName("Intaking");
@@ -35,8 +39,9 @@ public Command outtake() {
     return run(() -> hardware.setPower(OUTTAKE_POWER)).withName("Outtaking");
   }
 
-  private Roller(RollerIO hardware) {
-    this.hardware = hardware;
+  /** Stops the roller motors. */
+  public Command halt() {
+    return run(() -> hardware.setPower(0));
   }
 
   @Override

diff --git a/src/test/java/org/sciborgs1155/robot/ArmTest.java b/src/test/java/org/sciborgs1155/robot/ArmTest.java
@@ -35,7 +35,7 @@ public void pointExtension(Angle angle) {
     Angle positionSetpoint = angle;
     System.out.println("Position Setpoint: " + positionSetpoint.toString());
 
-    run(arm.moveArmTo(positionSetpoint));
+    arm.goTowards(positionSetpoint);
     fastForward(500);
 
     assertEquals(positionSetpoint.in(Radians), arm.position(), TOLERANCE);