From c7ee873b7eb8dfeab0e9d6d6f44b1f0551d1e407 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sat, 9 Mar 2024 18:52:33 -0500 Subject: [PATCH] Auto heading control inverted HELP --- .../pathplanner/autos/He He He Haw.auto | 25 ++++++ .../pathplanner/autos/Right 2S Center.auto | 62 +++++++++++++++ .../pathplanner/paths/He He He Haw.path | 74 ++++++++++++++++++ .../pathplanner/paths/Left Subwoofer.path | 2 +- .../paths/Right Sub from Center.path | 52 +++++++++++++ .../paths/Right Sub to Right Center Note.path | 76 +++++++++++++++++++ .../deploy/pathplanner/paths/Right Sub.path | 52 +++++++++++++ src/main/java/frc/robot/RobotContainer.java | 21 +++-- .../frc/robot/constants/AutoConstants.java | 6 +- .../robot/constants/DrivetrainConstants.java | 6 ++ .../frc/robot/constants/IntakeConstants.java | 9 ++- .../java/frc/robot/subsystems/Drivetrain.java | 4 + .../java/frc/robot/subsystems/Indexer.java | 4 +- .../java/frc/robot/subsystems/Intake.java | 70 ++++++++++++++--- .../frc/robot/utilities/MAXSwerveModule.java | 4 + 15 files changed, 441 insertions(+), 26 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/He He He Haw.auto create mode 100644 src/main/deploy/pathplanner/autos/Right 2S Center.auto create mode 100644 src/main/deploy/pathplanner/paths/He He He Haw.path create mode 100644 src/main/deploy/pathplanner/paths/Right Sub from Center.path create mode 100644 src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path create mode 100644 src/main/deploy/pathplanner/paths/Right Sub.path diff --git a/src/main/deploy/pathplanner/autos/He He He Haw.auto b/src/main/deploy/pathplanner/autos/He He He Haw.auto new file mode 100644 index 0000000..80b89eb --- /dev/null +++ b/src/main/deploy/pathplanner/autos/He He He Haw.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.4585613729632017, + "y": 7.0398809743488435 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "He He He Haw" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2S Center.auto b/src/main/deploy/pathplanner/autos/Right 2S Center.auto new file mode 100644 index 0000000..4b160af --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2S Center.auto @@ -0,0 +1,62 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.52, + "y": 4.034483282641779 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Sub" + } + }, + { + "type": "named", + "data": { + "name": "Charge Shooter" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Speaker Note Shot" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Sub to Right Center Note" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Sub from Center" + } + }, + { + "type": "named", + "data": { + "name": "Speaker Note Shot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/He He He Haw.path b/src/main/deploy/pathplanner/paths/He He He Haw.path new file mode 100644 index 0000000..49b6898 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/He He He Haw.path @@ -0,0 +1,74 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.4585613729632017, + "y": 7.0398809743488435 + }, + "prevControl": null, + "nextControl": { + "x": 1.4585613729632003, + "y": 7.0398809743488435 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7856981070087494, + "y": 5.98740707452925 + }, + "prevControl": { + "x": 1.7856981070087494, + "y": 5.98740707452925 + }, + "nextControl": { + "x": 3.7856981070087494, + "y": 5.98740707452925 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.69184528112646, + "y": 7.051575128791284 + }, + "prevControl": { + "x": 4.238771694067605, + "y": 6.519491101660266 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 90.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.210089391754, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Subwoofer.path b/src/main/deploy/pathplanner/paths/Left Subwoofer.path index 78479a7..388d4ab 100644 --- a/src/main/deploy/pathplanner/paths/Left Subwoofer.path +++ b/src/main/deploy/pathplanner/paths/Left Subwoofer.path @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -146.30993247402017, + "rotation": 149.21037688376774, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Right Sub from Center.path b/src/main/deploy/pathplanner/paths/Right Sub from Center.path new file mode 100644 index 0000000..6f57093 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Sub from Center.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.335023775460655, + "y": 1.1577212898015548 + }, + "prevControl": null, + "nextControl": { + "x": 3.803089543501023, + "y": 0.9472265098376356 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0432690950851984, + "y": 4.478861151454497 + }, + "prevControl": { + "x": 2.013883913807713, + "y": 1.5436283864020721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 143.130102354156, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path b/src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path new file mode 100644 index 0000000..dcc97e8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.335622956146197, + "y": 4.315142989260338 + }, + "prevControl": null, + "nextControl": { + "x": 1.8735540604984342, + "y": 1.4149926875352343 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.603689737294005, + "y": 0.8068966565283563 + }, + "prevControl": { + "x": 6.603689737294005, + "y": 0.8068966565283563 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Note Pickup", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Note Pickup" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 139.21417852273407, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Sub.path b/src/main/deploy/pathplanner/paths/Right Sub.path new file mode 100644 index 0000000..f8149fa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Sub.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.52, + "y": 4.034483282641779 + }, + "prevControl": null, + "nextControl": { + "x": 1.0783515584179242, + "y": 3.5199404871715854 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0783515584179242, + "y": 4.53733192366386 + }, + "prevControl": { + "x": 1.85016575161896, + "y": 4.034483282638943 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 143.6955028774248, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -178.76146677965983, + "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 81c5ea8..bb3c0a7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,8 @@ import java.util.Optional; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.util.PPLibTelemetry; +import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; @@ -97,6 +99,7 @@ public class RobotContainer { // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand()); + // TODO Specify a default auto string once we have one autoChooser = AutoBuilder.buildAutoChooser(); @@ -134,8 +137,8 @@ public class RobotContainer { drivetrain.setDefaultCommand( drivetrain.teleopCommand( - driver::getLeftY, - driver::getLeftX, + () -> { return -driver.getLeftY(); }, + () -> { return -driver.getLeftX(); }, () -> { return -driver.getRightX(); }, OIConstants.kTeleopDriveDeadband ) @@ -154,9 +157,7 @@ public class RobotContainer { } }, () -> { - if (driver.getLeftTriggerAxis() > .25) { - return -1.0; - }else if(driver.getRightTriggerAxis() > 0.25){ + if(driver.getRightTriggerAxis() > 0.25){ return 1.0; } else { return 0; @@ -270,6 +271,11 @@ public class RobotContainer { driver.povDown().whileTrue(indexer.shootNote(() -> -1.0)); operator.start().onTrue(shooter.zeroEncoder()); + + operator.povDown().whileTrue(intake.intakeDownCommand()); + operator.povUp().whileTrue(intake.intakeUpCommand()); + + driver.leftTrigger().whileTrue(Commands.parallel( intake.manualPivot(() -> 0.0, () -> 1.0), indexer.advanceNote())); } @@ -293,7 +299,10 @@ public class RobotContainer { teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak); teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak); teleopTab.addDouble("shooter angle", shooter::getShooterAngle); - teleopTab.addDouble("intake angle", intake::getIntakeAngle); + teleopTab.addDouble("intake angle", intake::getIntakeDegrees); + teleopTab.addDouble("intake pid", intake::getIntakePID); + teleopTab.addDouble("heading swerve", drivetrain::getHeading); + teleopTab.addDouble("steering encoder", drivetrain::getEncoderSteering); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 3d521dc..a4fda86 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -13,9 +13,9 @@ public final class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 1.05; - public static final double kPYController = 1.05; - public static final double kPThetaController = 0.95; // needs to be separate from heading control + public static final double kPXController = 5; + public static final double kPYController = 5; + public static final double kPThetaController = 0.5; // needs to be separate from heading control public static final double kPHeadingController = 0.02; // for heading control NOT PATHING public static final double kDHeadingController = 0.0025; diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 5eaedc0..5cc7784 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -26,10 +26,16 @@ public final class DrivetrainConstants { new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); // Angular offsets of the modules relative to the chassis in radians + /* public static final double kFrontLeftChassisAngularOffset = Math.PI; public static final double kFrontRightChassisAngularOffset = -Math.PI / 2; public static final double kBackLeftChassisAngularOffset = Math.PI / 2; public static final double kBackRightChassisAngularOffset = 0; +*/ + public static final double kFrontLeftChassisAngularOffset = -2*Math.PI; + public static final double kFrontRightChassisAngularOffset = -2*Math.PI; + public static final double kBackLeftChassisAngularOffset = -2*Math.PI; + public static final double kBackRightChassisAngularOffset = -2*Math.PI-(4/180.0); // SPARK MAX CAN IDs public static final int kFrontLeftDrivingCanId = 8; diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 3a0a547..0b0d832 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -4,19 +4,20 @@ public class IntakeConstants { public static final int kIntakePivotID = 10; public static final int kIntakeRollerID = 12; - public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0)); + public static final double kIntakePivotConversionFactor = (20.0*(28.0/15.0)); public static final int kPivotCurrentLimit = 40; - public static final double kPIntake = 0; + public static final double kPIntake = 2.5; public static final double kIIntake = 0; public static final double kDIntake = 0; public static final double kSFeedForward = 0; - public static final double kGFeedForward = 1.11; - public static final double kVFeedForward = 0.73; + public static final double kGFeedForward = 0;//1.11; + public static final double kVFeedForward = 0;//0.73; public static final double kStartingAngle = Math.toRadians(105.0); public static final double kUpAngle = Math.toRadians(90.0); public static final double kDownAngle = Math.toRadians(-34.0); + } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index ee790f8..815d8e1 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -451,4 +451,8 @@ public class Drivetrain extends SubsystemBase { public double getTurnRate() { return m_gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); } + + public double getEncoderSteering(){ + return m_frontLeft.steeringEncoder(); + } } diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index a07eed8..8d07d53 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -41,9 +41,9 @@ public class Indexer extends SubsystemBase{ public Command advanceNote(){ return run(() -> { if(indexerBeamBreak.get()){ - indexerMotor.set(0.75); + indexerMotor.set(1); }else{ - indexerMotor.set(0.75); + indexerMotor.set(0.0); } }); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 17f2649..306ac24 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import com.revrobotics.CANSparkMax; @@ -9,6 +10,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.IntakeConstants; @@ -23,7 +25,11 @@ public class Intake extends SubsystemBase{ private ArmFeedforward intakeFeedForward; + private double armOffset; + public Intake(){ + armOffset = 0; + intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless); intakeRoller.setSmartCurrentLimit(60); @@ -43,8 +49,8 @@ public class Intake extends SubsystemBase{ ); intakeEncoder = intakePivot.getEncoder(); - intakeEncoder.setPosition(IntakeConstants.kStartingAngle); - intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor); + intakeEncoder.setPosition(Units.radiansToRotations( IntakeConstants.kStartingAngle)); + // intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor); intakeAnglePID = new PIDController( IntakeConstants.kPIntake, @@ -52,24 +58,54 @@ public class Intake extends SubsystemBase{ IntakeConstants.kDIntake ); + armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle; } public Command intakeDownCommand() { return run(() -> { - intakeRoller.set(1.0); + intakeRoller.set(0.0); intakePivot.setVoltage( intakeAnglePID.calculate( - intakeEncoder.getPosition(), + getIntakeAngle(), IntakeConstants.kDownAngle ) + intakeFeedForward.calculate( IntakeConstants.kDownAngle, - intakeEncoder.getVelocity() + 0.0 ) ); }); } + public Command autoIntaking(BooleanSupplier beamBreak){ + return run(() -> { + if(beamBreak.getAsBoolean()){ + intakeRoller.set(1.0); + + intakePivot.setVoltage( + intakeAnglePID.calculate( + getIntakeAngle(), + IntakeConstants.kUpAngle + ) + intakeFeedForward.calculate( + IntakeConstants.kUpAngle, + 0.0 + ) + ); + }else{ + intakeRoller.set(0); + intakePivot.setVoltage( + intakeAnglePID.calculate( + getIntakeAngle(), + IntakeConstants.kDownAngle + ) + intakeFeedForward.calculate( + IntakeConstants.kDownAngle, + 0.0 + ) + ); + } + }); + } + public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){ return run(() ->{ intakePivot.set(pivotPower.getAsDouble()); @@ -83,11 +119,11 @@ public class Intake extends SubsystemBase{ intakePivot.setVoltage( intakeAnglePID.calculate( - intakeEncoder.getPosition(), + getIntakeAngle(), IntakeConstants.kDownAngle ) + intakeFeedForward.calculate( IntakeConstants.kDownAngle, - intakeEncoder.getVelocity() + 0.0 ) ); }); @@ -99,17 +135,31 @@ public class Intake extends SubsystemBase{ intakePivot.setVoltage( intakeAnglePID.calculate( - intakeEncoder.getPosition(), + getIntakeAngle(), IntakeConstants.kUpAngle ) + intakeFeedForward.calculate( IntakeConstants.kUpAngle, - intakeEncoder.getVelocity() + 0.0 ) ); }); } public double getIntakeAngle(){ - return intakeEncoder.getPosition(); + return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset; + } + + public double getIntakePID(){ + return intakeAnglePID.calculate( + getIntakeAngle(), + IntakeConstants.kDownAngle + ) + intakeFeedForward.calculate( + IntakeConstants.kDownAngle, + 0.0 + ); + } + + public double getIntakeDegrees(){ + return Math.toDegrees(getIntakeAngle()); } } diff --git a/src/main/java/frc/robot/utilities/MAXSwerveModule.java b/src/main/java/frc/robot/utilities/MAXSwerveModule.java index de4dad6..ec84461 100644 --- a/src/main/java/frc/robot/utilities/MAXSwerveModule.java +++ b/src/main/java/frc/robot/utilities/MAXSwerveModule.java @@ -157,4 +157,8 @@ public class MAXSwerveModule { public void resetEncoders() { m_drivingEncoder.setPosition(0); } + + public double steeringEncoder(){ + return m_turningEncoder.getPosition(); + } } \ No newline at end of file