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

Compare commits

base: Team_2648:eb00b1146e8e7a0f36b8484a4c892eed43a82566
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup
..
compare: Team_2648:44a036f420b6911e9c6c74989983dffec98a9687
Branches Tags
Team_2648:main
Team_2648:faster_elevator
Team_2648:maybe_fix_field_oriented
Team_2648:brad_elevator_control_rework
Team_2648:kraken_swerve
Team_2648:2025-2-1-setup

No commits in common. "eb00b1146e8e7a0f36b8484a4c892eed43a82566" and "44a036f420b6911e9c6c74989983dffec98a9687" have entirely different histories.
eb00b1146e ... 44a036f420

11 changed files with 68 additions and 215 deletions
Show Stats Expand all files Collapse all files

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

@ -1,19 +0,0 @@
{
"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
View File

@ -1,70 +0,0 @@
{
"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
}

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

@ -71,7 +71,6 @@ public class RobotContainer {
configureButtonBindings();
//elevatorSysIDBindings();
//elevatorOnlyBindings();
configureNamedCommands();
@ -87,13 +86,6 @@ public class RobotContainer {
operator.y().whileTrue(elevator.sysIdDynamic(Direction.kReverse));
}*/
private void elevatorOnlyBindings(){
elevator.setDefaultCommand(elevator.maintainPosition());
manipulatorPivot.setDefaultCommand(manipulatorPivot.maintainPosition());
driver.a().onTrue(safeMoveManipulator(ElevatorConstants.kL2Position, ManipulatorPivotConstants.kL2Position));
}
private void configureButtonBindings() {
//Default commands
climberPivot.setDefaultCommand(
@ -125,7 +117,7 @@ public class RobotContainer {
manipulator.setDefaultCommand(
manipulator.runUntilCollected(
() -> 0.0
() -> 0
)
);
@ -136,33 +128,24 @@ public class RobotContainer {
);
driver.rightTrigger().whileTrue(
manipulator.runManipulator(() -> 0.35, true)
manipulator.runManipulator(() -> 1, true)
);
driver.leftTrigger().whileTrue(
manipulator.runUntilCollected(() -> 0.35)
);
driver.leftBumper().whileTrue(
manipulator.runUntilCollected(() -> 0.5)
);
driver.rightBumper().whileTrue(
manipulator.runManipulator(() -> 1, false)
manipulator.runUntilCollected(() -> 0.75)
);
driver.start().and(driver.back()).onTrue(
startingConfig()
);
driver.y().whileTrue(drivetrain.zeroHeading());
driver.y().onTrue(drivetrain.zeroHeading());
driver.povDown().whileTrue(climberPivot.runPivot(-0.5));
driver.povUp().whileTrue(climberPivot.runPivot(0.5));
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(
@ -194,15 +177,19 @@ public class RobotContainer {
);
operator.a().onTrue(
safeMoveManipulator(ElevatorConstants.kL1Position, 0.0)
coralIntakeRoutine()
);
operator.x().onTrue(
safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
algaeIntakeRoutine(true)
);
operator.b().onTrue(
safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
algaeIntakeRoutine(false)
);
operator.y().whileTrue(
elevator.goToSetpoint(() -> ElevatorConstants.kL2Position)
);
}
@ -283,9 +270,6 @@ public class RobotContainer {
sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition);
sensorTab.addDouble("dt distance", drivetrain::driveDistance);
}
@ -414,8 +398,8 @@ public class RobotContainer {
.deadlineFor(manipulatorPivot.goToSetpoint(() -> armPosition),
elevator.maintainPosition());*/
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kPivotSafeStowPosition)
.andThen(elevator.goToSetpoint(() -> elevatorPosition), manipulatorPivot.goToSetpoint(() -> armPosition)
.raceWith(elevator.maintainPosition()));
.andThen(elevator.goToSetpoint(() -> elevatorPosition))
.andThen(manipulatorPivot.goToSetpoint(() -> armPosition));
}
@SuppressWarnings("unused")

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 = 5.5;
public static final double kMaxSpeedMetersPerSecond = 4.8;
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
// Chassis configuration

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

@ -41,18 +41,16 @@ public class ElevatorConstants {
public static final double kCoralIntakePosition = 0;
public static final double kL1Position = 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 kL2Position = 14.5;
public static final double kL3Position = 29.0;
public static final double kL4Position = 52.0;
public static final double kL4TransitionPosition = 40.0;
public static final double kL2AlgaePosition = 21.0;
public static final double kL2AlgaePosition = 22.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;
public static final double kMaxHeight = 52.0; //actual is 52
// 1, 7, 10 are the defaults for these, change as necessary
public static final double kSysIDRampRate = .25;

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

@ -34,19 +34,19 @@ 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.780;
public static final double kEncoderOffset = 0.7815;
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(22.0);
public static final double kL3Position = Units.degreesToRadians(22.0);
public static final double kL2Position = Units.degreesToRadians(60.0);
public static final double kL3Position = Units.degreesToRadians(60.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);
public static final double kProcesserPosition = Units.degreesToRadians(175.0);
public static final double kNetPosition = Units.degreesToRadians(175.0);
/**The closest position to the elevator brace without hitting it */
public static final double kPivotSafeStowPosition = Units.degreesToRadians(71.0);
public static final double kPivotSafeStowPosition = Units.degreesToRadians(67.0);
/**The forward rotation limit of the pivot */
public static final double kRotationLimit = 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 * 20) / (kDrivingMotorPinionTeeth * 15);
public static final double kDrivingMotorReduction = (45.0 * 22) / (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 = 6000; //for kraken not neo
public static final double kFreeSpeedRpm = 5676;
}

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

@ -281,8 +281,4 @@ public class Drivetrain extends SubsystemBase {
public void addVisionMeasurement(Pose2d pose, double timestamp){
m_estimator.addVisionMeasurement(pose, timestamp);
}
public double driveDistance(){
return m_frontLeft.getTotalDist();
}
}

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

@ -76,8 +76,6 @@ public class Elevator extends SubsystemBase {
pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
feedForward = new ElevatorFeedforward(
ElevatorConstants.kFeedForwardS,
ElevatorConstants.kFeedForwardG,
@ -143,32 +141,34 @@ public class Elevator extends SubsystemBase {
public Command maintainPosition() {
return startRun(() -> {
/*
pidControllerUp.reset();
pidControllerDown.reset();
*/
},
() -> {
/*
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{
/*
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(
MathUtil.clamp(
downOutput + feedForward.calculate(0), -1, 1)
);
}
*/
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
}*/
elevatorMotor1.setVoltage(
feedForward.calculate(0)
);
});
}
@ -181,62 +181,30 @@ public class Elevator extends SubsystemBase {
* @return Sets motor voltage to achieve the target destination
*/
public Command goToSetpoint(DoubleSupplier setpoint) {
if (setpoint.getAsDouble() == 0) {
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())
.andThen(runManualElevator(() -> -.1)
.until(() -> encoder.getPosition() == 0));
} else {
return startRun(() -> {
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());
}
pidControllerUp.setSetpoint(setpoint.getAsDouble());
pidControllerDown.setSetpoint(setpoint.getAsDouble());
pidControllerUp.reset();
pidControllerDown.reset();
},
() -> {
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)
);
}
}).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
/*
elevatorMotor1.setVoltage(

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

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