wip

simgui.json

@@ -103,6 +103,7 @@
           "Measurement": {
             "open": true
           },
+          "open": true,
           "setpoint": {
             "open": true
           }

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

@@ -2,11 +2,9 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.lib.Telemetry;
 import frc.robot.arm.Arm;
-import frc.robot.arm.ManualCommand;
 import frc.robot.auto.Auto;
 import frc.robot.intake.Intake;
 import frc.robot.odometry.Odometry;
@@ -86,8 +84,6 @@ private void configureBindings() {
     // operatorController.a().onTrue(superstructure.ampPosition());
     // operatorController.b().onTrue(superstructure.ampShoot());
     operatorController.x().onTrue(superstructure.stow());
-
-    operatorController.y().whileTrue(superstructure.manualControl().andThen(new ManualCommand(operatorController::getLeftY))).onFalse(Commands.runOnce(() -> superstructure.setSetpoint(superstructure.getState())));
   }
 
   /**

src/main/java/frc/robot/arm/Arm.java

@@ -1,12 +1,16 @@
 package frc.robot.arm;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.controller.PositionControllerIO;
 import frc.lib.controller.PositionControllerIO.PositionControllerIOValues;
+import frc.robot.RobotConstants;
 import frc.robot.arm.ArmConstants.ShoulderConstants;
+import frc.robot.superstructure.SuperstructureConstants;
+import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants;
 
 /** Subsystem class for the arm subsystem. */
 public class Arm extends Subsystem {
@@ -20,11 +24,18 @@ public class Arm extends Subsystem {
   /** Shoulder values. */
   private final PositionControllerIOValues shoulderValues;
 
+  /** Arm goal. */
+  private State setpoint, goal;
+
   /** Creates a new instance of the arm subsystem. */
   private Arm() {
     shoulder = ArmFactory.createShoulder();
     shoulderValues = new PositionControllerIOValues();
     shoulder.configure(ShoulderConstants.CONTROLLER_CONSTANTS);
+
+    setPosition(ShoulderAngleConstants.INITIAL.getRotations());
+    setpoint = new State(ShoulderAngleConstants.INITIAL.getRotations(), 0);
+    goal = new State(ShoulderAngleConstants.INITIAL.getRotations(), 0);
   }
 
   /**
@@ -43,27 +54,37 @@ public static Arm getInstance() {
   @Override
   public void periodic() {
     shoulder.update(shoulderValues);
+
+    setpoint = ShoulderAngleConstants.MOTION_PROFILE.calculate(
+        RobotConstants.PERIODIC_DURATION,
+        setpoint,
+        goal);
+
+    shoulder.setSetpoint(setpoint.position, setpoint.velocity);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
     PositionControllerIO.addToShuffleboard(tab, "Shoulder", shoulderValues);
   }
 
-  public State getMeasuredShoulderState() {
+  public State getState() {
     return new TrapezoidProfile.State(
         shoulderValues.positionRotations, shoulderValues.velocityRotationsPerSecond);
   }
 
-  public void setShoulderPosition(double positionRotations) {
-    shoulder.setPosition(positionRotations);
+  public void setGoal(State goal) {
+    this.goal = goal;
   }
 
-  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
-    shoulder.setSetpoint(positionRotations, velocityRotationsPerSecond);
+  public boolean atGoal() {
+    return MathUtil.isNear(
+        getState().position,
+        goal.position,
+        SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations());
   }
 
-  public void setVoltage(double volts) {
-    // TODO
+  public void setPosition(double positionRotations) {
+    shoulder.setPosition(positionRotations);
   }
 }

src/main/java/frc/robot/intake/Intake.java

@@ -1,11 +1,13 @@
 package frc.robot.intake;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.controller.VelocityControllerIO;
 import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues;
 import frc.robot.intake.IntakeConstants.BackRollerConstants;
 import frc.robot.intake.IntakeConstants.FrontRollerConstants;
+import frc.robot.intake.IntakeConstants.RollerConstants;
 
 /** Subsystem class for the intake subsystem. */
 public class Intake extends Subsystem {
@@ -19,6 +21,8 @@ public class Intake extends Subsystem {
   /** Roller values. */
   private final VelocityControllerIOValues frontRollerValues, backRollerValues;
 
+  private double rollerGoal;
+
   /** Creates a new instance of the intake subsystem. */
   private Intake() {
     frontRoller = IntakeFactory.createFrontRoller();
@@ -47,6 +51,9 @@ public static Intake getInstance() {
   public void periodic() {
     frontRoller.update(frontRollerValues);
     backRoller.update(backRollerValues);
+
+    frontRoller.setSetpoint(rollerGoal);
+    backRoller.setSetpoint(rollerGoal);
   }
 
   @Override
@@ -59,8 +66,18 @@ public double getRollerVelocity() {
     return (frontRollerValues.velocityRotationsPerSecond + backRollerValues.velocityRotationsPerSecond) / 2;
   }
 
-  public void setSetpoint(double rollerVelocityRotationsPerSecond) {
-    frontRoller.setSetpoint(rollerVelocityRotationsPerSecond);
-    backRoller.setSetpoint(rollerVelocityRotationsPerSecond);
+  public void setGoal(double rollerVelocityRotationsPerSecond) {
+    this.rollerGoal = rollerVelocityRotationsPerSecond;
+  }
+
+  public boolean atGoal() {
+    return MathUtil.isNear(
+        frontRollerValues.velocityRotationsPerSecond,
+        rollerGoal,
+        RollerConstants.SPEED_TOLERANCE)
+    && MathUtil.isNear(
+        backRollerValues.velocityRotationsPerSecond,
+        rollerGoal,
+        RollerConstants.SPEED_TOLERANCE);
   }
 }

src/main/java/frc/robot/shooter/Shooter.java

@@ -1,5 +1,6 @@
 package frc.robot.shooter;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.lib.Subsystem;
 import frc.lib.controller.VelocityControllerIO;
@@ -25,6 +26,8 @@ public class Shooter extends Subsystem {
   /** Flywheel values. */
   private final VelocityControllerIOValues flywheelValues;
 
+  private double serializerGoal, flywheelGoal;
+
   /** Creates a new instance of the shooter subsystem. */
   private Shooter() {
     serializer = ShooterFactory.createSerializer();
@@ -53,6 +56,9 @@ public static Shooter getInstance() {
   public void periodic() {
     serializer.update(serializerValues);
     flywheel.update(flywheelValues);
+
+    flywheel.setSetpoint(flywheelGoal);
+    serializer.setSetpoint(serializerGoal);
   }
 
   @Override
@@ -69,9 +75,20 @@ public double getSerializerVelocity() {
     return serializerValues.velocityRotationsPerSecond;
   }
 
-  public void setSetpoint(
+  public void setGoal(
       double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) {
-    flywheel.setSetpoint(flywheelVelocityRotationsPerSecond);
-    serializer.setSetpoint(serializerVelocityRotationsPerSecond);
+        this.flywheelGoal = flywheelVelocityRotationsPerSecond;
+        this.serializerGoal = serializerVelocityRotationsPerSecond;
+  }
+
+  public boolean atGoal() {
+    return MathUtil.isNear(
+        getFlywheelVelocity(),
+        flywheelGoal,
+        FlywheelConstants.SPEED_TOLERANCE) && 
+    MathUtil.isNear(
+        getSerializerVelocity(),
+        serializerGoal,
+        FlywheelConstants.SPEED_TOLERANCE);
   }
 }