diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e36da298..1226c612 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,30 +104,30 @@ private void configureBindings() { feederSubsystem.setFeederMotorSpeed(feederSubsystem.FEEDER_MOTOR_SPEED); })); - bButton.onTrue(new InstantCommand(() -> { + bButton.onFalse(new InstantCommand(() -> { feederSubsystem.setFeederMotorSpeed(0); })); pivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - pivotSubsystem.setPivotMotorSpeed(mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis()); - })); + pivotSubsystem.setPivotMotorSpeed(.3 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis())); + }, pivotSubsystem)); if(baseSwerveSubsystem instanceof SwerveSubsystem){ - final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; - - swerveSubsystem.setDefaultCommand(new RunCommand(() -> { - swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); - pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); - } - , swerveSubsystem)); - - driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { - swerveSubsystem.resetDriverHeading(); - } - )); - + final SwerveSubsystem swerveSubsystem = (SwerveSubsystem) baseSwerveSubsystem; + + swerveSubsystem.setDefaultCommand(new RunCommand(() -> { + // swerveSubsystem.setDrivePowers(driveController.getLeftPower(), driveController.getForwardPower(), driveController.getRotatePower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); + // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); + } + , swerveSubsystem)); + + driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { + swerveSubsystem.resetDriverHeading(); + } + )); + } else if(baseSwerveSubsystem instanceof TestSingleModuleSwerveSubsystem){ - final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem; + final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem; // LBumper.onTrue(new InstantCommand(() -> { // testSwerveSubsystem.decrementTest(); // System.out.println(testSwerveSubsystem.getTest()); @@ -146,7 +146,7 @@ private void configureBindings() { // })); } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){ - final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; + final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem; // System.out.println("1"); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ce1817a9..4d1c387b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.shooter.ShooterState; @@ -13,7 +14,7 @@ public class ShooterSubsystem extends SubsystemBase { //change later (ALL OF THEM ARE PLACEHOLDERS) int IDNUMBER = 10; //so I remember to change them later - public final double SHOOTER_MOTOR_SPEED = 0.1; + public final double SHOOTER_MOTOR_SPEED = 1; //motors private final CANSparkMax shooterMotor; @@ -29,6 +30,10 @@ public ShooterSubsystem(){ shooterMotor = new CANSparkMax(13, MotorType.kBrushless); shooterMotorTwo = new CANSparkMax(14, MotorType.kBrushless); + shooterMotor.setIdleMode(IdleMode.kCoast); + shooterMotorTwo.setIdleMode(IdleMode.kCoast); + + //second motor shooter follows first shooterMotorTwo.follow(shooterMotor, true); @@ -46,7 +51,7 @@ public void setShooterState(ShooterState newState){ } public void setShooterMotorSpeed(double speed){ - shooterMotor.setVoltage(speed * 12); + shooterMotor.setVoltage(-speed * 12); System.out.println("shooter motor speed is: " + shooterMotor.get()); }