Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

working on pathplanner

Browse Source
This commit is contained in:
Team 2648 2025-02-22 18:37:50 -05:00
parent 87e7eb4974
commit eb00b1146e
11 changed files with 201 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main/deploy/pathplanner/autos/fein.auto Normal file
View File

@ -0,0 +1,19 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "fein"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"choreoAuto": false
}

70
src/main/deploy/pathplanner/paths/fein.path Normal file
View File

@ -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
}

18
src/main/java/frc/robot/RobotContainer.java
View File

@ -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);
}

2
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -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

10
src/main/java/frc/robot/constants/ElevatorConstants.java
View File

@ -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;

6
src/main/java/frc/robot/constants/ManipulatorPivotConstants.java
View File

@ -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);

2
src/main/java/frc/robot/constants/ModuleConstants.java
View File

@ -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;

2
src/main/java/frc/robot/constants/NeoMotorConstants.java
View File

@ -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
}

4
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -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();
}
}

75
src/main/java/frc/robot/subsystems/Elevator.java
View File

@ -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)
/*
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{
else{
elevatorMotor1.setVoltage(
pidControllerDown.calculate(
encoder.getPosition()
) + feedForward.calculate(0)
MathUtil.clamp(
downOutput + feedForward.calculate(0), -1, 1)
);
}
} else {
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}*/
*/
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
});
}
@ -182,6 +182,7 @@ public class Elevator extends SubsystemBase {
*/
public Command goToSetpoint(DoubleSupplier setpoint) {
if (setpoint.getAsDouble() == 0) {
return startRun(() -> {
pidControllerUp.reset();
@ -189,24 +190,52 @@ public class Elevator extends SubsystemBase {
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)
elevatorMotor1.setVoltage( MathUtil.clamp(
upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
);
else{
elevatorMotor1.setVoltage(
downOutput + feedForward.calculate(0)
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());
}
/*

4
src/main/java/frc/robot/subsystems/MAXSwerveModule.java
View File

@ -138,4 +138,8 @@ public class MAXSwerveModule {
public void resetEncoders() {
m_drive.setPosition(0);
}
public double getTotalDist(){
return m_drive.getPosition().getValueAsDouble();
}
}