From 8fdba283d134dfb845469e6112bd3bbd95012c08 Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Sat, 26 Aug 2023 10:16:41 -0700 Subject: [PATCH 1/9] Fix syntax error --- src/main/java/frc/robot/managers/superstructure/Positions.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/managers/superstructure/Positions.java b/src/main/java/frc/robot/managers/superstructure/Positions.java index 94576ef..2045820 100644 --- a/src/main/java/frc/robot/managers/superstructure/Positions.java +++ b/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)); From 860c11709e23950a6282fe7e87ccf5a398a89b6c Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Sat, 26 Aug 2023 10:32:04 -0700 Subject: [PATCH 2/9] set wrist current limit --- src/main/java/frc/robot/wrist/WristSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/wrist/WristSubsystem.java b/src/main/java/frc/robot/wrist/WristSubsystem.java index 5864b3e..d533c1d 100644 --- a/src/main/java/frc/robot/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/wrist/WristSubsystem.java @@ -19,7 +19,7 @@ public WristSubsystem(CANSparkMax motor) { this.motor = motor; encoder = motor.getEncoder(); pid = motor.getPIDController(); - + motor.setSmartCurrentLimit(35); pid.setP(5); pid.setI(0); pid.setD(0); From d727f83fd39e96ccba88c6c8f64264aabe015bd7 Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Sat, 26 Aug 2023 10:50:23 -0700 Subject: [PATCH 3/9] change from double to rotation2d in wrist --- .../java/frc/robot/wrist/WristSubsystem.java | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/wrist/WristSubsystem.java b/src/main/java/frc/robot/wrist/WristSubsystem.java index d533c1d..6b4a57a 100644 --- a/src/main/java/frc/robot/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/wrist/WristSubsystem.java @@ -3,6 +3,8 @@ 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,7 +13,7 @@ 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); @@ -30,25 +32,26 @@ public WristSubsystem(CANSparkMax motor) { @Override public void enabledPeriodic() { - pid.setReference(goalAngle / 360.0, ControlType.kSmartMotion); + pid.setReference(goalAngle.getRotations(), ControlType.kSmartMotion); } - 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()); } } From 3d0ef2a446733f6822f0791c457a22767b215645 Mon Sep 17 00:00:00 2001 From: Jordan <117786700+jordanjcoderman@users.noreply.github.com> Date: Sat, 26 Aug 2023 11:18:58 -0700 Subject: [PATCH 4/9] Implement motion manager and work on superstructure manager --- .../superstructure/SuperstructureManager.java | 49 ++++++++++++------- .../SuperstructureMotionManager.java | 49 ++++++++++++++++--- .../SuperstructurePosition.java | 15 ++++-- .../util/scheduling/SubsystemPriority.java | 3 ++ .../java/frc/robot/wrist/WristSubsystem.java | 2 +- 5 files changed, 89 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java index 82c22d0..6f310dc 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java +++ b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java @@ -1,37 +1,50 @@ +// 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; - public SuperstructureManager (SuperstructureMotionManager motionManager, ShoulderSubsystem shoulder, WristSubsystem wrist, IntakeSubsystem intake) { + public SuperstructureManager(SuperstructureMotionManager motionManager, IntakeSubsystem intake) { + super(SubsystemPriority.SUPERSTRUCTURE_MANAGER); + 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(SuperstructurePosition goalPosition) { + this.goalPosition = goalPosition; + } -// create an atGoal() method -public boolean atGoal(Rotation2d shoulderAngle, Rotation2d wristAngle) { - return shoulderAngle == goalPosition.shoulderAngle && wristAngle == goalPosition.wristAngle; -} + public boolean atPosition(SuperstructurePosition position) { + return shoulder.atAngle(position.shoulderAngle.getDegrees()) + && wrist.atAngle(position.wristAngle.getDegrees()); + } + + // in enabledperiodic, go to the goal position + @Override + public void enabledPeriodic() { + motionManager.set(goalPosition); + } -// in enabledperiodic, go to the goal position -@Override -public void enabledPeriodic() { - motionmanager.set -} // create a setPositionCommand method, which finishes once at goal + public Command setGoalCommand(SuperstructurePosition newGoalPosition) { + return Commands.runOnce(() -> setGoal(newGoalPosition)) + .andThen(Commands.waitUntil(() -> atPosition(newGoalPosition))); + } } diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java index 879af3d..0149444 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java +++ b/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 positionList = + new ArrayList(); + + 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.getDegrees()) + && wrist.atAngle(position.wristAngle.getDegrees()); + } + + 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.getDegrees()); + wrist.set(currentPoint.wristAngle.getDegrees()); } } diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java b/src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java index 6acc848..049479d 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java +++ b/src/main/java/frc/robot/managers/superstructure/SuperstructurePosition.java @@ -5,9 +5,8 @@ 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; @@ -15,5 +14,15 @@ 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; + } +} diff --git a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java index e5db634..88de643 100644 --- a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java +++ b/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), diff --git a/src/main/java/frc/robot/wrist/WristSubsystem.java b/src/main/java/frc/robot/wrist/WristSubsystem.java index 5864b3e..c8b607e 100644 --- a/src/main/java/frc/robot/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/wrist/WristSubsystem.java @@ -44,7 +44,7 @@ public Command setPositionCommand(double angle) { .until(() -> atAngle(angle)); } - private boolean atAngle(double angle) { + public boolean atAngle(double angle) { return Math.abs(getWristAngle() - angle) < 1; } From 400ab03625210595205ebb03b91aa8d90498233b Mon Sep 17 00:00:00 2001 From: Owen <117956892+Owen756@users.noreply.github.com> Date: Sat, 26 Aug 2023 11:19:59 -0700 Subject: [PATCH 5/9] add wrist logging --- src/main/java/frc/robot/wrist/WristSubsystem.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/java/frc/robot/wrist/WristSubsystem.java b/src/main/java/frc/robot/wrist/WristSubsystem.java index 6b4a57a..87e57a2 100644 --- a/src/main/java/frc/robot/wrist/WristSubsystem.java +++ b/src/main/java/frc/robot/wrist/WristSubsystem.java @@ -1,4 +1,6 @@ package frc.robot.wrist; +import org.littletonrobotics.junction.Logger; + import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.RelativeEncoder; @@ -35,6 +37,15 @@ public void enabledPeriodic() { 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(Rotation2d angle) { goalAngle = angle; } From 9c40ce5a1cc593d4d55e347857e7deda2d28452e Mon Sep 17 00:00:00 2001 From: Chris <117956866+lechris07@users.noreply.github.com> Date: Sat, 26 Aug 2023 11:22:42 -0700 Subject: [PATCH 6/9] Implement shoulder subsystem (#16) * Finish shoulder subsystem * Add second motor --------- Co-authored-by: Chris <117790940+JelloSmello@users.noreply.github.com> --- .../frc/robot/shoulder/ShoulderSubsystem.java | 59 +++++++++++++++++++ .../util/scheduling/SubsystemPriority.java | 1 + 2 files changed, 60 insertions(+) create mode 100644 src/main/java/frc/robot/shoulder/ShoulderSubsystem.java diff --git a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java new file mode 100644 index 0000000..a084457 --- /dev/null +++ b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java @@ -0,0 +1,59 @@ +package frc.robot.shoulder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxPIDController; + +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 double goalAngle = 0.0; + + public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) { + super(SubsystemPriority.SHOULDER); + this.motor = motor; + this.motorFollower = motorFollower; + motorFollower.follow(motor); + encoder = motor.getEncoder(); + pid = motor.getPIDController(); + + pid.setP(5); + pid.setI(0); + pid.setD(0); + + encoder.setPosition(0); + encoder.setPositionConversionFactor(50); + } + + @Override + public void enabledPeriodic() { + pid.setReference(goalAngle / 360.0, ControlType.kSmartMotion); + } + + public void set(double angle) { + goalAngle = angle; + } + + public Command setPositionCommand(double angle) { + return run(() -> { + set(angle); + }) + .until(() -> atAngle(angle)); + } + + private boolean atAngle(double angle) { + return Math.abs(getWristAngle() - angle) < 1; + } + + private double getWristAngle() { + return encoder.getPosition() * 360.0; + } +} + diff --git a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java index e5db634..4afddc5 100644 --- a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java +++ b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java @@ -13,6 +13,7 @@ public enum SubsystemPriority { SWERVE(10), IMU(10), INTAKE(10), + SHOULDER(10), // Run localization after swerve & IMU LOCALIZATION(9), From eb760db32007135d08d0f2c9e7ed514821b13fed Mon Sep 17 00:00:00 2001 From: Chris <117790940+JelloSmello@users.noreply.github.com> Date: Sat, 26 Aug 2023 11:44:47 -0700 Subject: [PATCH 7/9] Add logger, rotation2d --- .../frc/robot/shoulder/ShoulderSubsystem.java | 36 ++++++++++++++----- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java index a084457..8174134 100644 --- a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java @@ -1,9 +1,13 @@ 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; @@ -14,7 +18,7 @@ public class ShoulderSubsystem extends LifecycleSubsystem { private RelativeEncoder encoder; private SparkMaxPIDController pid; - private double goalAngle = 0.0; + private Rotation2d goalAngle = new Rotation2d(); public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) { super(SubsystemPriority.SHOULDER); @@ -34,26 +38,42 @@ public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) { @Override public void enabledPeriodic() { - pid.setReference(goalAngle / 360.0, ControlType.kSmartMotion); + pid.setReference(goalAngle.getRotations(), ControlType.kSmartMotion); } - public void set(double angle) { +@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(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()); } } From f9d95b3de714512bfe12ab14d09d6eaa1ebca393 Mon Sep 17 00:00:00 2001 From: Jordan <117786700+jordanjcoderman@users.noreply.github.com> Date: Sat, 26 Aug 2023 11:46:12 -0700 Subject: [PATCH 8/9] Fixed compiler errors --- .../superstructure/SuperstructureManager.java | 21 ++++++++-------- .../SuperstructureMotionManager.java | 8 +++---- .../superstructure/SuperstructureState.java | 24 +++++++++++++++++++ 3 files changed, 38 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc/robot/managers/superstructure/SuperstructureState.java diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java index 6f310dc..0fbf12f 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java +++ b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java @@ -16,7 +16,7 @@ public class SuperstructureManager extends LifecycleSubsystem { 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); @@ -27,24 +27,23 @@ public SuperstructureManager(SuperstructureMotionManager motionManager, IntakeSu this.intake = intake; } - private void setGoal(SuperstructurePosition goalPosition) { - this.goalPosition = goalPosition; + private void setGoal(SuperstructureState goalState) { + this.goalState = goalState; } - public boolean atPosition(SuperstructurePosition position) { - return shoulder.atAngle(position.shoulderAngle.getDegrees()) - && wrist.atAngle(position.wristAngle.getDegrees()); + 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(goalPosition); + motionManager.set(goalState.position); } - // create a setPositionCommand method, which finishes once at goal - public Command setGoalCommand(SuperstructurePosition newGoalPosition) { - return Commands.runOnce(() -> setGoal(newGoalPosition)) - .andThen(Commands.waitUntil(() -> atPosition(newGoalPosition))); + public Command setStateCommand(SuperstructureState newGoalState) { + return Commands.runOnce(() -> setGoal(newGoalState)) + .andThen(Commands.waitUntil(() -> atGoal(newGoalState))); } } diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java index 0149444..dad9d9b 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java +++ b/src/main/java/frc/robot/managers/superstructure/SuperstructureMotionManager.java @@ -25,8 +25,8 @@ public SuperstructureMotionManager(WristSubsystem shoulder, WristSubsystem wrist } private boolean atPosition(SuperstructurePosition position) { - return shoulder.atAngle(position.shoulderAngle.getDegrees()) - && wrist.atAngle(position.wristAngle.getDegrees()); + return shoulder.atAngle(position.shoulderAngle) + && wrist.atAngle(position.wristAngle); } public void set(SuperstructurePosition newGoalPosition) { @@ -53,7 +53,7 @@ public void enabledPeriodic() { currentPoint = positionList.remove(0); } - shoulder.set(currentPoint.shoulderAngle.getDegrees()); - wrist.set(currentPoint.wristAngle.getDegrees()); + shoulder.set(currentPoint.shoulderAngle); + wrist.set(currentPoint.wristAngle); } } diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureState.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureState.java new file mode 100644 index 0000000..da94e96 --- /dev/null +++ b/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); + } +} From ed3a4ab1b7ee83d2d8cf54dce706221f35f151ae Mon Sep 17 00:00:00 2001 From: Chris <117790940+JelloSmello@users.noreply.github.com> Date: Sat, 26 Aug 2023 11:46:10 -0700 Subject: [PATCH 9/9] Add current limit --- src/main/java/frc/robot/shoulder/ShoulderSubsystem.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java index 8174134..1701584 100644 --- a/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java +++ b/src/main/java/frc/robot/shoulder/ShoulderSubsystem.java @@ -27,6 +27,8 @@ public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) { motorFollower.follow(motor); encoder = motor.getEncoder(); pid = motor.getPIDController(); + motor.setSmartCurrentLimit(35); + motorFollower.setSmartCurrentLimit(35); pid.setP(5); pid.setI(0);