diff --git a/src/main/deploy/pathplanner/balance 1 cone mobility.path b/src/main/deploy/pathplanner/balance 1 cone mobility.path new file mode 100644 index 0000000..570e8c2 --- /dev/null +++ b/src/main/deploy/pathplanner/balance 1 cone mobility.path @@ -0,0 +1,135 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8983607197983627, + "y": 3.3001395917321754 + }, + "prevControl": null, + "nextControl": { + "x": 2.3324555433831, + "y": 3.3001395917321754 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "score cone" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.5004889962273995, + "y": 3.3 + }, + "prevControl": { + "x": 3.032178118965819, + "y": 3.3 + }, + "nextControl": { + "x": 3.96879987348898, + "y": 3.3 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.2716490183574205, + "y": 3.3036114528286067 + }, + "prevControl": { + "x": 3.8375704029249773, + "y": 3.3036114528286067 + }, + "nextControl": { + "x": 4.705727633789864, + "y": 3.3036114528286067 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "balance reversed" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.6781053733047635, + "y": 3.3001395917321754 + }, + "prevControl": { + "x": 5.2440267578723185, + "y": 3.3001395917321754 + }, + "nextControl": { + "x": 5.2440267578723185, + "y": 3.3001395917321754 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": 0.1, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "balance" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.113679338469788, + "y": 3.3 + }, + "prevControl": { + "x": 4.527299200237959, + "y": 3.3 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "balance forward" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 1.0, + "maxAcceleration": 0.7, + "isReversed": true, + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/balance 1 cone.path b/src/main/deploy/pathplanner/balance 1 cone.path new file mode 100644 index 0000000..1762589 --- /dev/null +++ b/src/main/deploy/pathplanner/balance 1 cone.path @@ -0,0 +1,53 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8869415516608812, + "y": 3.3001395917321754 + }, + "prevControl": null, + "nextControl": { + "x": 2.8869415516608807, + "y": 3.3001395917321754 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "score cone" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.6226551085580803, + "y": 3.3 + }, + "prevControl": { + "x": 2.6395449744140085, + "y": 3.3 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "balance" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/balance 1 cube mobility.path b/src/main/deploy/pathplanner/balance 1 cube mobility.path index 6752a86..093d910 100644 --- a/src/main/deploy/pathplanner/balance 1 cube mobility.path +++ b/src/main/deploy/pathplanner/balance 1 cube mobility.path @@ -7,7 +7,7 @@ }, "prevControl": null, "nextControl": { - "x": 2.875522383523399, + "x": 2.3096172071081362, "y": 2.74 }, "holonomicAngle": 0, @@ -26,15 +26,67 @@ }, { "anchorPoint": { - "x": 5.769458718404617, + "x": 3.3828525776709673, "y": 2.74 }, "prevControl": { - "x": 4.539576670790269, + "x": 2.9145417004093868, "y": 2.74 }, "nextControl": { - "x": 4.539576670790269, + "x": 3.8511634549325477, + "y": 2.74 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.55902689583157, + "y": 2.740600352995578 + }, + "prevControl": { + "x": 4.124948280399127, + "y": 2.740600352995578 + }, + "nextControl": { + "x": 4.935859444368463, + "y": 2.740600352995578 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "balance reversed" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.79229705467958, + "y": 2.74 + }, + "prevControl": { + "x": 5.044450511140433, + "y": 2.74 + }, + "nextControl": { + "x": 5.044450511140433, "y": 2.74 }, "holonomicAngle": 0, @@ -51,17 +103,17 @@ }, { "anchorPoint": { - "x": 4.25, + "x": 4.330643533081939, "y": 2.74 }, "prevControl": { - "x": 5.59056037413483, + "x": 5.007849306202964, "y": 2.74 }, "nextControl": null, "holonomicAngle": 0, "isReversal": false, - "velOverride": 0.5, + "velOverride": null, "isLocked": false, "isStopPoint": false, "stopEvent": { @@ -76,6 +128,6 @@ ], "maxVelocity": 1.5, "maxAcceleration": 1.0, - "isReversed": null, + "isReversed": true, "markers": [] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/bump 1 cone 1 cube.path b/src/main/deploy/pathplanner/bump 1 cone 1 cube.path new file mode 100644 index 0000000..09ed4e1 --- /dev/null +++ b/src/main/deploy/pathplanner/bump 1 cone 1 cube.path @@ -0,0 +1,114 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8869415516608812, + "y": 0.513862566186671 + }, + "prevControl": null, + "nextControl": { + "x": 3.767276069358055, + "y": 0.513862566186671 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "score cone" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.461710780903748, + "y": 1.0853363274422518 + }, + "prevControl": { + "x": 5.461710780903748, + "y": 0.5807501401226086 + }, + "nextControl": { + "x": 5.461710780903748, + "y": 0.5807501401226086 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.080194777966754, + "y": 0.9234879277359507 + }, + "prevControl": { + "x": 6.698305460316281, + "y": 0.9234879277359507 + }, + "nextControl": { + "x": 6.698305460316281, + "y": 0.9234879277359507 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.901045987365135, + "y": 1.0567748451411407 + }, + "prevControl": { + "x": 3.1958331850155397, + "y": 1.0662953392415107 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "outtake cube" + ], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 1.7, + "maxAcceleration": 1.0, + "isReversed": true, + "markers": [ + { + "position": 0.25, + "names": [ + "event", + "intake cube" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/bump 2 cube simple.path b/src/main/deploy/pathplanner/bump 2 cube simple.path index 82c0701..9fc9ce0 100644 --- a/src/main/deploy/pathplanner/bump 2 cube simple.path +++ b/src/main/deploy/pathplanner/bump 2 cube simple.path @@ -26,16 +26,16 @@ }, { "anchorPoint": { - "x": 7.061153789766013, - "y": 0.942528915936693 + "x": 7.253950576277221, + "y": 0.9249526191360088 }, "prevControl": { - "x": 5.294720831721951, - "y": 0.9685996608097742 + "x": 5.9622555049158255, + "y": 0.9644721630227321 }, "nextControl": { - "x": 5.294720831721951, - "y": 0.9685996608097742 + "x": 5.9622555049158255, + "y": 0.9644721630227321 }, "holonomicAngle": 0, "isReversal": true, @@ -51,12 +51,12 @@ }, { "anchorPoint": { - "x": 2.1580993280751417, - "y": 1.0567748451411407 + "x": 2.081067409998068, + "y": 0.97 }, "prevControl": { - "x": 2.754248676931111, - "y": 1.0567748451411407 + "x": 5.121472853568112, + "y": 0.9700000000000004 }, "nextControl": null, "holonomicAngle": 0, @@ -66,7 +66,7 @@ "isStopPoint": false, "stopEvent": { "names": [ - "outtake cube" + "outtake mid cube" ], "executionBehavior": "parallel", "waitBehavior": "none", diff --git a/src/main/deploy/pathplanner/flat 1 cone 1 cube.path b/src/main/deploy/pathplanner/flat 1 cone 1 cube.path index 1ec8306..f22628b 100644 --- a/src/main/deploy/pathplanner/flat 1 cone 1 cube.path +++ b/src/main/deploy/pathplanner/flat 1 cone 1 cube.path @@ -99,8 +99,8 @@ } } ], - "maxVelocity": 1.4, - "maxAcceleration": 1.0, + "maxVelocity": 1.8, + "maxAcceleration": 2.0, "isReversed": true, "markers": [ { diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index e7c5141..04a0589 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -41,6 +41,7 @@ private static class DRIVER_MAP { private static final int ZERO_BUTTON = 6; private static final int WRIST_DEAD_BUTTON = 7; + private static final int ARM_DEAD_BUTTON = 8; } /** Buttons on the operator controller */ @@ -92,6 +93,7 @@ private static class OPERATOR_MAP { // hidden buttons public static final JoystickButton zeroButton = new JoystickButton(leftJoystick, DRIVER_MAP.ZERO_BUTTON); public static final JoystickButton wristDeadButton = new JoystickButton(leftJoystick, DRIVER_MAP.WRIST_DEAD_BUTTON); + public static final JoystickButton armDeadButton = new JoystickButton(leftJoystick, DRIVER_MAP.ARM_DEAD_BUTTON); public static final JoystickButton turnButton = new JoystickButton(leftJoystick, DRIVER_MAP.TURN_BUTTON); // left joystick @@ -169,7 +171,7 @@ private static double processDriveInput(double raw) { } ; public static DoubleSupplier manualMoveArmSupplier = () -> { - return processDriveInput(xboxController.getLeftY()); + return processDriveInput(xboxController.getLeftY()) * 0.5; } ; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 01719d7..5a69499 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -42,6 +42,12 @@ public void autonomousInit() { robotContainer.gyro.reset(); robotContainer.limelight.useApriltagPipeline(); + // initialize wrist deadness + if (robotContainer.wristDeadChooser.getSelected()) { + robotContainer.wrist.wristDead(); + } + + // get selected auto m_autonomousCommand = robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8fa447b..102af4f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -18,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.CurtisDriveCommand; import frc.robot.commands.DefaultLightCommand; import frc.robot.commands.DiscoCommand; @@ -28,7 +30,8 @@ import frc.robot.commands.TurnToAngleCommand; import frc.robot.commands.SpinClawCommand.Direction; import frc.robot.commands.autonomous.AutoIntakeCommand; -import frc.robot.commands.autonomous.AutoMidCubeBackwardsCommand; +import frc.robot.commands.autonomous.AutoLowCubeBackwardsCommand; +import frc.robot.commands.autonomous.AutoMidCubeBackwards; import frc.robot.commands.autonomous.AutoScoreConeCommand; import frc.robot.commands.autonomous.AutoShootBackwardsCommand; import frc.robot.commands.instant.SetClawCommand; @@ -72,6 +75,7 @@ public class RobotContainer { // Dashboard autonomous chooser public final SendableChooser autoChooser = new SendableChooser<>(); + public final SendableChooser wristDeadChooser = new SendableChooser(); // Create robotContainer public RobotContainer() { @@ -82,7 +86,7 @@ public RobotContainer() { lights.setDefaultCommand(new DefaultLightCommand(lights, targetSelection, claw)); // Put autonomous chooser on dashboard - autoChooser.addOption("arm angle", new SetWristAngleCommand(wrist, 0)); + //autoChooser.addOption("arm angle", new SetWristAngleCommand(wrist, 0)); autoChooser.addOption("flat 2 cube", createPathUtils.createPathCommand("flat 2 cube", 1.7, 1)); autoChooser.addOption("bump 2 cube turn", createPathUtils.createPathCommand("bump 2 cube turn", 1.5, 1)); @@ -90,9 +94,9 @@ public RobotContainer() { autoChooser.addOption("balance 2 cube", createPathUtils.createPathCommand("balance 2 cube", 1.5, 1)); autoChooser.addOption("outtake", new AutoShootBackwardsCommand(arm, wrist, claw, targetSelection)); - autoChooser.addOption("outtake mid", new AutoMidCubeBackwardsCommand(arm, wrist, claw)); - autoChooser.addOption("intake", new AutoIntakeCommand(drivetrain, arm, wrist, claw, targetSelection)); - autoChooser.addOption("move wrist", Commands.runOnce(() -> wrist.setMotors(0.04))); + autoChooser.addOption("outtake mid", new AutoMidCubeBackwards(arm, wrist, claw)); + //autoChooser.addOption("intake", new AutoIntakeCommand(drivetrain, arm, wrist, claw, targetSelection)); + //autoChooser.addOption("move wrist", Commands.runOnce(() -> wrist.setMotors(0.04))); // simple autos autoChooser.addOption("balance 1 cube mobility", createPathUtils.createPathCommand("balance 1 cube mobility", 1, 0.5)); @@ -105,12 +109,26 @@ public RobotContainer() { autoChooser.addOption("score cone", new AutoScoreConeCommand(arm, wrist, claw)); // CONES!! - autoChooser.addOption("flat 1 cone 1 cube", createPathUtils.createPathCommand("flat 1 cone 1 cube", 1.4, 1, true)); + autoChooser.addOption("flat 1 cone 1 cube", createPathUtils.createPathCommand("flat 1 cone 1 cube", 1.8, 2, true)); + autoChooser.addOption("bump 1 cone 1 cube", createPathUtils.createPathCommand("bump 1 cone 1 cube", 1.8, 2, true)); + autoChooser.addOption("balance 1 cone mobility", createPathUtils.createPathCommand("balance 1 cone mobility", 1, 1, true)); + autoChooser.addOption("balance 1 cone", createPathUtils.createPathCommand("balance 1 cone", 1, 1, true)); - Shuffleboard.getTab("my favourite tab") - .add(autoChooser) - .withPosition(3, 3) - .withSize(2, 1); + autoChooser.addOption("do nothing!", new WaitCommand(5)); + + ShuffleboardTab tab = Shuffleboard.getTab("my favourite tab"); + + // autonomous chooser + tab.add(autoChooser) + .withPosition(3, 3) + .withSize(2, 1); + + wristDeadChooser.addOption("DEAAAAAAD", true); + wristDeadChooser.addOption("ALIVE YAY!!!!!!!!", false); + + // wrist dead chooser for auto + tab.add(wristDeadChooser) + .withPosition(5, 3); //SmartDashboard.putData(autoChooser); configureBindings(); @@ -135,6 +153,7 @@ private void configureBindings() { OI.slowButton.whileTrue(new DriveSlowCommand(drivetrain)); OI.zeroButton.onTrue(Commands.runOnce(() -> arm.zeroTelescope())); OI.wristDeadButton.onTrue(Commands.runOnce(() -> wrist.wristDead())); + OI.armDeadButton.onTrue(Commands.runOnce(() -> arm.armDead())); // operator OI.driveStraightButton.whileTrue(new GyroDriveStraightCommand(drivetrain, gyro, OI.rightDriveSupplier)); @@ -154,7 +173,7 @@ private void configureBindings() { // Move arm and wrist into ground intake position. Only run if arm is already down to avoid smashing things in front of the robot. (Still sets arm to down position for completion sake) OI.groundIntakeButton.onTrue( new ConditionalCommand( - new LowerArmCommand(arm).andThen(new SetWristAngleCommand(wrist, Wrist.GROUND)), + new LowerArmCommand(arm).andThen(Commands.runOnce(() -> wrist.setMotors(-0.6))).andThen(new SetWristAngleCommand(wrist, Wrist.GROUND)), Commands.runOnce(() -> System.out.print("Lower arm before going to ground position")), () -> arm.getPistonsPosition() == ArmPosition.DOWN ) diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 20edecb..c65b23d 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -153,6 +153,8 @@ private class PCM { telescopeController1.restoreFactoryDefaults(); telescopeController2.restoreFactoryDefaults(); + telescopeController1.setClosedLoopRampRate(0.25); + telescopeController2.follow(telescopeController1, true); } } diff --git a/src/main/java/frc/robot/commands/ScoringPositionCommand.java b/src/main/java/frc/robot/commands/ScoringPositionCommand.java index 84422db..8c98bff 100644 --- a/src/main/java/frc/robot/commands/ScoringPositionCommand.java +++ b/src/main/java/frc/robot/commands/ScoringPositionCommand.java @@ -18,7 +18,7 @@ public class ScoringPositionCommand extends CommandBase { double wristAngle; double telescopePosition; double startTime; - double timeout = 1500; + double timeout = 1000; Type type; Height height; @@ -84,12 +84,12 @@ public void initialize() { if (!wrist.isWristDeadAgain) { switch(height){ case HIGH: - telescopePosition = Arm.ALL_CUBE; + telescopePosition = Arm.HIGH_CUBE; wristAngle = Wrist.SCORE_HIGH_CUBE_FORWARD; arm.raiseBothPistons(); break; case MID: - telescopePosition = Arm.ALL_CUBE; + telescopePosition = 0; wristAngle = Wrist.SCORE_MID_CUBE_FORWARD; arm.raiseBothPistons(); break; @@ -119,9 +119,9 @@ public void initialize() { wrist.setTargetAngle(wristAngle); // help wrist get to position when it starts way back with arm up - // probably temporary until motor alives more? - if (auto) { - wrist.setMotors(-1); + // probably temporary until motor alives? + if (arm.getCurrentAngle() > 10 && wrist.getCurrentAngle() > 120 && wristAngle < 120) { + wrist.setMotors(-0.7); } } diff --git a/src/main/java/frc/robot/commands/ShootScoreCommand.java b/src/main/java/frc/robot/commands/ShootScoreCommand.java index abf038b..eb3e61e 100644 --- a/src/main/java/frc/robot/commands/ShootScoreCommand.java +++ b/src/main/java/frc/robot/commands/ShootScoreCommand.java @@ -49,9 +49,9 @@ public void initialize() { // set for score mid/high cube forwards if (height == Height.HIGH) { - speed = 0.75; + speed = 0.35; } else if (height == Height.MID) { - speed = 0.50; + speed = 0.25; } else if (height == Height.LOW) { // set for scoring low cube BACKWARDS speed = 0.35; diff --git a/src/main/java/frc/robot/commands/autonomous/AutoMidCubeBackwardsCommand.java b/src/main/java/frc/robot/commands/autonomous/AutoLowCubeBackwardsCommand.java similarity index 66% rename from src/main/java/frc/robot/commands/autonomous/AutoMidCubeBackwardsCommand.java rename to src/main/java/frc/robot/commands/autonomous/AutoLowCubeBackwardsCommand.java index cc8eec3..84f0bb4 100644 --- a/src/main/java/frc/robot/commands/autonomous/AutoMidCubeBackwardsCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/AutoLowCubeBackwardsCommand.java @@ -2,42 +2,44 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.commands.SpinClawCommand; import frc.robot.commands.SpinClawCommand.Direction; import frc.robot.commands.instant.LowerArmCommand; import frc.robot.commands.instant.RaiseArmCommand; import frc.robot.commands.instant.SetWristAngleCommand; +import frc.robot.commands.instant.TransportPositionCommand; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Claw; import frc.robot.subsystems.Wrist; import frc.robot.subsystems.TargetSelection.Type; -public class AutoMidCubeBackwardsCommand extends SequentialCommandGroup { - private static final double CLAW_SPEED = 0.52; +public class AutoLowCubeBackwardsCommand extends SequentialCommandGroup { + private static final double CLAW_SPEED = 0.35; /** * @param arm * @param wrist */ - public AutoMidCubeBackwardsCommand(Arm arm, Wrist wrist, Claw claw) { + public AutoLowCubeBackwardsCommand(Arm arm, Wrist wrist, Claw claw) { addCommands( // Raise arm and set wrist to correct angle - new RaiseArmCommand(arm, true), - // new LowerArmCommand(arm), + //new RaiseArmCommand(arm, true), + new LowerArmCommand(arm), - new SetWristAngleCommand(wrist, 170), + new SetWristAngleCommand(wrist, Wrist.TRANSPORT), // Wait 1 second for arm + wrist to be in position - new WaitCommand(1.3), + new WaitUntilCommand(() -> Math.abs(wrist.getCurrentAngle()) - Wrist.TRANSPORT < 10) + .withTimeout(1), // Spin the claw for 1 second at 100% power to shoot the cube new SpinClawCommand(claw, Direction.OUT, () -> CLAW_SPEED, Type.CUBE).withTimeout(0.2), // Set arm and wrist to transport position - new SetWristAngleCommand(wrist, Wrist.TRANSPORT), - new LowerArmCommand(arm) - ); + new TransportPositionCommand(arm, wrist) + ); } } diff --git a/src/main/java/frc/robot/commands/autonomous/AutoMidCubeBackwards.java b/src/main/java/frc/robot/commands/autonomous/AutoMidCubeBackwards.java new file mode 100644 index 0000000..50940a8 --- /dev/null +++ b/src/main/java/frc/robot/commands/autonomous/AutoMidCubeBackwards.java @@ -0,0 +1,45 @@ +package frc.robot.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.commands.SpinClawCommand; +import frc.robot.commands.SpinClawCommand.Direction; +import frc.robot.commands.instant.LowerArmCommand; +import frc.robot.commands.instant.RaiseArmCommand; +import frc.robot.commands.instant.SetWristAngleCommand; +import frc.robot.commands.instant.TransportPositionCommand; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Wrist; +import frc.robot.subsystems.TargetSelection.Type; + + +public class AutoMidCubeBackwards extends SequentialCommandGroup { + private static final double CLAW_SPEED = 0.65; + + /** + * @param arm + * @param wrist + */ + public AutoMidCubeBackwards(Arm arm, Wrist wrist, Claw claw) { + + addCommands( + // Raise arm and set wrist to correct angle + //new RaiseArmCommand(arm, true), + new LowerArmCommand(arm), + + new SetWristAngleCommand(wrist, Wrist.TRANSPORT), + + // Wait 1 second for arm + wrist to be in position + new WaitUntilCommand(() -> Math.abs(wrist.getCurrentAngle()) - Wrist.TRANSPORT < 10) + .withTimeout(1), + + // Spin the claw for 1 second at 100% power to shoot the cube + new SpinClawCommand(claw, Direction.OUT, () -> CLAW_SPEED, Type.CUBE).withTimeout(0.2), + + // Set arm and wrist to transport position + new TransportPositionCommand(arm, wrist) + ); + } +} diff --git a/src/main/java/frc/robot/commands/autonomous/AutoShootBackwardsCommand.java b/src/main/java/frc/robot/commands/autonomous/AutoShootBackwardsCommand.java index 4287690..c1d4d76 100644 --- a/src/main/java/frc/robot/commands/autonomous/AutoShootBackwardsCommand.java +++ b/src/main/java/frc/robot/commands/autonomous/AutoShootBackwardsCommand.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.commands.SpinClawCommand; import frc.robot.commands.SpinClawCommand.Direction; import frc.robot.commands.instant.LowerArmCommand; @@ -26,21 +27,20 @@ // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutoShootBackwardsCommand extends SequentialCommandGroup { - // AIMING FOR MID CUBE FOR NOW - private static final double CLAW_SPEED = 0.45; + private static final double CLAW_SPEED = 0.75; /** Creates a new AutoShootBackwardsCommand. */ public AutoShootBackwardsCommand(Arm arm, Wrist wrist, Claw claw, TargetSelection targetSelection) { addCommands( // Raise arm and set wrist to correct angle new RaiseArmCommand(arm, true), - // help wrist move to position? - Commands.runOnce(() -> wrist.setMotors(-0.5)), + // Commands.runOnce(() -> wrist.setMotors(-0.5)), new SetWristAngleCommand(wrist, Wrist.SCORE_CUBE_BACKWARDS), // Wait 1 second for arm + wrist to be in position - new WaitCommand(1.3), + new WaitUntilCommand(() -> Math.abs(wrist.getCurrentAngle()) - Wrist.SCORE_CUBE_BACKWARDS < 10) + .withTimeout(1), // Spin the claw for 1 second at 100% power to shoot the cube new SpinClawCommand(claw, Direction.OUT, () -> CLAW_SPEED, Type.CUBE).withTimeout(0.2), diff --git a/src/main/java/frc/robot/commands/instant/SingleLoadingCommand.java b/src/main/java/frc/robot/commands/instant/SingleLoadingCommand.java index aa790bb..ad5908d 100644 --- a/src/main/java/frc/robot/commands/instant/SingleLoadingCommand.java +++ b/src/main/java/frc/robot/commands/instant/SingleLoadingCommand.java @@ -1,6 +1,9 @@ package frc.robot.commands.instant; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.Arm; import frc.robot.subsystems.TargetSelection; import frc.robot.subsystems.Wrist; @@ -17,6 +20,14 @@ public SingleLoadingCommand(Wrist wrist, Arm arm, TargetSelection targetSelectio addCommands( new RaiseArmCommand(arm, true), + new ConditionalCommand( + //if wrist very back, give kick + Commands.runOnce(() -> wrist.setMotors(-0.7)), + //otherwise, no help needed + new WaitCommand(telescopePosition), + + () -> wrist.getCurrentAngle() > 120), + new SetTelescopeCommand(arm, telescopePosition).withTimeout(0.3), new SetWristAngleCommand(wrist, wristAngle) diff --git a/src/main/java/frc/robot/commands/instant/TransportPositionCommand.java b/src/main/java/frc/robot/commands/instant/TransportPositionCommand.java index 8454ae2..f93a86a 100644 --- a/src/main/java/frc/robot/commands/instant/TransportPositionCommand.java +++ b/src/main/java/frc/robot/commands/instant/TransportPositionCommand.java @@ -1,6 +1,7 @@ package frc.robot.commands.instant; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.Arm; @@ -12,9 +13,14 @@ public class TransportPositionCommand extends SequentialCommandGroup { public TransportPositionCommand(Arm arm, Wrist wrist) { addCommands( - // wait until telescope position is low enough before lowering arm to avoid slam - new SetTelescopeCommand(arm, 0) - .until(() -> arm.getTelescopePosition() < 1), + new ConditionalCommand( + // if arm is dead, do nothing + new WaitCommand(0.01), + // if arm is alive, wait until arm is down before lowering pistons in order to avoid slamming the entire extended arm on the floor which is always highly unpleasant! + new SetTelescopeCommand(arm, 0) + .until(() -> arm.getTelescopePosition() < 1), + // check for arm deadness + () -> arm.isArmDead), new LowerArmCommand(arm), diff --git a/src/main/java/frc/robot/pathplannerUtils/CreatePathUtils.java b/src/main/java/frc/robot/pathplannerUtils/CreatePathUtils.java index 2385339..b153759 100644 --- a/src/main/java/frc/robot/pathplannerUtils/CreatePathUtils.java +++ b/src/main/java/frc/robot/pathplannerUtils/CreatePathUtils.java @@ -15,7 +15,8 @@ import frc.robot.commands.autonomous.AutoBalanceCommand; import frc.robot.commands.autonomous.AutoBumpBackCommand; import frc.robot.commands.autonomous.AutoIntakeCommand; -import frc.robot.commands.autonomous.AutoMidCubeBackwardsCommand; +import frc.robot.commands.autonomous.AutoLowCubeBackwardsCommand; +import frc.robot.commands.autonomous.AutoMidCubeBackwards; import frc.robot.commands.autonomous.AutoScoreConeCommand; import frc.robot.commands.autonomous.AutoShootBackwardsCommand; import frc.robot.commands.autonomous.AutoShootForwardsCommand; @@ -59,7 +60,8 @@ public CreatePathUtils(Drivetrain drivetrain, Limelight limelight, Arm arm, Wris eventMap.put("intake cube", new AutoIntakeCommand(drivetrain, arm, wrist, claw, targetSelection)); eventMap.put("outtake cube", new AutoShootBackwardsCommand(arm, wrist, claw, targetSelection)); - eventMap.put("outtake mid cube", new AutoMidCubeBackwardsCommand(arm, wrist, claw)); + eventMap.put("outtake low cube", new AutoLowCubeBackwardsCommand(arm, wrist, claw)); + eventMap.put("outtake mid cube", new AutoMidCubeBackwards(arm, wrist, claw)); eventMap.put("balance reversed", new AutoBalanceCommand(gyro, drivetrain)); eventMap.put("balance forward", new AutoBalanceCommand(gyro, drivetrain)); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 4177e35..bdfa174 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { - CANSparkMax telescopeController1; + public CANSparkMax telescopeController1; CANSparkMax telescopeController2; DoubleSolenoid leftSolenoid; @@ -28,23 +28,25 @@ public class Arm extends SubsystemBase { private static final double kI = 0; private static final double kD = 0; private static final double MAX_OUTPUT = 0.6; - private static final double MIN_OUTPUT = 0.001; + private static final double MIN_OUTPUT = 0.0003; public static final double MAX_POSITION = 23; public static final double MIN_POSITION = 0; public static final double HIGH_CONE = 20; - public static final double MID_CONE = 9; + public static final double MID_CONE = 10.8; // FOR WRIST DEAD MODE public static final double HIGH_CUBE_DEAD = 21; public static final double MID_CUBE_DEAD = 15; // TODO: calibrate value, then see if using telescope for all mid/high cubes is effective - public static final double ALL_CUBE = 15; + public static final double HIGH_CUBE = 10; public static final double DOUBLE_LOADING = 23; + public boolean isArmDead = false; + private ShuffleboardTab tab = Shuffleboard.getTab("my favourite tab"); private GenericEntry armPositionShuffle = tab @@ -62,6 +64,11 @@ public class Arm extends SubsystemBase { .withPosition(6, 1) .getEntry(); + private GenericEntry armDeadShuffle = tab + .add("arm dead?", "NO!") + .withPosition(2, 2) + .getEntry(); + public Arm(CANSparkMax controller1, CANSparkMax controller2, DoubleSolenoid leftSolenoid, DoubleSolenoid rightSolenoid) { this.telescopeController1 = controller1; this.telescopeController2 = controller2; @@ -175,11 +182,26 @@ public void zeroTelescope() { telescopeController1.getEncoder().setPosition(0); } + /** + * toggle wrist deadness; makes certain things go into wrist dead mode + */ + public void armDead() { + isArmDead = !isArmDead; + if (isArmDead) { + armDeadShuffle.setString("yes :("); + zeroTelescope(); + } else { + armDeadShuffle.setString("not yet!"); + } + } + @Override public void periodic() { - if (telescopeController1.getEncoder().getPosition() < 1 && telescopeTargetPosition < 1) { + if (isArmDead) { + telescopeController1.set(0); + } else if (telescopeController1.getEncoder().getPosition() < 1 && telescopeTargetPosition < 1) { // near bottom, put tiny bit of motor to avoid unspooling string - telescopeController1.set(0.003); + telescopeController1.set(0.009); } else if (telescopeTargetPosition <= 0) { // let arm gravity drop to transport position telescopeController1.set(0); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 75fc3b1..605d441 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap; public class Drivetrain extends SubsystemBase { @@ -176,5 +177,28 @@ public void periodic() { odometry.update( gyro.getRotation2d(), leftEncoder.getPosition(), rightEncoder.getPosition()); + + // Extra diagnositcs, breaks best practices alot but I need the data + // mfw we kill 1/3 of the left drive motors and dont realize until 2 comps later :facepalm: + //SmartDashboard.putNumber("Left 1 Applied", RobotMap.leftController1.getAppliedOutput()); + //SmartDashboard.putNumber("Left 2 Applied", RobotMap.leftController2.getAppliedOutput()); + //SmartDashboard.putNumber("Left 3 Applied", RobotMap.leftController3.getAppliedOutput()); + //SmartDashboard.putNumber("Right 1 Applied", RobotMap.rightController1.getAppliedOutput()); + //SmartDashboard.putNumber("Right 2 Applied", RobotMap.rightController2.getAppliedOutput()); + //SmartDashboard.putNumber("Right 3 Applied", RobotMap.rightController3.getAppliedOutput()); + // + //SmartDashboard.putNumber("Left 1 Bus", RobotMap.leftController1.getBusVoltage()); + //SmartDashboard.putNumber("Left 2 Bus", RobotMap.leftController2.getBusVoltage()); + //SmartDashboard.putNumber("Left 3 Bus", RobotMap.leftController3.getBusVoltage()); + //SmartDashboard.putNumber("Right 1 Bus", RobotMap.rightController1.getBusVoltage()); + //SmartDashboard.putNumber("Right 2 Bus", RobotMap.rightController2.getBusVoltage()); + //SmartDashboard.putNumber("Right 3 Bus", RobotMap.rightController3.getBusVoltage()); + // + //SmartDashboard.putNumber("Left 1 Current", RobotMap.leftController1.getOutputCurrent()); + //SmartDashboard.putNumber("Left 2 Current", RobotMap.leftController2.getOutputCurrent()); + //SmartDashboard.putNumber("Left 3 Current", RobotMap.leftController3.getOutputCurrent()); + //SmartDashboard.putNumber("Right 1 Current", RobotMap.rightController1.getOutputCurrent()); + //SmartDashboard.putNumber("Right 2 Current", RobotMap.rightController2.getOutputCurrent()); + //SmartDashboard.putNumber("Right 3 Current", RobotMap.rightController3.getOutputCurrent()); } } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index 1316e3c..87f5238 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -22,8 +22,8 @@ public class Wrist extends SubsystemBase { private Arm arm; private static final double kF = 0; - private static final double kP = 0.7; - private static final double kI = 0.0000; + private static final double kP = 0.25; + private static final double kI = 0; private static final double kD = 0.5; private static final double MAX_OUTPUT = 0.5; @@ -47,19 +47,21 @@ public class Wrist extends SubsystemBase { private double targetAngle; private double targetVolts; - public static final double TRANSPORT = 120; + public static final double TRANSPORT = 110; //PLACEHOLDER VALUE - public static final double SINGLE_LOADING = -12; + public static final double SINGLE_LOADING = -30; public static final double DOUBLE_LOADING = -60; // lower than actually needed so P drives it down faster public static final double GROUND = -70; // Placeholder for testing, needs bettter calibration - public static final double SCORE_HIGH_CONE = 0; - public static final double SCORE_MID_CONE = -10; + public static final double SCORE_HIGH_CONE = 5; + public static final double SCORE_MID_CONE = -30; public static final double SCORE_CUBE_BACKWARDS = 100; - public static final double SCORE_MID_CUBE_FORWARD = -25; - public static final double SCORE_HIGH_CUBE_FORWARD = 20; + public static final double SCORE_MID_CUBE_FORWARD = -45; + public static final double SCORE_HIGH_CUBE_FORWARD = -20; + + public static final double SCORE_CUBE_FORWARD_NO_ARM = 0; public boolean isWristDeadAgain = false; @@ -97,7 +99,7 @@ public Wrist(CANSparkMax controller, Arm arm) { // Initialize angle to where wrist is so it doesn't try to move on enable targetAngle = getCurrentAngle(); - targetVolts = convertAngleToVolts(targetAngle); + targetVolts = convertAngleToVolts(targetAngle - arm.getCurrentAngle()); controller.setSmartCurrentLimit(24, 40, 5); // TODO: Adjust ramp rate for best performance/jerk tradeoff @@ -205,23 +207,26 @@ public void periodic() { // if wrist is dead kill motor just in case if (isWristDeadAgain){ wristController.set(0); - } else if (arm.getCurrentAngle() > 10 && getCurrentAngle() > 90 && targetAngle < 150) { + } else if (arm.getCurrentAngle() > 10 && getCurrentAngle() > 90 && targetAngle < 150) { //System.out.println("1"); // if its past vertical, spring pulls against so make AFF a tad more downward to help // since arm is up, it needs even more help - pidController.setOutputRange(-0.9, MAX_OUTPUT, 0); + pidController.setOutputRange(-0.3, MAX_OUTPUT, 0); pidController.setReference(targetVolts, ControlType.kPosition, 0, getArbitraryFeedforward() - 0.5); - } else if (getCurrentAngle() > 90 && targetAngle > 100) { + } else if (arm.getCurrentAngle() > 10 && getCurrentAngle() <-20 && targetAngle < -20) { + // if arm is up and we are at the bottom hard stop, do nothing + wristController.set(0); + } else if (getCurrentAngle() > 70 && targetAngle > 100) { //System.out.println("12"); // also stop pid if its far back enought to fall onto hardstop alone // try adding back pid cuz slam - wristController.setVoltage(getArbitraryFeedforward()); + wristController.setVoltage(0); } else if (getCurrentAngle() > 90 && targetAngle < 100) { // if its past vertical and we want to go down, spring pulls against so make AFF a tad more downward to help pidController.setReference(targetVolts, ControlType.kPosition, 0, getArbitraryFeedforward() - 0.5); - } else if (getCurrentAngle() < -55 && targetAngle < -50) { + } else if (getCurrentAngle() < -50 && targetAngle < -50) { // System.out.println("14"); // Let gravity lower arm to ground instead of slamming: @@ -230,7 +235,7 @@ public void periodic() { } else if (getCurrentAngle() < 90 && targetAngle < -40) { // System.out.println("16"); - // no ff on the way down + // yes ff on the way down and make it a bit slower pidController.setReference(targetVolts, ControlType.kPosition, 0, getArbitraryFeedforward()); } else { // System.out.println("15");