diff --git a/final-final-final.json b/final-final-final.json index 76814ce..4158159 100644 --- a/final-final-final.json +++ b/final-final-final.json @@ -9,7 +9,7 @@ "showGrid": true, "hgap": 16.0, "vgap": 16.0, - "titleType": 1, + "titleType": 2, "tiles": { "0,0": { "size": [ @@ -147,6 +147,24 @@ "Visible data/FMSInfo/FMSControlData": true, "Visible data/Shuffleboard/PDP/PDP/Voltage": true } + }, + "24,0": { + "size": [ + 3, + 3 + ], + "content": { + "_type": "Match Time", + "_source0": "network_table:///Shuffleboard/stuff/time", + "_title": "Shuffleboard/stuff/time", + "_glyph": 148, + "_showGlyph": false, + "Appearance/Colors on": true, + "Appearance/Font size": 60, + "Match length/Autonomous length": 15.0, + "Match length/Teleop length": 105.0, + "Match length/Endgame length": 30.0 + } } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 93e0692..a166d60 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -106,8 +106,8 @@ private void configureButtonBindings() { // .whileTrue(m_OpenClawCommand); new JoystickButton(m_armController2, XboxController.Button.kLeftBumper.value) .whileTrue(m_FineDriveCommand); - new JoystickButton(m_armController2, XboxController.Button.kB.value) - .whileTrue(m_PlaceConeSecondLevelCommand); + // new JoystickButton(m_armController2, XboxController.Button.kB.value) + // .whileTrue(m_PlaceConeSecondLevelCommand); } public static double getDriverControllerLeftStickY() { diff --git a/src/main/java/frc/robot/commands/AutoCommand.java b/src/main/java/frc/robot/commands/AutoCommand.java index d1e73f7..a291648 100644 --- a/src/main/java/frc/robot/commands/AutoCommand.java +++ b/src/main/java/frc/robot/commands/AutoCommand.java @@ -10,8 +10,7 @@ public class AutoCommand extends SequentialCommandGroup { public AutoCommand(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw)); - - //new BalanceCommand(m_pid, m_driveBase)) + addCommands(new BalanceCommand(m_pid, m_driveBase)); // new PlaceCubeFirstLevelCommand(m_driveBase, m_claw, m_arm) // new PlaceConeSecondLevelCommand(m_driveBase, m_arm) } diff --git a/src/main/java/frc/robot/commands/BalanceCommand.java b/src/main/java/frc/robot/commands/BalanceCommand.java index 46e0161..06d99ae 100644 --- a/src/main/java/frc/robot/commands/BalanceCommand.java +++ b/src/main/java/frc/robot/commands/BalanceCommand.java @@ -53,7 +53,7 @@ public void execute() { SmartDashboard.putNumber("max offset", max); if (pitchOffset < 16 && !onRamp) { - driveBase.setAllMotors(0.5); + driveBase.setAllMotors(0.25); SmartDashboard.putBoolean("onRamp", onRamp); } else { diff --git a/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java b/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java index 1b2292f..643d29c 100644 --- a/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java +++ b/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java @@ -37,25 +37,25 @@ public void initialize() { @Override public void execute() { if (firstStep) { - if (Math.abs(arm.armMotor.getEncoder().getPosition()) < 105) { + if (Math.abs(arm.armMotor.getEncoder().getPosition()) < 115) { claw.closeClaw(); arm.armMotor.set(0.4); } - if (Math.abs(arm.armMotor.getEncoder().getPosition()) >= 105) { + if (Math.abs(arm.armMotor.getEncoder().getPosition()) >= 115) { firstStep = false; arm.armMotor.set(0); arm.armMotor.getEncoder().setPosition(0); } // go over charge station - if (Math.abs(drive.getAverageDistance()) < 4.6 && firstStep == false) { + if (Math.abs(drive.getAverageDistance()) < 4.8 && firstStep == false) { claw.stopClaw(); drive.setAllMotors(-0.3); // move back so that cone falls in arm.armMotor.set(-0.45); } - if (Math.abs(drive.getAverageDistance()) >= 4.6) { + if (Math.abs(drive.getAverageDistance()) >= 4.8) { drive.setAllMotors(0); } } @@ -75,6 +75,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return Math.abs(drive.getAverageDistance()) >= 4.6; + return Math.abs(drive.getAverageDistance()) >= 4.8; } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index f77991d..815e93f 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -24,7 +24,7 @@ public class Arm extends SubsystemBase { public Arm() { armMotor = new CANSparkMax(7, MotorType.kBrushless); armMotor.setIdleMode(IdleMode.kBrake); - armMotor.setSmartCurrentLimit(20, 40); + armMotor.setSmartCurrentLimit(30, 40); armMotor.burnFlash(); addChild("Arm Motor", armMotor); diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 6c7d2e9..906c095 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -30,7 +30,7 @@ public class Claw extends SubsystemBase { public Claw() { clawMotor = new CANSparkMax(8, MotorType.kBrushless); clawMotor.setIdleMode(IdleMode.kBrake); - clawMotor.setSmartCurrentLimit(15); + clawMotor.setSmartCurrentLimit(17); clawMotor.burnFlash(); addChild("Claw Motor", clawMotor);