Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

Browse Source
This commit is contained in:
66945
2022-03-20 20:36:27 -06:00
parent b61548d114 757f236752
commit a635a8630c
5 changed files with 109 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/Move Forward.path
+1 -1
View File
@@ -1 +1 @@
{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":4.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":{"x":3.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
src/main/java/frc4388/robot/Constants.java
+2 -2
View File
@@ -175,7 +175,7 @@ public final class Constants {
public static final int ELBOW_ID = 31;
public static final int GYRO_ID = 14;
public static final double INPUT_MULTIPLIER = 0.7;
public static final double INPUT_MULTIPLIER = 0.4;
public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0;
public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0;
@@ -269,7 +269,7 @@ public final class Constants {
true, 27, 0, 0);
public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21;
public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22;
public static final double TURRET_SPEED_MULTIPLIER = 0.8;
public static final double TURRET_SPEED_MULTIPLIER = 0.4;
public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5;
public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844;
public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0;
src/main/java/frc4388/robot/RobotContainer.java
+93 -143
View File
@@ -127,10 +127,6 @@ public class RobotContainer {
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30);
private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31);
// public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -171,79 +167,57 @@ public class RobotContainer {
public RobotContainer() {
configureButtonBindings();
/* Default Commands */
// IK command
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000,
// getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Swerve Drive with Input
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> {
// if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) {
m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(),
getDriverController().getLeftY(),
getDriverController().getRightX(),
getDriverController().getRightY(),
true); //}
// if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
// m_robotSwerveDrive.driveWithInput( 0,
// 0,
// 0,
// 0,
// true);
}//}
true); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) {
m_robotSwerveDrive.driveWithInput( 0,
0,
0,
0,
true);
}}
, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers
// Intake with Triggers
m_robotIntake.setDefaultCommand(
new RunCommand(() -> m_robotIntake.runWithTriggers(
getOperatorController().getLeftTriggerAxis(),
getOperatorController().getRightTriggerAxis()),
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
// m_robotStorage.setDefaultCommand(
// new ManageStorage(m_robotStorage,
// m_robotBoomBoom,
// m_robotTurret).withName("Storage ManageStorage defaultCommand"));
// Storage Management
// m_robotStorage.setDefaultCommand(
// new RunCommand(() -> m_robotStorage.manageStorage(),
// m_robotStorage).withName("Storage manageStorage defaultCommand"));
// Serializer Management
// Serializer Manual
m_robotSerializer.setDefaultCommand(
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
// Turret Manual
// m_robotTurret.setDefaultCommand(
// new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
// m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
// Turret Manual
m_robotTurret.setDefaultCommand(
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
}, m_robotTurret));
// Hood Manual
m_robotHood.setDefaultCommand(
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
}, m_robotHood));
//Climber Manual
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getRightY() * 0.5)
, m_robotClimber));
// m_robotTurret.setDefaultCommand(
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
// continually sends updates to the Blinkin LED controller to keep the lights on
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
new RunCommand(() -> {
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); }
}, m_robotClimber));
// autoInit();
// recordInit();
}
@@ -256,138 +230,115 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
//! Driver Buttons
// Start > Calibrate Odometry
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
// Start > Calibrate Odometry
// Start > Calibrate Odometry
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
.whenPressed(m_robotSwerveDrive::resetGyro);
.whenPressed(m_robotSwerveDrive::resetGyro);
// Left Bumper > Shift Down
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
// Right Bumper > Shift Up
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
// new JoystickButton(getDriverController(), XboxController.Button.kA.value)
// .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true))
// .whenReleased(() -> m_robotSwerveDrive.stopModules());
// new JoystickButton(getDriverController(), XboxController.Button.kB.value)
// .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry))
// .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret));
// new JoystickButton(getDriverController(), XboxController.Button.kY.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false));
// .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
// .whenReleased(() -> m_robotSwerveDrive.stopModules());
// .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret))
// .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood))
// .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
/* Operator Buttons */
//! Operator Buttons
// Right Bumper > Storage Out
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// Left Bumper > Storage In
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// B > Toggle claws
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whenPressed(new RunCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
// X > Toggles extender in and out
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
// A > Spit Out Ball
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whenPressed(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret))
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
// Y > Full aim command
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
.whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
// .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
.whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry));
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry));
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
//Toggles extender in and out
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
// new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
// .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret))
// .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret));
// Right Bumper > Storage In
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// Left Bumper > Storage Out (note: necessary?)
// new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
// .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
// A > Shoot with Odo
/*new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret));
//B > Shoot with Lime
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
// .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)))
.whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// .whileHeld%
/* Button Box Buttons */
//! Button Box Buttons
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender)
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
.whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
.whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
.whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
// Middle Switch > Climber and Shooter mode switching
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
.whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER));
// Left Button > Extender In
new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
// Left Button > Extender Out
new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
}
/**
@@ -402,7 +353,6 @@ public class RobotContainer {
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
ArrayList<Command> commands = new ArrayList<Command>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
@@ -473,7 +423,7 @@ public class RobotContainer {
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
return null;
return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Move Forward")); // test command
}
public static XboxController getDriverController() {
src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java
+1
View File
@@ -74,6 +74,7 @@ public class AimToCenter extends CommandBase {
public static double angleToCenter(double x, double y, double gyro) {
double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0)
// double angle = Math.toDegrees(Math.atan2(y, -x) - gyro);
return (angle - 360);
}
src/main/java/frc4388/robot/subsystems/Claws.java
+12 -117
View File
@@ -32,140 +32,35 @@ public class Claws extends SubsystemBase {
private boolean m_open;
public static enum ClawType {LEFT, RIGHT}
public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) {
public Claws(Servo leftClaw, Servo rightClaw) {
m_leftClaw = leftClaw;
m_rightClaw = rightClaw;
// m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
// m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_leftLimitSwitchF.enableLimitSwitch(true);
// m_rightLimitSwitchF.enableLimitSwitch(true);
// m_leftLimitSwitchR.enableLimitSwitch(true);
// m_rightLimitSwitchR.enableLimitSwitch(true);
// m_leftClaw.setInverted(true);
// m_rightClaw.setInverted(true);
m_open = false;
// m_leftClaw.set(ClawConstants.CALIBRATION_SPEED);
// m_rightClaw.set(ClawConstants.CALIBRATION_SPEED);
}
/**
* Run a specific claw to open or close.
* @param which Which claw to run.
* @param open Whether to open or close the claw.
*/
// public void runClaw(ClawType which, boolean open) {
// int direction = open ? 1 : -1;
// if (which == Claws.ClawType.LEFT) {
// // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset;
// // m_leftClaw.getEncoder().setPosition(setPos);
// m_leftClaw.setSpeed(direction * 0.1);
// } else if (which == Claws.ClawType.RIGHT) {
// // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset;
// // m_rightClaw.getEncoder().setPosition(setPos);
// m_rightClaw.setSpeed(direction * 0.1);
// }
// m_open = open;
// }
// public void runClaw(ClawType which, double input) {
// if (which == Claws.ClawType.LEFT) {
// m_leftClaw.setSpeed(input);
// } else if (which == Claws.ClawType.RIGHT) {
// m_rightClaw.setSpeed(input);
// }
// }
// public void runClaws(double input)
// {
// m_leftClaw.setSpeed(input);
// m_rightClaw.setSpeed(input);
// }
/**
* Sets the state of both hooks
* @param open The state of the hooks
*/
public void setOpen(boolean open) {
SmartDashboard.putBoolean("open", open);
if(open) {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset);
// m_leftClaw.setPosition(.7);
// m_rightClaw.setPosition(.7);
m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);//ClawConstants.OPEN_POSITION);
m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);//ClawConstants.OPEN_POSITION);
m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);
m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);
SmartDashboard.putBoolean("Claws Closed", false);
} else {
// m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset);
// m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset);
// m_leftClaw.setPosition(.3);
// m_rightClaw.setPosition(.3);
m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 100);
m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 700);
m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 400);
m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 400);
SmartDashboard.putBoolean("Claws Closed", true);
}
}
// public double[] getOffsets() {
// return new double[] {m_leftOffset, m_rightOffset};
// }
public void toggleClaws() {
m_open = !m_open;
setOpen(m_open);
}
// public boolean fullyOpen() {
// return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
// Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
// public boolean fullyClosed() {
// return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
// Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
// }
/**
* Check if a limit switch is pressed or current limit exceeded for a claw.
* @param which Which claw to check.
* @param limit The current limit.
* @return Whether to interrupt the RunClaw command or not.
*/
// public boolean checkSwitchAndCurrent(ClawType which) {
// if (which == ClawType.LEFT) {
// if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
// return true;
// }
// }
// else if (which == ClawType.RIGHT) {
// if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
// return true;
// }
// }
// return false;
// }
// @Override
@Override
public void periodic() {
// SmartDashboard.putNumber("Servo Left Pos", m_leftClaw.getRaw());
// SmartDashboard.putNumber("Servo Right Pos", m_rightClaw.getRaw());
// if (fullyOpen() || fullyClosed()) {
// m_leftClaw.setSpeed(0.0);
// m_rightClaw.setSpeed(0.0);
// }
// if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
// // m_leftOffset = m_leftClaw.getEncoder().getPosition();
// m_leftOffset = m_leftClaw.getPosition();
// if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
// // m_rightOffset = m_rightClaw.getEncoder().getPosition();
// m_rightOffset = m_rightClaw.getPosition();
}
}