diff --git a/2023-Robot/src/main/java/frc/team670/mustanglib b/2023-Robot/src/main/java/frc/team670/mustanglib index b442c1a1..15777234 160000 --- a/2023-Robot/src/main/java/frc/team670/mustanglib +++ b/2023-Robot/src/main/java/frc/team670/mustanglib @@ -1 +1 @@ -Subproject commit b442c1a12aff127104e9dfb2bddc22aea3581c99 +Subproject commit 157772347d3bfc7800e53d4bd3da5e478ad9c4e2 diff --git a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java index bfa1a5e8..be5fc851 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java +++ b/2023-Robot/src/main/java/frc/team670/robot/RobotContainer.java @@ -36,7 +36,7 @@ public class RobotContainer extends RobotContainerBase { private final Vision vision = new Vision(pd); private final DriveBase driveBase = new DriveBase(getDriverController()); private final Arm arm = new Arm(); - private final Claw claw = new Claw(arm); + private final Claw claw = new Claw(); private static OI oi = new OI(); private Notifier updateArbitraryFeedForwards; diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveElbow.java b/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveElbow.java index 5544f277..07153684 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveElbow.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveElbow.java @@ -30,7 +30,7 @@ public ManualMoveElbow(Arm arm, MustangController controller) { @Override public void execute() { - arm.setElbowOffset(arm.getElbow().getOffset() + controller.getLeftY()); + arm.adjustElbowOffset(-controller.getLeftY()); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveShoulder.java b/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveShoulder.java index 708b0b32..4a6ad68c 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveShoulder.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/arm/ManualMoveShoulder.java @@ -30,7 +30,7 @@ public ManualMoveShoulder(Arm arm, MustangController controller) { @Override public void execute() { - arm.setShoulderOffset(arm.getShoulder().getOffset() + controller.getRightY()); + arm.adjustShoulderOffset(controller.getRightY()); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/arm/MoveToTarget.java b/2023-Robot/src/main/java/frc/team670/robot/commands/arm/MoveToTarget.java index ac6183ff..ba2d52ef 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/arm/MoveToTarget.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/arm/MoveToTarget.java @@ -14,7 +14,9 @@ import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; import frc.team670.mustanglib.utils.Logger; import frc.team670.robot.commands.claw.ClawIntake; +import frc.team670.robot.commands.routines.ClawToggle; import frc.team670.robot.subsystems.Claw; +import frc.team670.robot.subsystems.Claw.Status; import frc.team670.robot.subsystems.arm.Arm; import frc.team670.robot.subsystems.arm.ArmState; @@ -39,18 +41,12 @@ public class MoveToTarget extends CommandGroupBase implements MustangCommand { private Map healthReqs; private ArmState target; private Arm arm; - private Claw claw; private final List m_commands = new ArrayList<>(); private int m_currentCommandIndex = -1; private boolean m_runWhenDisabled = true; private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelSelf; - public MoveToTarget(Arm arm, Claw claw, ArmState target) { - this(arm, target); - this.claw = claw; - } - public MoveToTarget(Arm arm, ArmState target) { healthReqs = new HashMap(); healthReqs.put(arm, HealthState.GREEN); @@ -69,12 +65,10 @@ public void initialize() { // 3) then call move directly to target for each of those returned paths m_commands.clear(); ArmState[] path = Arm.getValidPath(arm.getTargetState(), target); + for (int i = 1; i < path.length; i++) { addCommands(new MoveDirectlyToTarget(arm, path[i])); } - if (target != ArmState.STOWED && claw != null) { - addCommands(new ClawIntake(claw)); - } m_currentCommandIndex = 0; diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/claw/ClawIntake.java b/2023-Robot/src/main/java/frc/team670/robot/commands/claw/ClawIntake.java index 8d0d1de1..b753cac1 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/claw/ClawIntake.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/claw/ClawIntake.java @@ -4,6 +4,8 @@ import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; import frc.team670.mustanglib.commands.MustangCommand; import java.util.Map; + +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.team670.robot.subsystems.Claw; import frc.team670.robot.subsystems.Claw.Status; @@ -12,19 +14,23 @@ * @author Tarini, Samanyu and Ishaan */ -public class ClawIntake extends InstantCommand implements MustangCommand { +public class ClawIntake extends CommandBase implements MustangCommand { private Claw claw; - - public ClawIntake(Claw claw) { + private boolean waitTillFull; + public ClawIntake(Claw claw,boolean waitTillFull) { this.claw = claw; + this.waitTillFull=waitTillFull; addRequirements(claw); } @Override - public void execute() { + public void initialize(){ claw.setStatus(Status.INTAKING); } - + @Override + public boolean isFinished(){ + return !waitTillFull||claw.isFull(); + } @Override public Map getHealthRequirements() { return null; diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java index a4d3d369..6e9fb6dc 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/drivebase/MoveToPose.java @@ -44,12 +44,12 @@ public MoveToPose(DriveBase driveBase, Pose2d goalPose) { } // public MoveToPose(DriveBase driveBase, Pose2d goalPose) { - // this.driveBase = driveBase; - // path = null; - // this.goalPose = goalPose; - // this.healthReqs = new HashMap(); - // this.healthReqs.put(driveBase, HealthState.GREEN); - // addRequirements(driveBase); + // this.driveBase = driveBase; + // path = null; + // this.goalPose = goalPose; + // this.healthReqs = new HashMap(); + // this.healthReqs.put(driveBase, HealthState.GREEN); + // addRequirements(driveBase); // } @Override @@ -59,8 +59,12 @@ public Map getHealthRequirements() { @Override public void initialize() { - goalPose = FieldConstants.allianceFlip(goalPose).transformBy(new Transform2d(FieldConstants.allianceFlip(new Translation2d(RobotConstants.DRIVEBASE_WIDTH + 0.1, 0)), FieldConstants.getRobotFacingRotation())); - SmartDashboard.putString("GOAL POSE", String.format("%f, %f, %f degrees", goalPose.getX(), goalPose.getY(), goalPose.getRotation().getDegrees())); + goalPose = FieldConstants.allianceFlip(goalPose) + .transformBy(new Transform2d( + FieldConstants.allianceFlip(new Translation2d(RobotConstants.DRIVEBASE_WIDTH + 0.1, 0)), + FieldConstants.getRobotFacingRotation())); + SmartDashboard.putString("GOAL POSE", String.format("%f, %f, %f degrees", goalPose.getX(), goalPose.getY(), + goalPose.getRotation().getDegrees())); PathPlannerTrajectory traj = PathPlanner.generatePath(new PathConstraints(1, 0.5), calcStartPoint(), calcEndPoint(goalPose)); driveBase.getPoseEstimator().addTrajectory(traj); @@ -77,15 +81,14 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { if (interrupted) { - pathDrivingCommand.cancel(); + pathDrivingCommand.cancel(); driveBase.getPoseEstimator().removeTrajectory(); - } + } - driveBase.stop(); + driveBase.stop(); // driveBase.getPoseEstimator().removeTrajectory(); } - private PathPoint calcStartPoint() { return new PathPoint(driveBase.getPoseEstimator().getCurrentPose().getTranslation(), new Rotation2d(), driveBase.getGyroscopeRotation()); diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java index 223676ca..9341f9e3 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/ConeCube.java @@ -20,8 +20,10 @@ import frc.team670.robot.commands.arm.MoveToTarget; import frc.team670.robot.commands.claw.ClawEject; import frc.team670.robot.commands.claw.ClawIntake; +import frc.team670.robot.commands.routines.ClawToggle; import frc.team670.robot.subsystems.Claw; import frc.team670.robot.subsystems.DriveBase; +import frc.team670.robot.subsystems.Claw.Status; import frc.team670.robot.subsystems.arm.Arm; import frc.team670.robot.subsystems.arm.ArmState; @@ -33,40 +35,39 @@ public Map getHealthRequirements() { return new HashMap(); } - public ConeCube(DriveBase driveBase, Claw claw, Arm arm, String pathName) { this.pathName = pathName; List trajectoryGroup = PathPlanner.loadPathGroup(pathName, 2.0, 1.0); PIDConstants PID_translation = new PIDConstants(1.0, 0, 0); - PIDConstants PID_theta = new PIDConstants(1.0, 0, 0); + PIDConstants PID_theta = new PIDConstants(1.0, 0, 0); driveBase.resetOdometry(trajectoryGroup.get(0).getInitialHolonomicPose()); HashMap eventMap = new HashMap<>(); // eventMap stuff - eventMap.put("clawIntake1", new ClawIntake(claw)); + eventMap.put("clawIntake1", new ClawIntake(claw, false)); eventMap.put("moveToHigh1", new MoveToTarget(arm, ArmState.SCORE_HIGH)); eventMap.put("clawEject1", new ClawEject(claw)); eventMap.put("moveToBackward", new MoveToTarget(arm, ArmState.BACKWARD_GROUND)); - eventMap.put("clawIntake2", new ClawIntake(claw)); + eventMap.put("clawIntake2", new ClawIntake(claw, false)); eventMap.put("moveToHigh2", new MoveToTarget(arm, ArmState.SCORE_HIGH)); - eventMap.put("clawEject2", new ClawEject(claw)); - eventMap.put("moveToStowed", new MoveToTarget(arm, ArmState.STOWED)); - + eventMap.put("clawEject2", new ClawToggle(claw, arm, Status.EJECTING)); + SwerveDriveKinematics driveBaseKinematics = driveBase.getSwerveKinematics(); SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase.getPoseEstimator()::getCurrentPose, driveBase::resetOdometry, driveBaseKinematics, PID_translation, PID_theta, - driveBase::setModuleStates, eventMap, false, new Subsystem[] {driveBase}); + driveBase::setModuleStates, eventMap, false, new Subsystem[] { driveBase }); CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); addCommands(fullAuto); } - // not exactly needed right now bc markers and events are the same for both sides (maybe + // not exactly needed right now bc markers and events are the same for both + // sides (maybe // utilized later) // public HashMap initialzeEventMap() { diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java index e5686fc1..b06601db 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/pathplanner/CubeEngage.java @@ -42,11 +42,10 @@ public CubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { Map eventMap = new HashMap<>(); - eventMap.put("clawIntake1", new ClawIntake(claw)); eventMap.put("moveToHigh", new MoveToTarget(arm, ArmState.SCORE_HIGH)); eventMap.put("clawEject", new ClawEject(claw)); eventMap.put("moveToGround", new MoveToTarget(arm, ArmState.BACKWARD_GROUND)); - eventMap.put("clawIntake2", new ClawIntake(claw)); + eventMap.put("clawIntake2", new ClawIntake(claw, false)); eventMap.put("autoLevel", new NonPidAutoLevel(driveBase, false)); // regardless of what side // (right/left) you are // on, markers are the @@ -56,7 +55,7 @@ public CubeEngage(DriveBase driveBase, Claw claw, Arm arm, String pathName) { SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(driveBase::getOdometerPose, driveBase::resetOdometry, driveBaseKinematics, PID_translation, PID_theta, - driveBase::setModuleStates, eventMap, false, new Subsystem[] {driveBase}); + driveBase::setModuleStates, eventMap, false, new Subsystem[] { driveBase }); CommandBase fullAuto = autoBuilder.fullAuto(trajectoryGroup); diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/routines/ClawToggle.java b/2023-Robot/src/main/java/frc/team670/robot/commands/routines/ClawToggle.java new file mode 100644 index 00000000..00bf7c50 --- /dev/null +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/routines/ClawToggle.java @@ -0,0 +1,144 @@ +package frc.team670.robot.commands.routines; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandGroupBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.team670.mustanglib.commands.MustangCommand; +import frc.team670.mustanglib.subsystems.MustangSubsystemBase; +import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; +import frc.team670.mustanglib.utils.Logger; +import frc.team670.robot.commands.arm.MoveToTarget; +import frc.team670.robot.commands.claw.ClawEject; +import frc.team670.robot.commands.claw.ClawIdle; +import frc.team670.robot.commands.claw.ClawIntake; +import frc.team670.robot.subsystems.Claw; +import frc.team670.robot.subsystems.Claw.Status; + +import frc.team670.robot.subsystems.arm.Arm; +import frc.team670.robot.subsystems.arm.ArmState; + +//An implementation of an automatic claw decision program. To ensure that dynamic decisions are made in toggle, we have to make decisions on init. As such, we copied and pasted a clas from wpilib +public class ClawToggle extends CommandGroupBase implements MustangCommand { + private Map healthReqs; + private Status status; + private Claw claw; + private Arm arm; + + public ClawToggle(Claw claw, Arm arm, Status status) { + healthReqs = new HashMap(); + healthReqs.put(claw, HealthState.GREEN); + this.claw = claw; + this.arm = arm; + } + + private final List m_commands = new ArrayList<>(); + private int m_currentCommandIndex = -1; + private boolean m_runWhenDisabled = true; + private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelSelf; + + @Override + public void initialize() { + Logger.consoleLog("Ran ToggleClaw with the status " + status.toString()); + m_commands.clear(); + if (status == Status.EJECTING) { + addCommands(new ClawEject(claw), new MoveToTarget(arm, ArmState.STOWED)); + healthReqs.put(arm, HealthState.GREEN); + } else if (status == Status.INTAKING) { + if (!claw.isFull()) { + addCommands(new ClawIntake(claw, true), new MoveToTarget(arm, ArmState.STOWED)); + healthReqs.put(arm, HealthState.GREEN); + } else { + addCommands(new ClawIntake(claw, false)); + } + + } else if (status == Status.IDLE) { + addCommands(new ClawIdle(claw), new MoveToTarget(arm, ArmState.STOWED)); + healthReqs.put(arm, HealthState.GREEN); + } + m_currentCommandIndex = 0; + if (!m_commands.isEmpty()) { + m_commands.get(0).initialize(); + } + } + + @Override + public Map getHealthRequirements() { + return healthReqs; + } + + @Override + public final void addCommands(Command... commands) { + if (m_currentCommandIndex != -1) { + throw new IllegalStateException( + "Commands cannot be added to a composition while it's running"); + } + + CommandScheduler.getInstance().registerComposedCommands(commands); + + for (Command command : commands) { + m_commands.add(command); + m_requirements.addAll(command.getRequirements()); + m_runWhenDisabled &= command.runsWhenDisabled(); + if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) { + m_interruptBehavior = InterruptionBehavior.kCancelSelf; + } + } + } + + @Override + public final void execute() { + if (m_commands.isEmpty()) { + return; + } + + Command currentCommand = m_commands.get(m_currentCommandIndex); + + currentCommand.execute(); + if (currentCommand.isFinished()) { + currentCommand.end(false); + m_currentCommandIndex++; + if (m_currentCommandIndex < m_commands.size()) { + m_commands.get(m_currentCommandIndex).initialize(); + } + } + } + + @Override + public final void end(boolean interrupted) { + if (interrupted + && !m_commands.isEmpty() + && m_currentCommandIndex > -1 + && m_currentCommandIndex < m_commands.size()) { + m_commands.get(m_currentCommandIndex).end(true); + } + m_currentCommandIndex = -1; + } + + @Override + public final boolean isFinished() { + return m_currentCommandIndex == m_commands.size(); + } + + @Override + public boolean runsWhenDisabled() { + return m_runWhenDisabled; + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + return m_interruptBehavior; + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + + builder.addIntegerProperty("index", () -> m_currentCommandIndex, null); + } +} diff --git a/2023-Robot/src/main/java/frc/team670/robot/commands/routines/MoveArm.java b/2023-Robot/src/main/java/frc/team670/robot/commands/routines/MoveArm.java new file mode 100644 index 00000000..732b4932 --- /dev/null +++ b/2023-Robot/src/main/java/frc/team670/robot/commands/routines/MoveArm.java @@ -0,0 +1,35 @@ +package frc.team670.robot.commands.routines; + +import java.util.HashMap; +import java.util.Map; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.team670.mustanglib.commands.MustangCommand; +import frc.team670.mustanglib.subsystems.MustangSubsystemBase; +import frc.team670.mustanglib.subsystems.MustangSubsystemBase.HealthState; +import frc.team670.robot.commands.arm.MoveToTarget; +import frc.team670.robot.commands.claw.ClawIntake; +import frc.team670.robot.subsystems.Claw; +import frc.team670.robot.subsystems.Claw.Status; +import frc.team670.robot.subsystems.arm.Arm; +import frc.team670.robot.subsystems.arm.ArmState; + +public class MoveArm extends SequentialCommandGroup implements MustangCommand { + private Map healthReqs; + + public MoveArm(Arm arm, Claw claw, ArmState target) { + healthReqs = new HashMap(); + healthReqs.put(arm, HealthState.GREEN); + healthReqs.put(claw, HealthState.GREEN); + addCommands(new ClawIntake(claw, false)); + addCommands(new MoveToTarget(arm, target)); + addCommands(new ClawToggle(claw, arm, Status.INTAKING)); + } + + @Override + public Map getHealthRequirements() { + return healthReqs; + } + +} diff --git a/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java b/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java index 25091ed5..2d02cada 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java +++ b/2023-Robot/src/main/java/frc/team670/robot/constants/OI.java @@ -21,12 +21,15 @@ import frc.team670.robot.subsystems.Claw; import frc.team670.robot.subsystems.DriveBase; import frc.team670.robot.subsystems.Vision; +import frc.team670.robot.subsystems.Claw.Status; import frc.team670.robot.commands.arm.MoveToTarget; import frc.team670.robot.commands.arm.ManualMoveElbow; import frc.team670.robot.commands.arm.ManualMoveShoulder; import frc.team670.robot.commands.claw.ClawEject; import frc.team670.robot.commands.claw.ClawIntake; import frc.team670.robot.commands.drivebase.TurnToAngle; +import frc.team670.robot.commands.routines.ClawToggle; +import frc.team670.robot.commands.routines.MoveArm; import frc.team670.robot.commands.claw.ClawIdle; import frc.team670.robot.subsystems.arm.Arm; import frc.team670.robot.subsystems.arm.ArmState; @@ -113,24 +116,24 @@ public void configureButtonBindings(MustangSubsystemBase... subsystemBases) { // true)); // //arm movement commands - backward.onTrue(new MoveToTarget(arm, ArmState.BACKWARD_GROUND)); - scoreMidR.onTrue(new MoveToTarget(arm, ArmState.SCORE_MID)); - scoreMidL.onTrue(new MoveToTarget(arm, ArmState.SCORE_MID)); - scoreHigh.onTrue(new MoveToTarget(arm, ArmState.SCORE_HIGH)); - stow.onTrue(new MoveToTarget(arm, ArmState.STOWED)); + backward.onTrue(new MoveArm(arm, claw, ArmState.BACKWARD_GROUND)); + scoreMidR.onTrue(new MoveArm(arm, claw, ArmState.SCORE_MID)); + scoreMidL.onTrue(new MoveArm(arm, claw, ArmState.SCORE_MID)); + scoreHigh.onTrue(new MoveArm(arm, claw, ArmState.SCORE_HIGH)); + stow.onTrue(new MoveArm(arm, claw, ArmState.STOWED)); manualShoulderControl.onTrue(new ManualMoveShoulder(arm, operatorController)); manualElbowControl.onTrue(new ManualMoveElbow(arm, operatorController)); // Claw control commands - clawSuckOp.onTrue(new ClawIntake(claw)); - clawEjectOp.onTrue(new ClawEject(claw)); - clawSuckDriver.onTrue(new ClawIntake(claw)); - clawEjectDriver.onTrue(new ClawEject(claw)); - clawIdle.onTrue(new ClawIdle(claw)); + clawSuckOp.onTrue(new ClawToggle(claw, arm, Status.INTAKING)); + clawEjectOp.onTrue(new ClawToggle(claw, arm, Status.EJECTING)); + clawSuckDriver.onTrue(new ClawToggle(claw, arm, Status.INTAKING)); + clawEjectDriver.onTrue(new ClawToggle(claw, arm, Status.EJECTING)); + clawIdle.onTrue(new ClawToggle(claw, arm, Status.IDLE)); // Rotate to angle TODO: temporary until vision is finished - rotateTo0.onTrue(new TurnToAngle(driveBase, 0, false, driverController)); - rotateTo90.onTrue(new TurnToAngle(driveBase, 90, false, driverController)); - rotateTo180.onTrue(new TurnToAngle(driveBase, 180, false, driverController)); - rotateTo270.onTrue(new TurnToAngle(driveBase, 270, false, driverController)); + // rotateTo0.onTrue(new TurnToAngle(driveBase, 0, false, driverController)); + // rotateTo90.onTrue(new TurnToAngle(driveBase, 90, false, driverController)); + // rotateTo180.onTrue(new TurnToAngle(driveBase, 180, false, driverController)); + // rotateTo270.onTrue(new TurnToAngle(driveBase, 270, false, driverController)); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java index 8a3e1e54..3a88e5e4 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java +++ b/2023-Robot/src/main/java/frc/team670/robot/constants/RobotConstants.java @@ -20,256 +20,258 @@ import frc.team670.mustanglib.constants.RobotConstantsBase; /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared * globally (i.e. public static). Do not put anything functional in this class. * *

- * It is advised to statically import this class (or one of its inner classes) wherever the + * It is advised to statically import this class (or one of its inner classes) + * wherever the * constants are needed, to reduce verbosity. */ public final class RobotConstants extends RobotConstantsBase { - public static final String MAC_ADDRESS = getMACAddress(); + public static final String MAC_ADDRESS = getMACAddress(); + // Set your team number using the WPILib extension's "Set Team Number" action. + // 1) Set all of the *_ANGLE_OFFSET constants to -Math.toRadians(0.0). + // 2) Deploy the code to your robot. + // NOTE: The robot isn't drivable quite yet, we still have to setup the module + // offsets + // 3) Turn the robot on its side and align all the wheels so they are facing in + // the forwards direction. + // NOTE: The wheels will be pointed forwards (not backwards) when modules are + // turned so the large bevel gears are towards the LEFT side of the robot. When + // aligning the wheels they must be as straight as possible. It is recommended + // to use a long strait edge such as a piece of 2x1 in order to make the wheels + // straight. + // 4) Record the angles of each module using the angle put onto Shuffleboard. + // The values are named Front Left Module Angle, Front Right Module Angle, etc. + // 5) Set the values of the *_ANGLE_OFFSET to -Math.toRadians() + // NOTE: All angles must be in degrees. + // 6) Re-deploy and try to drive the robot forwards. All the wheels should stay + // parallel to each other. If not go back to step 3. + // 7) Make sure all the wheels are spinning in the correct direction. If not, + // add 180 degrees to the offset of each wheel that is spinning in the incorrect + // direction. i.e -Math.toRadians( + 180.0). + public static Map> hardwareSpecificConstants = Map.ofEntries( + entry("00:80:2F:34:0B:07", Map.ofEntries( // Mac address from 670_bench + entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(228.85)), + entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(228.3)), + entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(2.37)), + entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(30.2)), + entry("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL", 0.957), + entry("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL", 0.494))), + entry("00:80:2F:24:4A:34", Map.ofEntries( // The mac address is from 670_MadMax + entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(292.5)), + entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(232.91)), + entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(352.35)), + entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(136.67)), + entry("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL", 0.0), + entry("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL", 0.0))), + entry("00:80:2F:22:B4:F6", Map.ofEntries( // The mac address is from 670_WCD (test + // bench) + entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(292.5)), + entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(232.91)), + entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(352.35)), + entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(136.67)))) + ); - // Set your team number using the WPILib extension's "Set Team Number" action. - // 1) Set all of the *_ANGLE_OFFSET constants to -Math.toRadians(0.0). - // 2) Deploy the code to your robot. - // NOTE: The robot isn't drivable quite yet, we still have to setup the module - // offsets - // 3) Turn the robot on its side and align all the wheels so they are facing in - // the forwards direction. - // NOTE: The wheels will be pointed forwards (not backwards) when modules are - // turned so the large bevel gears are towards the LEFT side of the robot. When - // aligning the wheels they must be as straight as possible. It is recommended - // to use a long strait edge such as a piece of 2x1 in order to make the wheels - // straight. - // 4) Record the angles of each module using the angle put onto Shuffleboard. - // The values are named Front Left Module Angle, Front Right Module Angle, etc. - // 5) Set the values of the *_ANGLE_OFFSET to -Math.toRadians() - // NOTE: All angles must be in degrees. - // 6) Re-deploy and try to drive the robot forwards. All the wheels should stay - // parallel to each other. If not go back to step 3. - // 7) Make sure all the wheels are spinning in the correct direction. If not, - // add 180 degrees to the offset of each wheel that is spinning in the incorrect - // direction. i.e -Math.toRadians( + 180.0). - public static Map> hardwareSpecificConstants = Map.ofEntries( - entry("00:80:2F:34:0B:07", Map.ofEntries( // Mac address from 670_bench - entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(228.85)), - entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(228.3)), - entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(2.37)), - entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(30.2)), - entry("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL", 0.957), - entry("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL", 0.494))), - entry("00:80:2F:24:4A:34", Map.ofEntries( // The mac address is from 670_MadMax - entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(292.5)), - entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(232.91)), - entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(352.35)), - entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(136.67)), - entry("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL", 0.0), - entry("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL", 0.0))), - entry("00:80:2F:22:B4:F6", Map.ofEntries( // The mac address is from 670_WCD (test - // bench) - entry("BACK_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(292.5)), - entry("BACK_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(232.91)), - entry("FRONT_RIGHT_MODULE_STEER_OFFSET", -Math.toRadians(352.35)), - entry("FRONT_LEFT_MODULE_STEER_OFFSET", -Math.toRadians(136.67)))) + /** + * The left-to-right distance between the drivetrain wheels + * + * Should be measured from center to center. + */ - ); + public static final double DRIVEBASE_WIDTH = Units.inchesToMeters(36); - /** - * The left-to-right distance between the drivetrain wheels - * - * Should be measured from center to center. - */ + public static final double DRIVETRAIN_TRACKWIDTH_METERS = 0.6096; - public static final double DRIVEBASE_WIDTH = Units.inchesToMeters(36); + /** + * The front-to-back distance between the drivetrain wheels. + * + * Should be measured from center to center. + */ + public static final double DRIVETRAIN_WHEELBASE_METERS = 0.6096; - public static final double DRIVETRAIN_TRACKWIDTH_METERS = 0.6096; + public static final double LIMIT = 1.0; - /** - * The front-to-back distance between the drivetrain wheels. - * - * Should be measured from center to center. - */ - public static final double DRIVETRAIN_WHEELBASE_METERS = 0.6096; + public static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 25; // Set back right module drive motor + // ID + public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 24; // Set back right module steer motor + // ID + public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 34; // Set back right steer encoder ID + public static final double BACK_RIGHT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) + .get("BACK_RIGHT_MODULE_STEER_OFFSET"); // Measure + // and + // set + // back + // right + // steer + // offset - public static final double LIMIT = 1.0; + public static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 27; // Set back left drive motor ID + public static final int BACK_LEFT_MODULE_STEER_MOTOR = 26; // Set back left steer motor ID + public static final int BACK_LEFT_MODULE_STEER_ENCODER = 36; // Set back left steer encoder ID + public static final double BACK_LEFT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) + .get("BACK_LEFT_MODULE_STEER_OFFSET"); // Measure + // and + // set + // back + // left + // steer + // offset - public static final int BACK_RIGHT_MODULE_DRIVE_MOTOR = 25; // Set back right module drive motor - // ID - public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 24; // Set back right module steer motor - // ID - public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 34; // Set back right steer encoder ID - public static final double BACK_RIGHT_MODULE_STEER_OFFSET = - hardwareSpecificConstants.get(MAC_ADDRESS).get("BACK_RIGHT_MODULE_STEER_OFFSET"); // Measure - // and - // set - // back - // right - // steer - // offset + public static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 23; // Set front right drive motor ID + public static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 22; // Set front right steer motor ID + public static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 32; // Set front right steer encoder + // ID + public static final double FRONT_RIGHT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) + .get("FRONT_RIGHT_MODULE_STEER_OFFSET"); // Measure + // and + // set + // back + // left + // steer + // offset + // Measure and set front right steer offset - public static final int BACK_LEFT_MODULE_DRIVE_MOTOR = 27; // Set back left drive motor ID - public static final int BACK_LEFT_MODULE_STEER_MOTOR = 26; // Set back left steer motor ID - public static final int BACK_LEFT_MODULE_STEER_ENCODER = 36; // Set back left steer encoder ID - public static final double BACK_LEFT_MODULE_STEER_OFFSET = - hardwareSpecificConstants.get(MAC_ADDRESS).get("BACK_LEFT_MODULE_STEER_OFFSET"); // Measure - // and - // set - // back - // left - // steer - // offset + public static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 21; // Set front left drive motor ID + public static final int FRONT_LEFT_MODULE_STEER_MOTOR = 20; // Set front left steer motor ID + public static final int FRONT_LEFT_MODULE_STEER_ENCODER = 30; // Set front left steer encoder ID + public static final double FRONT_LEFT_MODULE_STEER_OFFSET = hardwareSpecificConstants.get(MAC_ADDRESS) + .get("FRONT_LEFT_MODULE_STEER_OFFSET"); // Measure + // and + // set + // front + // left + // steer + // offset - public static final int FRONT_RIGHT_MODULE_DRIVE_MOTOR = 23; // Set front right drive motor ID - public static final int FRONT_RIGHT_MODULE_STEER_MOTOR = 22; // Set front right steer motor ID - public static final int FRONT_RIGHT_MODULE_STEER_ENCODER = 32; // Set front right steer encoder - // ID - public static final double FRONT_RIGHT_MODULE_STEER_OFFSET = - hardwareSpecificConstants.get(MAC_ADDRESS).get("FRONT_RIGHT_MODULE_STEER_OFFSET"); // Measure - // and - // set - // back - // left - // steer - // offset - // Measure and set front right steer offset + public final static SerialPort.Port NAVX_PORT = SerialPort.Port.kMXP; + public static final double kMaxSpeedMetersPerSecond = 2; + public static final double kMaxAccelerationMetersPerSecondSquared = 2; + public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI * 16; + public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI * 16; - public static final int FRONT_LEFT_MODULE_DRIVE_MOTOR = 21; // Set front left drive motor ID - public static final int FRONT_LEFT_MODULE_STEER_MOTOR = 20; // Set front left steer motor ID - public static final int FRONT_LEFT_MODULE_STEER_ENCODER = 30; // Set front left steer encoder ID - public static final double FRONT_LEFT_MODULE_STEER_OFFSET = - hardwareSpecificConstants.get(MAC_ADDRESS).get("FRONT_LEFT_MODULE_STEER_OFFSET"); // Measure - // and - // set - // front - // left - // steer - // offset + // TODO: TUNE PID CONTROLLERS AND + public static final PIDController xController = new PIDController(3, 0, 0); + public static final PIDController yController = new PIDController(3, 0, 0); + public static final PIDController thetaController = new PIDController(0.2, 0, 0); - public final static SerialPort.Port NAVX_PORT = SerialPort.Port.kMXP; - public static final double kMaxSpeedMetersPerSecond = 2; - public static final double kMaxAccelerationMetersPerSecondSquared = 2; - public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI * 16; - public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI * 16; + // vision - // TODO: TUNE PID CONTROLLERS AND - public static final PIDController xController = new PIDController(3, 0, 0); - public static final PIDController yController = new PIDController(3, 0, 0); - public static final PIDController thetaController = new PIDController(0.2, 0, 0); + // public static final String VISION_CAMERA_NAME = "Arducam_OV9281_USB_Camera"; + // public static final String VISION_CAMERA_NAME = "Arducam_A"; + public static final String VISION_CAMERA_NAME = "Arducam_B"; + public static final Transform3d CAMERA_OFFSET = new Transform3d(new Translation3d(0, 0, 0), + new Rotation3d(Math.PI / 2.0, 0, Math.PI)); + // new Transform2d(new Translation2d(0, 0), new Rotation2d(0)); // TODO: changed + // when camera + // actually mounted, may need to change based on robot + // for actual specs + public static final double LOCKED_ON_ERROR_X = 0.3; // TODO: test what angles are appropriate + // for grabbing + public static final double LOCKED_ON_ERROR_Y = 0.3; + public static final double LOCKED_ON_ERROR_DEGREES = 10; - // vision + // Everything below is copied from 2022 robot + public static final double kTrackwidthMeters = 0.702; - // public static final String VISION_CAMERA_NAME = "Arducam_OV9281_USB_Camera"; - // public static final String VISION_CAMERA_NAME = "Arducam_A"; - public static final String VISION_CAMERA_NAME = "Arducam_B"; - public static final Transform3d CAMERA_OFFSET = - new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(Math.PI / 2.0, 0, Math.PI)); - // new Transform2d(new Translation2d(0, 0), new Rotation2d(0)); // TODO: changed when camera - // actually mounted, may need to change based on robot - // for actual specs - public static final double LOCKED_ON_ERROR_X = 0.3; // TODO: test what angles are appropriate - // for grabbing - public static final double LOCKED_ON_ERROR_Y = 0.3; - public static final double LOCKED_ON_ERROR_DEGREES = 10; + // Arm + public static final double ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL = hardwareSpecificConstants.get(MAC_ADDRESS) + .get("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL"); + public static final double SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL = hardwareSpecificConstants.get(MAC_ADDRESS) + .get("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL"); - // Everything below is copied from 2022 robot - public static final double kTrackwidthMeters = 0.702; + public static final int ELBOW_GEAR_RATIO = 75; + public static final int ELBOW_SOFT_LIMIT_MIN = 20; + public static final int ELBOW_SOFT_LIMIT_MAX = 340; + public static final double ELBOW_ARBITRARY_FF = 0.8; + public static final int SHOULDER_GEAR_RATIO = 96; + public static final int SHOULDER_SOFT_LIMIT_MIN = 60; + public static final int SHOULDER_SOFT_LIMIT_MAX = 300; + public static final double SHOULDER_ARBITRARY_FF = 0.5; + public static final double ARM_MAX_XCM = armXCM(90, 180); + public static final double ELBOW_MAX_OVERRIDE_DEGREES = 15; + public static final double SHOULDER_MAX_OVERRIDE_DEGREES = 15; + public static final double SHOULDER_LENGTH_INCHES = 25; + public static final double ELBOW_LENGTH_INCHES = 35; + public static final double SHOULDER_TO_ELBOW_MASS_LB = 6.5; + public static final double ELBOW_TO_CLAW_MASS_LB = 2; + public static final double CLAW_MASS_LB = 6; - // Arm - public static final double ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL = - hardwareSpecificConstants.get(MAC_ADDRESS).get("ELBOW_ABSOLUTE_ENCODER_AT_VERTICAL"); - public static final double SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL = - hardwareSpecificConstants.get(MAC_ADDRESS).get("SHOULDER_ABSOLUTE_ENCODER_AT_VERTICAL"); + // claw constants + public static final double CLAW_ROLLING_SPEED = 0.3; + public static final double CLAW_EJECTING_SPEED = -0.6; + public static final double CLAW_CURRENT_MAX = 24.0; + public static final double CLAW_IDLE_SPEED = 0.05; + public static final double CLAW_EJECT_PERIODICS = 25; - public static final int ELBOW_GEAR_RATIO = 75; - public static final int ELBOW_SOFT_LIMIT_MIN = 20; - public static final int ELBOW_SOFT_LIMIT_MAX = 340; - public static final double ELBOW_ARBITRARY_FF = 0.8; - public static final int SHOULDER_GEAR_RATIO = 96; - public static final int SHOULDER_SOFT_LIMIT_MIN = 60; - public static final int SHOULDER_SOFT_LIMIT_MAX = 300; - public static final double SHOULDER_ARBITRARY_FF = 0.5; - public static final double ARM_MAX_XCM = armXCM(90, 180); - public static final double ELBOW_MAX_OVERRIDE_DEGREES = 15; - public static final double SHOULDER_MAX_OVERRIDE_DEGREES = 15; - public static final double SHOULDER_LENGTH_INCHES = 25; - public static final double ELBOW_LENGTH_INCHES = 35; - public static final double SHOULDER_TO_ELBOW_MASS_LB = 6.5; - public static final double ELBOW_TO_CLAW_MASS_LB = 2; - public static final double CLAW_MASS_LB = 6; + public static final int kTimeoutMs = 0; + public static final double leftKsVolts = 0.4; + public static final double leftKvVoltSecondsPerMeter = 2.1; + public static final double leftKaVoltSecondsSquaredPerMeter = 0.15; + public static final double rightKsVolts = leftKsVolts; + public static final double rightKvVoltSecondsPerMeter = leftKvVoltSecondsPerMeter; + public static final double rightKaVoltSecondsSquaredPerMeter = leftKaVoltSecondsSquaredPerMeter; + public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics( + kTrackwidthMeters); - // claw constants - public static final double CLAW_ROLLING_SPEED = 0.3; - public static final double CLAW_EJECTING_SPEED = -0.6; - public static final double CLAW_CURRENT_MAX = 25.0; - public static final double CLAW_IDLE_SPEED = 0.05; + public static final PathConstraints kAutoPathConstraints = new PathConstraints(kMaxSpeedMetersPerSecond, + kMaxAccelerationMetersPerSecondSquared); - public static final int kTimeoutMs = 0; - public static final double leftKsVolts = 0.4; - public static final double leftKvVoltSecondsPerMeter = 2.1; - public static final double leftKaVoltSecondsSquaredPerMeter = 0.15; - public static final double rightKsVolts = leftKsVolts; - public static final double rightKvVoltSecondsPerMeter = leftKvVoltSecondsPerMeter; - public static final double rightKaVoltSecondsSquaredPerMeter = leftKaVoltSecondsSquaredPerMeter; - public static final DifferentialDriveKinematics kDriveKinematics = - new DifferentialDriveKinematics(kTrackwidthMeters); - - public static final PathConstraints kAutoPathConstraints = - new PathConstraints(kMaxSpeedMetersPerSecond, kMaxAccelerationMetersPerSecondSquared); - - - /** - * This is code from Poofs 2022 - * - * @return the MAC address of the robot - */ - public static String getMACAddress() { - try { - Enumeration nwInterface = NetworkInterface.getNetworkInterfaces(); - StringBuilder ret = new StringBuilder(); - while (nwInterface.hasMoreElements()) { - NetworkInterface nis = nwInterface.nextElement(); - System.out.println("NIS: " + nis.getDisplayName()); - if (nis != null && "eth0".equals(nis.getDisplayName())) { - byte[] mac = nis.getHardwareAddress(); - if (mac != null) { - for (int i = 0; i < mac.length; i++) { - ret.append(String.format("%02X%s", mac[i], - (i < mac.length - 1) ? ":" : "")); + /** + * This is code from Poofs 2022 + * + * @return the MAC address of the robot + */ + public static String getMACAddress() { + try { + Enumeration nwInterface = NetworkInterface.getNetworkInterfaces(); + StringBuilder ret = new StringBuilder(); + while (nwInterface.hasMoreElements()) { + NetworkInterface nis = nwInterface.nextElement(); + System.out.println("NIS: " + nis.getDisplayName()); + if (nis != null && "eth0".equals(nis.getDisplayName())) { + byte[] mac = nis.getHardwareAddress(); + if (mac != null) { + for (int i = 0; i < mac.length; i++) { + ret.append(String.format("%02X%s", mac[i], + (i < mac.length - 1) ? ":" : "")); + } + String addr = ret.toString(); + System.out.println("NIS " + nis.getDisplayName() + " addr: " + addr); + return addr; + } else { + System.out.println("Address doesn't exist or is not accessible"); + } + } else { + System.out.println("Skipping adaptor: " + nis.getDisplayName()); + } } - String addr = ret.toString(); - System.out.println("NIS " + nis.getDisplayName() + " addr: " + addr); - return addr; - } else { - System.out.println("Address doesn't exist or is not accessible"); - } - } else { - System.out.println("Skipping adaptor: " + nis.getDisplayName()); + } catch (SocketException | NullPointerException e) { + e.printStackTrace(); } - } - } catch (SocketException | NullPointerException e) { - e.printStackTrace(); + System.out.println("\n\nMAC ADDRESS NOTHING"); + return ""; } - System.out.println("\n\nMAC ADDRESS NOTHING"); - return ""; - } - public static double armXCM(double shoulderAngleDegrees, double elbowAngleDegrees) { - double x1 = RobotConstants.SHOULDER_LENGTH_INCHES - * Math.sin(Math.toRadians(shoulderAngleDegrees)); - double x2 = RobotConstants.ELBOW_LENGTH_INCHES - * Math.sin(Math.toRadians(shoulderAngleDegrees + elbowAngleDegrees - 180)); - double xcm = (RobotConstants.SHOULDER_TO_ELBOW_MASS_LB * x1 / 4.0 // first segment CM - + RobotConstants.ELBOW_TO_CLAW_MASS_LB * (x1 + x2 / 2.0) // second segment CM - + RobotConstants.CLAW_MASS_LB * (x1 + x2)) // claw CM - / (RobotConstants.SHOULDER_TO_ELBOW_MASS_LB + RobotConstants.ELBOW_TO_CLAW_MASS_LB - + RobotConstants.CLAW_MASS_LB); // divide by total mass - return xcm; - } + public static double armXCM(double shoulderAngleDegrees, double elbowAngleDegrees) { + double x1 = RobotConstants.SHOULDER_LENGTH_INCHES + * Math.sin(Math.toRadians(shoulderAngleDegrees)); + double x2 = RobotConstants.ELBOW_LENGTH_INCHES + * Math.sin(Math.toRadians(shoulderAngleDegrees + elbowAngleDegrees - 180)); + double xcm = (RobotConstants.SHOULDER_TO_ELBOW_MASS_LB * x1 / 4.0 // first segment CM + + RobotConstants.ELBOW_TO_CLAW_MASS_LB * (x1 + x2 / 2.0) // second segment CM + + RobotConstants.CLAW_MASS_LB * (x1 + x2)) // claw CM + / (RobotConstants.SHOULDER_TO_ELBOW_MASS_LB + RobotConstants.ELBOW_TO_CLAW_MASS_LB + + RobotConstants.CLAW_MASS_LB); // divide by total mass + return xcm; + } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java index 08468ef0..737c2811 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/Claw.java @@ -25,14 +25,12 @@ public enum Status { private SparkMAXLite leader, follower; - private int count = 0; + private int currentSpikeCounter = 0; private boolean isFull = false; - private int ticker = 0; + private int ejectCounter = 0; private Claw.Status status; - private Arm arm; - private boolean returnToStowed = true; - public Claw(Arm arm) { + public Claw() { List motorControllers = SparkMAXFactory.buildSparkMAXPair(RobotMap.CLAW_LEADER_MOTOR, RobotMap.CLAW_FOLLOWER_MOTOR, true, SparkMAXFactory.defaultConfig, Motor_Type.NEO_550); leader = motorControllers.get(0); @@ -40,23 +38,7 @@ public Claw(Arm arm) { status = Status.IDLE; leader.setIdleMode(IdleMode.kBrake); follower.setIdleMode(IdleMode.kBrake); - this.arm = arm; - } - - public void setArm(Arm arm) { - this.arm = arm; - } - - public Arm getArm() { - return arm; - } - public boolean willReturnToStowed() { - return returnToStowed; - } - - public void setReturnToStowed(boolean returnToStowed) { - this.returnToStowed = returnToStowed; } public void setStatus(Claw.Status status) { @@ -98,16 +80,14 @@ public void mustangPeriodic() { case EJECTING: leader.set(RobotConstants.CLAW_EJECTING_SPEED); - ticker++; + ejectCounter++; // some arbitrary amount of time until ejection is finished // TODO: adjust length of time or find a better way to check for finishing // ejection - if (ticker > 25) { + if (ejectCounter > RobotConstants.CLAW_EJECT_PERIODICS) { isFull = false; - ticker = 0; - if (returnToStowed) { - MustangScheduler.getInstance().schedule(new MoveToTarget(arm, this, ArmState.STOWED)); - } + ejectCounter = 0; + } break; default: @@ -117,17 +97,15 @@ public void mustangPeriodic() { if (this.status == Status.INTAKING) { if (leader.getOutputCurrent() > RobotConstants.CLAW_CURRENT_MAX || follower.getOutputCurrent() > RobotConstants.CLAW_CURRENT_MAX) { - count++; - if (count > 5) { + currentSpikeCounter++; + if (currentSpikeCounter > 5) { setStatus(Status.IDLE); isFull = true; - ticker = 0; - if (returnToStowed) { - MustangScheduler.getInstance().schedule(new MoveToTarget(arm, this, ArmState.STOWED)); - } + ejectCounter = 0; + } } else { - count = 0; + currentSpikeCounter = 0; } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java index 5555f32e..a7ab3a2d 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/DriveBase.java @@ -4,6 +4,7 @@ package frc.team670.robot.subsystems; +import frc.team670.mustanglib.swervelib.SdsModuleConfigurations; import com.pathplanner.lib.PathPlannerTrajectory; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -20,33 +21,36 @@ public class DriveBase extends SwerveDrive { /** - * The maximum voltage that will be delivered to the drive motors. This can be reduced to cap - * the robot's maximum speed. Typically, this is useful during initial testing of the robot. + * The maximum voltage that will be delivered to the drive motors. This can be + * reduced to cap + * the robot's maximum speed. Typically, this is useful during initial testing + * of the robot. */ public static final double MAX_VOLTAGE = 12.0; - // The formula for calculating the theoretical maximum velocity is: - // / 60 * * * pi + // / 60 * * * + // pi // An example of this constant for a Mk4 L2 module with NEOs to drive is: // 5880.0 / 60.0 / SdsModuleConfigurations.MK4_L2.getDriveReduction() * // SdsModuleConfigurations.MK4_L2.getWheelDiameter() * Math.PI /** - * The maximum velocity of the robot in meters per second. This is a measure of how fast the + * The maximum velocity of the robot in meters per second. This is a measure of + * how fast the * robot should be able to drive in a straight line. */ - public static final double MAX_VELOCITY_METERS_PER_SECOND = - 5676.0 / 60.0 * SdsModuleConfigurations.MK4I_L1.getDriveReduction() - * SdsModuleConfigurations.MK4I_L1.getWheelDiameter() * Math.PI; + public static final double MAX_VELOCITY_METERS_PER_SECOND = 5676.0 / 60.0 + * SdsModuleConfigurations.MK4I_L1.getDriveReduction() + * SdsModuleConfigurations.MK4I_L1.getWheelDiameter() * Math.PI; /** - * The maximum angular velocity of the robot in radians per second. This is a measure of how + * The maximum angular velocity of the robot in radians per second. This is a + * measure of how * fast the robot can rotate in place. */ - public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = - MAX_VELOCITY_METERS_PER_SECOND - / Math.hypot(RobotConstants.DRIVETRAIN_TRACKWIDTH_METERS / 2.0, - RobotConstants.DRIVETRAIN_WHEELBASE_METERS / 2.0); + public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND + / Math.hypot(RobotConstants.DRIVETRAIN_TRACKWIDTH_METERS / 2.0, + RobotConstants.DRIVETRAIN_WHEELBASE_METERS / 2.0); private MustangCommand defaultCommand; private MustangController mController; @@ -74,7 +78,6 @@ public DriveBase(MustangController mustangController) { this.mController = mustangController; } - /** * Makes the DriveBase's default command initialize teleop */ @@ -87,11 +90,12 @@ public void initDefaultCommand() { // TODO: switch to super class's init default public void cancelDefaultCommand() { MustangScheduler.getInstance().cancel(defaultCommand); } - public void mustangPeriodic(){ + + public void mustangPeriodic() { super.mustangPeriodic(); debugSubsystem(); } - + @Override public HealthState checkHealth() { return HealthState.GREEN; @@ -99,7 +103,7 @@ public HealthState checkHealth() { @Override public void debugSubsystem() { - SmartDashboard.putNumber("pitch",getPitch()); + SmartDashboard.putNumber("pitch", getPitch()); } @@ -107,6 +111,6 @@ public MustangPPSwerveControllerCommand getFollowTrajectoryCommand(PathPlannerTr return new MustangPPSwerveControllerCommand(traj, getPoseEstimator()::getCurrentPose, getSwerveKinematics(), RobotConstants.xController, RobotConstants.yController, RobotConstants.thetaController, - this::setModuleStates, new Subsystem[] {this}); + this::setModuleStates, new Subsystem[] { this }); } } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java index fd69ec01..df9862f6 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Arm.java @@ -58,7 +58,7 @@ public HealthState checkHealth() { return HealthState.RED; } - if(elbow.checkHealth() == HealthState.YELLOW || shoulder.checkHealth() == HealthState.YELLOW) { + if (elbow.checkHealth() == HealthState.YELLOW || shoulder.checkHealth() == HealthState.YELLOW) { return HealthState.YELLOW; } @@ -88,7 +88,7 @@ public void resetPositionFromAbsolute() { * We must handle checking for valid paths ELSEWHERE. */ public void moveToTarget(ArmState target) { - if(checkHealth() == HealthState.GREEN) { + if (checkHealth() == HealthState.GREEN) { this.targetState = target; elbow.setSystemTargetAngleInDegrees(target.getElbowAngle()); this.elbowOffset = 0; @@ -112,37 +112,24 @@ public ArmState getTargetState() { return targetState; } - public void setArmOffsets(double elbowOffset, double shoulderOffset) { - elbow.setOffset(elbowOffset); - shoulder.setOffset(shoulderOffset); - elbow.setSystemTargetAngleInDegrees(targetState.getElbowAngle() + elbow.getOffset()); - shoulder.setSystemTargetAngleInDegrees(targetState.getShoulderAngle() + shoulder.getOffset()); + public void adjustElbowOffset(double elbowOffset) { + elbow.adjustOffset(elbowOffset); } - public void setElbowOffset(double elbowOffset) { - elbow.setOffset(elbowOffset); - elbow.setSystemTargetAngleInDegrees(targetState.getElbowAngle() + elbow.getOffset()); - - } - - public void setShoulderOffset(double shoulderOffset) { - shoulder.setOffset(shoulderOffset); - shoulder.setSystemTargetAngleInDegrees(targetState.getShoulderAngle() + shoulder.getOffset()); + public void adjustShoulderOffset(double shoulderOffset) { + shoulder.adjustOffset(shoulderOffset); } public void resetElbowOffset() { - elbow.setOffset(0); - elbow.setSystemTargetAngleInDegrees(targetState.getElbowAngle() + elbow.getOffset()); + elbow.resetOffset(); } public void resetShoulderOffset() { - - shoulder.setOffset(0); - shoulder.setSystemTargetAngleInDegrees(targetState.getShoulderAngle() + shoulder.getOffset()); + shoulder.resetOffset(); } diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java index 69147362..2783ffec 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Elbow.java @@ -205,17 +205,23 @@ public void resetPositionFromAbsolute() { relativePositionIsSet = false; } - public void setOffset(double offset) { - if (Math.abs(offset) > RobotConstants.ELBOW_MAX_OVERRIDE_DEGREES) { - this.offset = RobotConstants.ELBOW_MAX_OVERRIDE_DEGREES * this.offset / Math.abs(this.offset); + public void adjustOffset(double offsetDiff) { + double orgSetpoint = getSetpoint() - super.getMotorRotationsFromAngle(offset); + if (Math.abs(this.offset + offsetDiff) > RobotConstants.ELBOW_MAX_OVERRIDE_DEGREES) { + this.offset = RobotConstants.ELBOW_MAX_OVERRIDE_DEGREES * (this.offset + offsetDiff) + / Math.abs(this.offset + offsetDiff); } else { - this.offset = offset; + this.offset += offsetDiff; + } + setSystemMotionTarget(orgSetpoint + super.getMotorRotationsFromAngle(offset)); } - public double getOffset() { - return offset; + public void resetOffset() { + super.setSystemTargetAngleInDegrees(getSetpoint() - super.getMotorRotationsFromAngle(offset)); + offset = 0; + } @Override diff --git a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java index 4678cd93..97647fb0 100644 --- a/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java +++ b/2023-Robot/src/main/java/frc/team670/robot/subsystems/arm/Shoulder.java @@ -161,17 +161,23 @@ public boolean getTimeout() { return false; } - protected void setOffset(double offset) { - if (Math.abs(offset) > RobotConstants.SHOULDER_MAX_OVERRIDE_DEGREES) { - this.offset = RobotConstants.SHOULDER_MAX_OVERRIDE_DEGREES * this.offset / Math.abs(this.offset); + public void adjustOffset(double offsetDiff) { + double orgSetpoint = getSetpoint() - super.getMotorRotationsFromAngle(offset); + if (Math.abs(this.offset + offsetDiff) > RobotConstants.SHOULDER_MAX_OVERRIDE_DEGREES) { + this.offset = RobotConstants.SHOULDER_MAX_OVERRIDE_DEGREES * (this.offset + offsetDiff) + / Math.abs(this.offset + offsetDiff); } else { - this.offset = offset; + this.offset += offsetDiff; + } + setSystemMotionTarget(orgSetpoint + super.getMotorRotationsFromAngle(offset)); } - public double getOffset() { - return offset; + public void resetOffset() { + super.setSystemTargetAngleInDegrees(getSetpoint() - super.getMotorRotationsFromAngle(offset)); + offset = 0; + } @Override