diff --git a/src/main/deploy/pathplanner/autos/Center Subwoofer 1S.auto b/src/main/deploy/pathplanner/autos/Center Subwoofer 1S.auto new file mode 100644 index 0000000..42fd024 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Subwoofer 1S.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.335622956146197, + "y": 5.5196408968316515 + }, + "rotation": 179.37978729180955 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Charge Shooter" + } + }, + { + "type": "named", + "data": { + "name": "Speaker Note Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Subwoofer to Center" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Subwoofer Center 1S.auto b/src/main/deploy/pathplanner/autos/Left Subwoofer Center 1S.auto new file mode 100644 index 0000000..07d4e33 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Subwoofer Center 1S.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto b/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto index 166a05f..b7048f2 100644 --- a/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto +++ b/src/main/deploy/pathplanner/autos/Right Subwoofer 1S.auto @@ -17,6 +17,12 @@ "name": "Charge Shooter" } }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path b/src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path new file mode 100644 index 0000000..93629d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.335622956146197, + "y": 5.5196408968316515 + }, + "prevControl": null, + "nextControl": { + "x": 2.3356229561461985, + "y": 5.5196408968316515 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.072055095677142, + "y": 5.028486410249174 + }, + "prevControl": { + "x": 3.364392809755599, + "y": 5.618204981850461 + }, + "nextControl": { + "x": 4.843869288878178, + "y": 4.385307915914977 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.299641721790566, + "y": 0.877061583182996 + }, + "prevControl": { + "x": 6.773404771880769, + "y": 4.046177437084219 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9, + "rotationDegrees": 90.25335933192582, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 179.04515874612784, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 20a357a..7189454 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -189,7 +189,7 @@ public class RobotContainer { ); // Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back - driver.a().onTrue( + driver.leftBumper().onTrue( drivetrain.driveAprilTagLock( driver::getLeftY, driver::getLeftX, @@ -260,6 +260,13 @@ public class RobotContainer { operator.y().whileTrue(Commands.parallel(climber.setSpeedWithSupplier(operator::getRightTriggerAxis), shooter.climbState())); driver.a().whileTrue(indexer.shootNote(() -> 1.0)); + + operator.back().toggleOnTrue(shooter.manualPivot( + () -> operator.getLeftY(), + () -> operator.getRightTriggerAxis()-operator.getLeftTriggerAxis() + )); + + operator.start().onTrue(shooter.zeroEncoder()); } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 4c577d0..5fe793b 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -29,8 +29,10 @@ public class Climber extends SubsystemBase { public Command setSpeedWithSupplier(DoubleSupplier speed) { return run(() -> { - if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){ + if(shooterAngle.getAsDouble() > Math.toRadians(-5.0)){ motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble()); + }else{ + motor1.set(VictorSPXControlMode.PercentOutput, 0); } }); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 3e35e08..40f73d0 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,8 +4,6 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -21,9 +19,6 @@ public class Shooter extends SubsystemBase{ private CANSparkMax topShooter; private CANSparkMax bottomShooter; - private RelativeEncoder bottomEncoder; - private RelativeEncoder topEncoder; - private CANSparkMax shooterPivot; private Encoder pivotEncoder; @@ -42,9 +37,6 @@ public class Shooter extends SubsystemBase{ shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless); shooterPivot.setInverted(true); - topEncoder = topShooter.getEncoder(); - bottomEncoder = bottomShooter.getEncoder(); - /* topPID = topShooter.getPIDController(); topPID.setFeedbackDevice(topEncoder); @@ -137,6 +129,12 @@ public class Shooter extends SubsystemBase{ return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle; } + public Command zeroEncoder(){ + return run(() -> { + pivotEncoder.reset(); + }); + } + public Boolean getBeamBreak(){ return shooterBeamBreak.get(); }