diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97dda40..2566688 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,13 +179,13 @@ public class RobotContainer { // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), getDriverController().getRightX(), getDriverController().getRightY(), true); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotSwerveDrive.driveWithInput( 0, 0, 0, @@ -223,14 +223,14 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); m_robotHood.setDefaultCommand( new RunCommand(() -> { - if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } - if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); m_robotClimber.setDefaultCommand( @@ -279,9 +279,9 @@ public class RobotContainer { .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()); + .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()); new JoystickButton(getDriverController(), XboxController.Button.kX.value) .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java index 64a79dc..7424701 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java @@ -148,7 +148,7 @@ public class Shoot extends CommandBase { SmartDashboard.putNumber("Normalized Output", this.normOutput); this.turret.m_boomBoomRotateMotor.set(normOutput); - this.swerve.driveWithInput(0, 0, normOutput, true); + this.swerve.driveWithInput(0, 0, normOutput, false); // ? should the output be field relative if (this.toShoot) { this.hood.runAngleAdjustPID(this.targetHood); @@ -164,7 +164,11 @@ public class Shoot extends CommandBase { // swerve.driveWithInput(0, 0, initialSwerveRotation, true); // this.swerve.driveWithInput(0.0, 0.0, 0.0, 0.0, true); - this.turret.m_boomBoomRotateMotor.set(0.0); + // this.turret.m_boomBoomRotateMotor.set(0.0); + + // ? should stop the turret and the swerve + ////this.swerve.stopModules(); + ////this.turret.runTurretWithInput(0); if (this.toShoot) { this.hood.runHood(0.0); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 35cd854..72a7194 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -97,6 +97,7 @@ public class SwerveDrive extends SubsystemBase { ignoreAngles = true; else ignoreAngles = false; + Translation2d speed = new Translation2d(speedX, speedY); double mag = speed.getNorm(); speed = speed.times(mag * speedAdjust); @@ -111,6 +112,7 @@ public class SwerveDrive extends SubsystemBase { SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(states); } + // new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180))) public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;