From 4e922e5f352e744c31d598522722ee3942d0dac3 Mon Sep 17 00:00:00 2001 From: Aarav Date: Thu, 23 Mar 2023 18:18:58 -0600 Subject: [PATCH] cone mid alomst done --- .../java/frc4388/robot/RobotContainer.java | 29 ++++++++++--------- .../java/frc4388/robot/subsystems/Arm.java | 4 +++ .../java/frc4388/robot/subsystems/Claw.java | 22 +++++++------- 3 files changed, 30 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 675433e..3df6123 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,6 +34,7 @@ import frc4388.robot.commands.Autos.AutoBalance; import frc4388.robot.commands.Autos.PlaybackChooser; import frc4388.robot.commands.Placement.AprilRotAlign; import frc4388.robot.commands.Placement.LimeAlign; +import frc4388.robot.commands.Swerve.JoystickPlayback; import frc4388.robot.commands.Swerve.JoystickRecorder; import frc4388.robot.commands.Swerve.RotateToAngle; import frc4388.utility.controller.DeadbandedXboxController; @@ -163,8 +164,8 @@ public class RobotContainer { ); private SequentialCommandGroup placeConeMid = new SequentialCommandGroup( - new PivotCommand(m_robotArm, 0), - new TeleCommand(m_robotArm, 0), + new PivotCommand(m_robotArm, 189), + new TeleCommand(m_robotArm, 34500), toggleClaw.asProxy(), armToHome.asProxy() ); @@ -264,8 +265,8 @@ public class RobotContainer { // m_robotSwerveDrive.setToSlow(); // })); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final @@ -279,11 +280,11 @@ public class RobotContainer { // () -> getDeadbandedDriverController().getLeftY(), // () -> getDeadbandedDriverController().getRightX(), // () -> getDeadbandedDriverController().getRightY(), - // "Blue1Path.txt")) + // "Red1Balance.txt")) // .onFalse(new InstantCommand()); // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Red1Balance.txt")) // .onFalse(new InstantCommand()); // * Operator Buttons @@ -307,22 +308,22 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); // outtake - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_TRIGGER_AXIS) // final - .onTrue (new InstantCommand(() -> m_robotClaw.reversespinnyspin())) + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final + .whileTrue (new RunCommand(() -> m_robotClaw.reversespinnyspin())) .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin())); // intake - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS) // final - .onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin())) + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final + .whileTrue (new RunCommand(() -> m_robotClaw.yesspinnyspin())) .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin())); // arm to Home - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(armToHome.asProxy()); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(armToHome.asProxy()); // interupt - new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(interruptCommand.asProxy()); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final + // .onTrue(interruptCommand.asProxy()); // // place high diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index ddc127d..b3e9d51 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.CANCoderConfiguration; import frc4388.robot.Constants.ArmConstants; import frc4388.utility.DeferredBlock; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Arm extends SubsystemBase { @@ -146,6 +147,9 @@ public class Arm extends SubsystemBase { tele_reset = true; } + SmartDashboard.putNumber("Pivot CANCoder", getArmRotation()); + SmartDashboard.putNumber("Tele Encoder", getArmLength()); + // double x = Math.cos(Math.toRadians(degrees)); } diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java index f48c4df..e93c53b 100644 --- a/src/main/java/frc4388/robot/subsystems/Claw.java +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -32,7 +32,7 @@ public class Claw extends SubsystemBase { m_rightMotor = rightMotor; m_spinnyspin = spinnyspin; - setClaw(false); + // setClaw(false); } public void setClaw(boolean open) { @@ -52,16 +52,16 @@ public class Claw extends SubsystemBase { // m_leftMotor.setRaw(m_open ? 1500 : 2000); // m_rightMotor.setRaw(m_open ? 1500 : 1000); - if (m_open) - m_spinnyspin.set(0.2); - else - m_spinnyspin.set(-0.2); - new Timer().schedule(new TimerTask() { - @Override - public void run() { - nospinnyspin(); - } - }, 750); + // if (m_open) + // m_spinnyspin.set(0.2); + // else + // m_spinnyspin.set(-0.2); + // new Timer().schedule(new TimerTask() { + // @Override + // public void run() { + // nospinnyspin(); + // } + // }, 750); } public void toggle() {