Merge branch 'main' of https://github.com/team581/offseason-2023-bot-1

src/main/java/frc/robot/managers/superstructure/Positions.java

@@ -7,7 +7,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public class Positions {
-  public static final SuperstructurePosition STOWED = new SuperstructurePosition(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0)):
+  public static final SuperstructurePosition STOWED = new SuperstructurePosition(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0));
 
   public static final SuperstructurePosition INTAKING_CUBE_FLOOR = new SuperstructurePosition(Rotation2d.fromDegrees(20), Rotation2d.fromDegrees(135));
   public static final SuperstructurePosition INTAKING_CUBE_SHELF =  new SuperstructurePosition(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(180));

src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java

@@ -1,37 +1,49 @@
+// 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.managers.superstructure;
 
-import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.intake.IntakeSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+import frc.robot.wrist.WristSubsystem;
 
 public class SuperstructureManager extends LifecycleSubsystem {
   private SuperstructureMotionManager motionManager;
-  private ShoulderSubsystem shoulder;
+  private WristSubsystem shoulder;
   private WristSubsystem wrist;
   private IntakeSubsystem intake;
-  private SuperstructurePosition goalPosition;
+  private SuperstructureState goalState;
+
+  public SuperstructureManager(SuperstructureMotionManager motionManager, IntakeSubsystem intake) {
+    super(SubsystemPriority.SUPERSTRUCTURE_MANAGER);
 
-  public SuperstructureManager (SuperstructureMotionManager motionManager, ShoulderSubsystem shoulder, WristSubsystem wrist, IntakeSubsystem intake) {
     this.motionManager = motionManager;
-    this.shoulder = shoulder;
-    this.wrist = wrist;
+    this.shoulder = motionManager.shoulder;
+    this.wrist = motionManager.wrist;
     this.intake = intake;
   }
 
-// create a setGoal method - you give it a position, it returns void
-private void setGoal(SuperstructurePosition goalPosition) {
-  this.goalPosition = goalPosition;
-}
+  private void setGoal(SuperstructureState goalState) {
+    this.goalState = goalState;
+  }
 
-// create an atGoal() method
-public boolean atGoal(Rotation2d shoulderAngle, Rotation2d wristAngle) {
-  return shoulderAngle == goalPosition.shoulderAngle && wristAngle == goalPosition.wristAngle;
-}
+  public boolean atGoal(SuperstructureState state) {
+    return shoulder.atAngle(state.position.shoulderAngle.getDegrees())
+        && wrist.atAngle(state.position.wristAngle.getDegrees());
+  }
 
-// in enabledperiodic, go to the goal position
-@Override
-public void enabledPeriodic() {
-  motionmanager.set
-}
-  // create a setPositionCommand method, which finishes once at goal
+  // in enabledperiodic, go to the goal position
+  @Override
+  public void enabledPeriodic() {
+    motionManager.set(goalState.position);
+  }
+
+  public Command setStateCommand(SuperstructureState newGoalState) {
+    return Commands.runOnce(() -> setGoal(newGoalState))
+        .andThen(Commands.waitUntil(() -> atGoal(newGoalState)));
+  }
 }

src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java

@@ -1,24 +1,59 @@
+// 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.managers.superstructure;
 
 import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+import frc.robot.wrist.WristSubsystem;
+import java.util.ArrayList;
 
 public class SuperstructureMotionManager extends LifecycleSubsystem {
-  private ShoulderSubsystem shoulder;
-  private WristSubsystem wrist;
+  public final WristSubsystem shoulder;
+  public final WristSubsystem wrist;
   private SuperstructurePosition currentPoint = Positions.STOWED;
   private SuperstructurePosition goalPosition = Positions.STOWED;
+  private final ArrayList<SuperstructurePosition> positionList =
+      new ArrayList<SuperstructurePosition>();
+
+  public SuperstructureMotionManager(WristSubsystem shoulder, WristSubsystem wrist) {
+    super(SubsystemPriority.SUPERSTRUCTURE_MOTION_MANAGER);
 
-  public SuperstructureMotionManager (ShoulderSubsystem shoulder, WristSubsystem wrist) {
     this.shoulder = shoulder;
     this.wrist = wrist;
   }
 
-  public void set(SuperstructurePosition position) {
+  private boolean atPosition(SuperstructurePosition position) {
+    return shoulder.atAngle(position.shoulderAngle)
+        && wrist.atAngle(position.wristAngle);
+  }
+
+  public void set(SuperstructurePosition newGoalPosition) {
+    if (!newGoalPosition.equals(goalPosition)) {
+      positionList.clear();
+      positionList.add(
+          new SuperstructurePosition(currentPoint.shoulderAngle, Positions.STOWED.wristAngle));
+      positionList.add(
+          new SuperstructurePosition(newGoalPosition.shoulderAngle, Positions.STOWED.wristAngle));
+      positionList.add(newGoalPosition);
+
+      goalPosition = newGoalPosition;
+    }
   }
 
   @Override
-  public void enabledperiodic() {
-      shoulder.setAngle(currentpoint.shoulderAngle);
-      wrist.setAngle(currentpoint.wristAngle);
+  public void enabledInit() {
+    positionList.clear();
+  }
+
+  @Override
+  public void enabledPeriodic() {
+    if (atPosition(currentPoint) && !positionList.isEmpty()) {
+      currentPoint = positionList.remove(0);
+    }
+
+    shoulder.set(currentPoint.shoulderAngle);
+    wrist.set(currentPoint.wristAngle);
   }
 }

src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java

@@ -5,15 +5,24 @@
 package frc.robot.managers.superstructure;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import frc.robot.util.scheduling.LifecycleSubsystem;
 
-public class SuperstructurePosition extends LifecycleSubsystem {
+public class SuperstructurePosition {
   public final Rotation2d shoulderAngle;
   public final Rotation2d wristAngle;
 
   public SuperstructurePosition(Rotation2d shoulderAngle, Rotation2d wristAngle) {
     this.shoulderAngle = shoulderAngle;
     this.wristAngle = wristAngle;
   }
-}
 
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SuperstructurePosition) {
+      SuperstructurePosition position = (SuperstructurePosition) obj;
+
+      return shoulderAngle.equals(position.shoulderAngle) && wristAngle.equals(position.wristAngle);
+    }
+
+    return false;
+  }
+}

src/main/java/frc/robot/managers/superstructure/SuperstructureState.java

@@ -0,0 +1,24 @@
+// 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.managers.superstructure;
+
+import frc.robot.intake.IntakeState;
+
+public class SuperstructureState {
+  public final SuperstructurePosition position;
+  public final IntakeState intakeMode;
+  public final boolean intakeNow;
+
+  public SuperstructureState(
+      SuperstructurePosition position, IntakeState intakeMode, boolean intakeNow) {
+    this.position = position;
+    this.intakeMode = intakeMode;
+    this.intakeNow = intakeNow;
+  }
+
+  public SuperstructureState(SuperstructurePosition position, IntakeState intakeMode) {
+    this(position, intakeMode, false);
+  }
+}

src/main/java/frc/robot/shoulder/ShoulderSubsystem.java

@@ -0,0 +1,81 @@
+package frc.robot.shoulder;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+
+public class ShoulderSubsystem extends LifecycleSubsystem {
+  private final CANSparkMax motor;
+  private final CANSparkMax motorFollower;
+
+  private RelativeEncoder encoder;
+  private SparkMaxPIDController pid;
+  private Rotation2d goalAngle = new Rotation2d();
+
+  public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) {
+    super(SubsystemPriority.SHOULDER);
+    this.motor = motor;
+    this.motorFollower = motorFollower;
+    motorFollower.follow(motor);
+    encoder = motor.getEncoder();
+    pid = motor.getPIDController();
+    motor.setSmartCurrentLimit(35);
+    motorFollower.setSmartCurrentLimit(35);
+
+    pid.setP(5);
+    pid.setI(0);
+    pid.setD(0);
+
+    encoder.setPosition(0);
+    encoder.setPositionConversionFactor(50);
+  }
+
+  @Override
+  public void enabledPeriodic() {
+    pid.setReference(goalAngle.getRotations(), ControlType.kSmartMotion);
+  }
+
+@Override
+public void robotPeriodic(){
+  Logger.getInstance().recordOutput("Shoulder/MainMotor/Velocity", encoder.getVelocity());
+  Logger.getInstance().recordOutput("Shoulder/FollowerMotor/Velocity", encoder.getVelocity());
+  Logger.getInstance().recordOutput("Shoulder/MainMotor/Angle", getWristAngle().getDegrees());
+  Logger.getInstance().recordOutput("Shoulder/FollowerMotor/Angle", getWristAngle().getDegrees());
+  Logger.getInstance().recordOutput("Shoulder/MainMotor/GoalAngle", goalAngle.getDegrees());
+  Logger.getInstance().recordOutput("Shoulder/FollowerMotor/GoalAngle", goalAngle.getDegrees());
+  Logger.getInstance().recordOutput("Shoulder/MainMotor/DutyCycleOutput", motor.getAppliedOutput());
+  Logger.getInstance().recordOutput("Shoulder/FollowerMotor/DutyCycleOutput", motor.getAppliedOutput());
+  Logger.getInstance().recordOutput("Shoulder/MainMotor/StatorCurrent", motor.getOutputCurrent());
+  Logger.getInstance().recordOutput("Shoulder/FollowerMotor/StatorCurrent", motor.getOutputCurrent());
+
+}
+
+public void set(Rotation2d angle) {
+    goalAngle = angle;
+  }
+
+  public Command setPositionCommand(Rotation2d angle) {
+    return run(() -> {
+          set(angle);
+        })
+        .until(() -> atAngle(angle));
+  }
+
+  private boolean atAngle(Rotation2d angle) {
+    double actualAngle = getWristAngle().getDegrees();
+    return Math.abs(actualAngle - angle.getDegrees()) < 1;
+  }
+
+  private Rotation2d getWristAngle() {
+    return Rotation2d.fromRotations(encoder.getPosition());
+  }
+}
+

src/main/java/frc/robot/util/scheduling/SubsystemPriority.java

@@ -5,6 +5,9 @@
 package frc.robot.util.scheduling;
 
 public enum SubsystemPriority {
+  SUPERSTRUCTURE_MANAGER(21),
+  SUPERSTRUCTURE_MOTION_MANAGER(20),
+
   // Run autobalance and autorotate before swerve
   AUTOBALANCE(11),
   AUTOROTATE(11),
@@ -13,6 +16,7 @@ public enum SubsystemPriority {
   SWERVE(10),
   IMU(10),
   INTAKE(10),
+  SHOULDER(10),
   // Run localization after swerve & IMU
   LOCALIZATION(9),
 

src/main/java/frc/robot/wrist/WristSubsystem.java

@@ -1,8 +1,12 @@
 package frc.robot.wrist;
+import org.littletonrobotics.junction.Logger;
+
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.ControlType;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxPIDController;
+
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
@@ -11,15 +15,15 @@ public class WristSubsystem extends LifecycleSubsystem {
   private final CANSparkMax motor;
   private RelativeEncoder encoder;
   private SparkMaxPIDController pid;
-  private double goalAngle = 0.0;
+  private Rotation2d goalAngle = new Rotation2d();
 
   public WristSubsystem(CANSparkMax motor) {
     super(SubsystemPriority.WRIST);
 
     this.motor = motor;
     encoder = motor.getEncoder();
     pid = motor.getPIDController();
-
+    motor.setSmartCurrentLimit(35);
     pid.setP(5);
     pid.setI(0);
     pid.setD(0);
@@ -30,25 +34,35 @@ public WristSubsystem(CANSparkMax motor) {
 
   @Override
   public void enabledPeriodic() {
-    pid.setReference(goalAngle / 360.0, ControlType.kSmartMotion);
+    pid.setReference(goalAngle.getRotations(), ControlType.kSmartMotion);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    Logger.getInstance().recordOutput("Wrist/Velocity", encoder.getVelocity());
+    Logger.getInstance().recordOutput("Wrist/Angle", getWristAngle().getDegrees());
+    Logger.getInstance().recordOutput("Wrist/GoalAngle", goalAngle.getDegrees());
+    Logger.getInstance().recordOutput("Wrist/DutyCycleOutput", motor.getAppliedOutput());
+    Logger.getInstance().recordOutput("Wrist/StatorCurrent", motor.getOutputCurrent());
   }
 
-  public void set(double angle) {
+  public void set(Rotation2d angle) {
     goalAngle = angle;
   }
 
-  public Command setPositionCommand(double angle) {
+  public Command setPositionCommand(Rotation2d angle) {
     return run(() -> {
           set(angle);
         })
         .until(() -> atAngle(angle));
   }
 
-  private boolean atAngle(double angle) {
-    return Math.abs(getWristAngle() - angle) < 1;
+  private boolean atAngle(Rotation2d angle) {
+    double actualAngle = getWristAngle().getDegrees();
+    return Math.abs(actualAngle - angle.getDegrees()) < 1;
   }
 
-  private double getWristAngle() {
-    return encoder.getPosition() * 360.0;
+  private Rotation2d getWristAngle() {
+    return Rotation2d.fromRotations(encoder.getPosition());
   }
 }