From eb00b1146e8e7a0f36b8484a4c892eed43a82566 Mon Sep 17 00:00:00 2001 From: Team 2648 Date: Sat, 22 Feb 2025 18:37:50 -0500 Subject: [PATCH] working on pathplanner --- src/main/deploy/pathplanner/autos/fein.auto | 19 +++ src/main/deploy/pathplanner/paths/fein.path | 70 ++++++++++ src/main/java/frc/robot/RobotContainer.java | 18 ++- .../robot/constants/DrivetrainConstants.java | 2 +- .../robot/constants/ElevatorConstants.java | 10 +- .../constants/ManipulatorPivotConstants.java | 6 +- .../frc/robot/constants/ModuleConstants.java | 2 +- .../robot/constants/NeoMotorConstants.java | 2 +- .../java/frc/robot/subsystems/Drivetrain.java | 4 + .../java/frc/robot/subsystems/Elevator.java | 125 +++++++++++------- .../frc/robot/subsystems/MAXSwerveModule.java | 4 + 11 files changed, 201 insertions(+), 61 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/fein.auto create mode 100644 src/main/deploy/pathplanner/paths/fein.path diff --git a/src/main/deploy/pathplanner/autos/fein.auto b/src/main/deploy/pathplanner/autos/fein.auto new file mode 100644 index 0000000..dbd2a5c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/fein.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "fein" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/fein.path b/src/main/deploy/pathplanner/paths/fein.path new file mode 100644 index 0000000..9504997 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/fein.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.600204918039607, + "y": 6.374590163938041 + }, + "prevControl": null, + "nextControl": { + "x": 7.600204918032786, + "y": 7.573360655737705 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.459426229508197, + "y": 5.978995901639344 + }, + "prevControl": { + "x": 5.370491803278689, + "y": 5.978995901639344 + }, + "nextControl": { + "x": 2.995158619919281, + "y": 5.978995901639344 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6543032786885246, + "y": 6.254713114754098 + }, + "prevControl": { + "x": 2.613319672131147, + "y": 6.278688524590164 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.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 6420ef4..0b30b53 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { manipulator.setDefaultCommand( manipulator.runUntilCollected( - () -> 0 + () -> 0.0 ) ); @@ -136,11 +136,19 @@ public class RobotContainer { ); driver.rightTrigger().whileTrue( - manipulator.runManipulator(() -> 1, true) + manipulator.runManipulator(() -> 0.35, true) ); driver.leftTrigger().whileTrue( - manipulator.runUntilCollected(() -> 0.75) + manipulator.runUntilCollected(() -> 0.35) + ); + + driver.leftBumper().whileTrue( + manipulator.runUntilCollected(() -> 0.5) + ); + + driver.rightBumper().whileTrue( + manipulator.runManipulator(() -> 1, false) ); driver.start().and(driver.back()).onTrue( @@ -154,6 +162,7 @@ public class RobotContainer { driver.povLeft().whileTrue(climberRollers.runRoller(0.5)); driver.povRight().whileTrue(climberRollers.runRoller(-0.5)); + driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false)); //Operator inputs operator.povUp().onTrue( @@ -274,6 +283,9 @@ public class RobotContainer { sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition); + sensorTab.addDouble("dt distance", drivetrain::driveDistance); + + } diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 9bcf84b..7d57c66 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; public class DrivetrainConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds - public static final double kMaxSpeedMetersPerSecond = 4.8; + public static final double kMaxSpeedMetersPerSecond = 5.5; public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second // Chassis configuration diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index f531601..01ea613 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -41,17 +41,19 @@ public class ElevatorConstants { public static final double kCoralIntakePosition = 0; public static final double kL1Position = 0; - public static final double kL2Position = 14.5; - public static final double kL3Position = 29.0; - public static final double kL4Position = 49.0; + public static final double kL2Position = 8; + public static final double kL3Position = 25.0; + public static final double kL4Position = 50.5; public static final double kL4TransitionPosition = 40.0; - public static final double kL2AlgaePosition = 22.0; + public static final double kL2AlgaePosition = 21.0; public static final double kL3AlgaePosition = 39.0; public static final double kProcessorPosition = 4.0; /**The position of the top of the elevator brace */ public static final double kBracePosition = 0; public static final double kMaxHeight = 51.0; //actual is 51 + public static final double kVoltageLimit = 7; + // 1, 7, 10 are the defaults for these, change as necessary public static final double kSysIDRampRate = .25; public static final double kSysIDStepVolts = 3; diff --git a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java index d4e27e0..9134148 100644 --- a/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java +++ b/src/main/java/frc/robot/constants/ManipulatorPivotConstants.java @@ -34,12 +34,12 @@ public class ManipulatorPivotConstants { public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100 public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168 - public static final double kEncoderOffset = 0.781; + public static final double kEncoderOffset = 0.780; public static final double kCoralIntakePosition = Units.degreesToRadians(175.0); public static final double kL1Position = Units.degreesToRadians(0.0); - public static final double kL2Position = Units.degreesToRadians(60.0); - public static final double kL3Position = Units.degreesToRadians(60.0); + public static final double kL2Position = Units.degreesToRadians(22.0); + public static final double kL3Position = Units.degreesToRadians(22.0); public static final double kL4Position = Units.degreesToRadians(45.0); public static final double kL2AlgaePosition = Units.degreesToRadians(175.0); public static final double kL3AlgaePosition = Units.degreesToRadians(175.0); diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/ModuleConstants.java index 2048d3d..d52fff7 100644 --- a/src/main/java/frc/robot/constants/ModuleConstants.java +++ b/src/main/java/frc/robot/constants/ModuleConstants.java @@ -22,7 +22,7 @@ public class ModuleConstants { public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI; // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 // teeth on the bevel pinion - public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + public static final double kDrivingMotorReduction = (45.0 * 20) / (kDrivingMotorPinionTeeth * 15); public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters) / kDrivingMotorReduction; diff --git a/src/main/java/frc/robot/constants/NeoMotorConstants.java b/src/main/java/frc/robot/constants/NeoMotorConstants.java index 3bbe106..1f353e9 100644 --- a/src/main/java/frc/robot/constants/NeoMotorConstants.java +++ b/src/main/java/frc/robot/constants/NeoMotorConstants.java @@ -1,5 +1,5 @@ package frc.robot.constants; public class NeoMotorConstants { - public static final double kFreeSpeedRpm = 5676; + public static final double kFreeSpeedRpm = 6000; //for kraken not neo } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index b0d6239..66711ea 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -281,4 +281,8 @@ public class Drivetrain extends SubsystemBase { public void addVisionMeasurement(Pose2d pose, double timestamp){ m_estimator.addVisionMeasurement(pose, timestamp); } + + public double driveDistance(){ + return m_frontLeft.getTotalDist(); + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 9059e80..aa89493 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -76,6 +76,8 @@ public class Elevator extends SubsystemBase { pidControllerUp.setTolerance(ElevatorConstants.kAllowedError); + + feedForward = new ElevatorFeedforward( ElevatorConstants.kFeedForwardS, ElevatorConstants.kFeedForwardG, @@ -141,34 +143,32 @@ public class Elevator extends SubsystemBase { public Command maintainPosition() { return startRun(() -> { - System.out.println("maintain position"); - +/* + pidControllerUp.reset(); + pidControllerDown.reset(); + */ }, () -> { - /* - if (!pidControllerUp.atSetpoint()) { - if(encoder.getPosition()>pidControllerUp.getSetpoint()){ - elevatorMotor1.setVoltage( - pidControllerUp.calculate( - encoder.getPosition() - ) + feedForward.calculate(0) - ); - }else{ - elevatorMotor1.setVoltage( - pidControllerDown.calculate( - encoder.getPosition() - ) + feedForward.calculate(0) - ); - } - } else { - elevatorMotor1.setVoltage( - feedForward.calculate(0) +/* + double upOutput = pidControllerUp.calculate(getEncoderPosition()); + double downOutput = pidControllerDown.calculate(getEncoderPosition()); + + if(pidControllerUp.getSetpoint()>encoder.getPosition()) + elevatorMotor1.setVoltage( MathUtil.clamp( + upOutput + feedForward.calculate(0), -1, 1) ); - }*/ + else{ + elevatorMotor1.setVoltage( + MathUtil.clamp( + downOutput + feedForward.calculate(0), -1, 1) + ); + } + */ elevatorMotor1.setVoltage( - feedForward.calculate(0) - ); + feedForward.calculate(0) + ); + }); } @@ -181,33 +181,62 @@ public class Elevator extends SubsystemBase { * @return Sets motor voltage to achieve the target destination */ public Command goToSetpoint(DoubleSupplier setpoint) { - - return startRun(() -> { + + if (setpoint.getAsDouble() == 0) { + return startRun(() -> { - pidControllerUp.reset(); - pidControllerDown.reset(); - pidControllerUp.setSetpoint(setpoint.getAsDouble()); - pidControllerDown.setSetpoint(setpoint.getAsDouble()); - - - System.out.println("go to setpoint"); - }, - () -> { - double upOutput = pidControllerUp.calculate(getEncoderPosition()); - double downOutput = pidControllerDown.calculate(getEncoderPosition()); - - if(setpoint.getAsDouble()>encoder.getPosition()) - elevatorMotor1.setVoltage( - upOutput + feedForward.calculate(0) - ); - else{ - elevatorMotor1.setVoltage( - downOutput + feedForward.calculate(0) - ); - } + pidControllerUp.reset(); + pidControllerDown.reset(); + pidControllerUp.setSetpoint(setpoint.getAsDouble()); + pidControllerDown.setSetpoint(setpoint.getAsDouble()); + + }, + () -> { + double upOutput = pidControllerUp.calculate(getEncoderPosition()); + double downOutput = pidControllerDown.calculate(getEncoderPosition()); + + if(setpoint.getAsDouble()>encoder.getPosition()) + elevatorMotor1.setVoltage( MathUtil.clamp( + upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit) + ); + else{ + elevatorMotor1.setVoltage( + MathUtil.clamp( + downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit) + ); + } + + }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint()) + .andThen(runManualElevator(() -> -.1) + .until(() -> encoder.getPosition() == 0)); + + } else { + return startRun(() -> { + + pidControllerUp.reset(); + pidControllerDown.reset(); + pidControllerUp.setSetpoint(setpoint.getAsDouble()); + pidControllerDown.setSetpoint(setpoint.getAsDouble()); + + }, + () -> { + double upOutput = pidControllerUp.calculate(getEncoderPosition()); + double downOutput = pidControllerDown.calculate(getEncoderPosition()); + + if(setpoint.getAsDouble()>encoder.getPosition()) + elevatorMotor1.setVoltage( MathUtil.clamp( + upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit) + ); + else{ + elevatorMotor1.setVoltage( + MathUtil.clamp( + downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit) + ); + } + + }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint()); + } - }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint()); - /* elevatorMotor1.setVoltage( diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index cf83963..b88bc3a 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -138,4 +138,8 @@ public class MAXSwerveModule { public void resetEncoders() { m_drive.setPosition(0); } + + public double getTotalDist(){ + return m_drive.getPosition().getValueAsDouble(); + } }