diff --git a/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto index ae003cf..b83e34c 100644 --- a/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto +++ b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto index 8d2f7b2..863d559 100644 --- a/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto +++ b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto index 7d849f7..2964dc9 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Intake in" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Middle5thNote.path b/src/main/deploy/pathplanner/paths/Middle5thNote.path index 3fd72f1..cd26d17 100644 --- a/src/main/deploy/pathplanner/paths/Middle5thNote.path +++ b/src/main/deploy/pathplanner/paths/Middle5thNote.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.0, - "y": 0.75 + "x": 7.96240792481585, + "y": 0.5130810261620535 }, "prevControl": { - "x": 7.0, - "y": 0.75 + "x": 6.96240792481585, + "y": 0.5130810261620535 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/MoveOut-Left.path b/src/main/deploy/pathplanner/paths/MoveOut-Left.path index 014b5a9..2177a8c 100644 --- a/src/main/deploy/pathplanner/paths/MoveOut-Left.path +++ b/src/main/deploy/pathplanner/paths/MoveOut-Left.path @@ -16,28 +16,28 @@ }, { "anchor": { - "x": 3.45, - "y": 7.5 + "x": 2.157262600239487, + "y": 8.004064008128015 }, "prevControl": { - "x": 1.9500000000000002, - "y": 7.5 + "x": 0.6572626002394868, + "y": 8.004064008128015 }, "nextControl": { - "x": 4.950000000000003, - "y": 7.5 + "x": 3.6572626002394935, + "y": 8.004064008128015 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 7.5 + "x": 7.1268188250662226, + "y": 7.81349105555354 }, "prevControl": { - "x": 6.0, - "y": 7.5 + "x": 6.1268188250662226, + "y": 7.81349105555354 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 481626e..85c7735 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,7 +41,7 @@ public static final class DriveConstants { // TODO: set motor and encoder constants public static final int kFrontLeftDriveMotorPort = 32; public static final int kRearLeftDriveMotorPort = 29; - public static final int kFrontRightDriveMotorPort = 38; + public static final int kFrontRightDriveMotorPort = 36; public static final int kRearRightDriveMotorPort = 34; public static final int kFrontLeftTurningMotorPort = 28; @@ -98,15 +98,15 @@ public static final class DriveConstants { public static final class IntakeConstants { public static final int kIntakeMotorID = 25; public static final int kArmMotorID = 39; - public static final int kArmEncoderChannel = 0; + public static final int kArmEncoderChannel = 1; // In degrees - public static final double kIntakeLoweredAngle = -193; + public static final double kIntakeLoweredAngle = -188; public static final double kIntakeRaisedAngle = 0; public static final double kIntakeAmpScoringAngle = -71; // 193 - 100 (previous angle) /** Encoder offset in rotations */ - public static final double kArmEncoderOffset = 0.715; + public static double kArmEncoderOffset = 0.259; public static final double kIntakeSpeed = 0.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2c4b878..5420005 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -37,6 +38,7 @@ import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.ServoSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; @@ -57,15 +59,25 @@ public class RobotContainer { private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); +// private final ServoSubsystem m_servoSubsystem = new ServoSubsystem(); + private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); private final SendableChooser autoChooser; + private final Servo m_ampServo = new Servo(1); + /** * The container for the robot. Contains subsystems, IO devices, and commands. */ public RobotContainer() { + + SmartDashboard.putBoolean("fast", false); + SmartDashboard.putBoolean("shoot fast", false); + + + NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), @@ -145,24 +157,21 @@ public RobotContainer() { scaleJoysticks(-m_driverController.getLeftY(), -m_driverController.getLeftX()), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getRightTriggerAxis() + * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1) * IOConstants.kSlowModeScalar), // * 0.8, MathUtil.applyDeadband( scaleJoysticks(-m_driverController.getLeftX(), -m_driverController.getLeftY()), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getRightTriggerAxis() + * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1) * IOConstants.kSlowModeScalar), // * 0.8, MathUtil.applyDeadband( -m_driverController.getRightX(), IOConstants.kControllerDeadband) * DriveConstants.kMaxAngularSpeedRadiansPerSecond - * (1 - m_driverController - .getRightTriggerAxis() + * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1) * IOConstants.kSlowModeScalar) * 0.7, !m_driverController.getLeftBumper()), @@ -196,10 +205,54 @@ private void configureBindings() { new JoystickButton(m_driverController, Button.kX.value) .onTrue(new SequentialCommandGroup( - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), + new ShooterSetSpeedCommand(m_shooterSubsystem, SmartDashboard.getBoolean("shoot fast", false) ? ShootSpeed.Shooting : ShootSpeed.Prep, ShooterConstants.kShooterOnTime), new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)))) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); + // new JoystickButton(m_driverController, Button.kA.value) + // .onTrue(new InstantCommand(() -> m_ampServo.setAngle(135))); + + // i need some more sleep + new JoystickButton(m_driverController, Button.kY.value) + .onTrue(new InstantCommand(() -> { + m_intakeSubsystem.getCurrentCommand().cancel(); + m_intakeSubsystem.resetArmEncoder(); + DriverStation.reportError("reset intake in", false); + })); + + // soft reset, should stop driving + // new JoystickButton(m_driverController, Button.kA.value) + // .onTrue(new InstantCommand(() -> m_intakeSubsystem.reset())); + + new JoystickButton(m_driverController, Button.kA.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.resetEvenHarder())); + + // slightly harder reset + new JoystickButton(m_driverController, Button.kB.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.resetHard())); + + // i better run + // new JoystickButton(m_driverController, Button.kX.value) + // .onTrue(new InstantCommand(() -> m_intakeSubsystem.daveImSorry())); + + new JoystickButton(m_driverController, Button.kBack.value) + .onTrue(new InstantCommand(() -> m_intakeSubsystem.heCantKillMeTwice())); + + // new JoystickButton(m_driverController, Button.kX.value) + // .onTrue(new InstantCommand(() -> { + // // m_intakeSubsystem.getCurrentCommand().cancel(); + // DriverStation.reportError("reset intake out", false); + // })); + + // new JoystickButton(m_driverController, Button.kA.value) + // .onTrue(new InstantCommand(() -> m_servoSubsystem.setPulseWidth(2500))); + + // new JoystickButton(m_driverController, Button.kB.value) + // .onTrue(new InstantCommand(() -> m_ampServo.setAngle(-20))); + + // new JoystickButton(m_driverController, Button.kB.value) + // .onTrue(new InstantCommand(() -> m_servoSubsystem.setPulseWidth(1500))); + new JoystickButton(m_driverController, Button.kRightBumper.value) .onTrue(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp)) .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); @@ -219,6 +272,10 @@ private void configureBindings() { return m_operatorController.getRightTriggerAxis() > 0.5; }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem)); + new Trigger(() -> { + return m_operatorController.getAButton() && !m_operatorController.getRightBumper(); + }).onTrue(new InstantCommand(() -> m_shooterSubsystem.setShootingSpeed(ShootSpeed.Amp))).onFalse(new InstantCommand(() -> m_shooterSubsystem.setShootingSpeed(ShootSpeed.Off))); + // Amp Outtake, Operator Controller X Button new JoystickButton(m_operatorController, Button.kX.value) .whileTrue(new AmpOuttakeCommand(m_intakeSubsystem)); @@ -231,7 +288,7 @@ private void configureBindings() { // Spin up Shooter, Operator Controller Left Trigger new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; - }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime)) + }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, SmartDashboard.getBoolean("shoot fast", false) ? ShootSpeed.Shooting : ShootSpeed.SlowShoot, ShooterConstants.kShooterOnTime)) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); // Climber Up, Operator Controller Right Bumper + A Button diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index cc211db..4fe897f 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -68,6 +68,7 @@ public void reverse() { public Value getState(){ return m_state; + // return Value.kOff; } /** diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a0a33dd..740d13f 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -58,6 +58,40 @@ public void reset() { m_armSetpoint = getArmPosition(); } + /** + * what time is it! + * + * + * + * bed time + */ + public void resetHard() { + reset(); + + m_armEncoder.reset(); + } + + /** CS swears the problem isnt their fault yet here I am */ + public void resetEvenHarder() { + reset(); + + // m_armEncoder.reset(); + m_armEncoder.setPositionOffset(0.7); + // m_armEncoder.reset(); + } + + /** just play some hans zimmer and hope he gets distracted */ + public void daveImSorry() { + IntakeConstants.kArmEncoderOffset = m_armEncoder.getAbsolutePosition(); + } + + /** i wish i was at the hans zimmer concert rn */ + public void heCantKillMeTwice() { + SmartDashboard.putNumber("yolo", m_armEncoder.getAbsolutePosition() - IntakeConstants.kIntakeLoweredAngle); + IntakeConstants.kArmEncoderOffset = m_armEncoder.getAbsolutePosition() - IntakeConstants.kIntakeLoweredAngle; + // m_armSetpoint = m_armEncoder.get() - 60; + } + public void setArmPosition(ArmPosition position) { m_armPosition = position; switch (position) { @@ -155,4 +189,18 @@ public static enum ArmPosition { public void colorSensorToggle() { m_colorSensorToggle = !m_colorSensorToggle; } + + /** honestly its been a rough day i dont care enough + * + * yolo + */ + public void resetArmEncoder() { + // Double initalizing the encoder crashes the code + // m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel); + + // m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset); + // m_armEncoder.setDistancePerRotation(360); + + // m_armSetpoint = m_armEncoder.getDistance(); + } } diff --git a/src/main/java/frc/robot/subsystems/ServoSubsystem.java b/src/main/java/frc/robot/subsystems/ServoSubsystem.java new file mode 100644 index 0000000..5efc727 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ServoSubsystem.java @@ -0,0 +1,54 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * Currently this subsystem is just in case anything goes wrong with the normal, default Servo + * implementation in WPILib because the servo we have might not work with that pulse length + * + * current stats of MG995 servo: + * 4.8 to 7.2 V power and signal + * 20ms (50 Hz) PWM period + * 5 microsecond dead band width + * Rotate about 120 degrees (60 in each direction) + * + * + * different source claims that it "provides a running angle of about 180 degrees over a servo + * pulse range of 600 us to 2400 us" + * + * studica claims pulse width range of 500 to 250 us with a neutral position of 1500 us and a + * dead band width of 4 us + * + * another source: https://components101.com/motors/mg995-servo-motor + * + * WATCH OUT FOR MS VS US + * a cycle for the servo has to be 20ms (50 Hz) so a pulse width of 1500 us would be 1.5 ms + */ +public class ServoSubsystem extends SubsystemBase { + + private PWM m_iloveservos = new PWM(1); + + private int m_pulseWidth = 1500; + + /** Creates a new ServoSubsystem. */ + public ServoSubsystem() { + m_iloveservos.setBoundsMicroseconds(20000, 2500, 1500, 500, 20000); + // m_iloveservos.setB + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + m_iloveservos.setPulseTimeMicroseconds(m_pulseWidth); + } + + /** Set pulse width in MICROSECONDS (us) */ + public void setPulseWidth(int pulseWidth) { + m_pulseWidth = pulseWidth; + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 54ffc08..be02068 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -46,9 +46,12 @@ public void setShootingSpeed(ShootSpeed speed) { m_topSpeed = ShooterConstants.kShooterSpeedTop; m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; break; + case SlowShoot: + // m_topSpeed = case Amp: m_topSpeed = 0.2; m_bottomSpeed = 0.3; + break; case Prep: m_topSpeed = ShooterConstants.kPrepShooterSpeed; m_bottomSpeed = ShooterConstants.kPrepShooterSpeed; @@ -80,6 +83,7 @@ public static enum ShootSpeed { Shooting, Prep, Amp, - Off + Off, + SlowShoot } }