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

Compare commits

base: Team_2648:a19285cb0bd9980d24054177558e8ae37611589f
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:3dcbac25cca9cdbfa8175f02237d744ddece052f
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

2 Commits
a19285cb0b ... 3dcbac25cc

Author SHA1 Message Date
Team 2648
3dcbac25cc auto sucky 2025-04-03 19:59:58 -04:00
Team 2648
a391cc7910 algae setpoints 2025-04-03 08:20:37 -04:00
11 changed files with 133 additions and 35 deletions
Show Stats Expand all files Collapse all files

8
src/main/deploy/pathplanner/paths/30 Right to HP.path
View File

@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
"x": 1.0549180327868852,
"y": 7.285655737704918
},
"prevControl": {
"x": 2.055234532899169,
"y": 6.243135947675724
"x": 1.9113820738827756,
"y": 6.339037587019986
},
"nextControl": null,
"isLocked": false,

20
src/main/deploy/pathplanner/paths/HP to 330 Right.path
View File

@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
"x": 1.0549180327868852,
"y": 7.285655737704918
},
"prevControl": null,
"nextControl": {
"x": 2.490979395912868,
"y": 6.964368824388053
"x": 2.3471269368964744,
"y": 7.060270463732315
},
"isLocked": false,
"linkedName": "HP Left Position"
},
{
"anchor": {
"x": 3.6202868852459016,
"y": 5.031967213114754
"x": 3.65625,
"y": 5.05594262295082
},
"prevControl": {
"x": 3.304747501684258,
"y": 5.557866185717494
"x": 3.3407106164383564,
"y": 5.581841595553559
},
"nextControl": null,
"isLocked": false,
@ -46,7 +46,7 @@
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAcceleration": 1.25,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -62,5 +62,5 @@
"velocity": 0,
"rotation": -53.97262661489646
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

8
src/main/deploy/pathplanner/paths/HP to K.path
View File

@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
"x": 1.0549180327868852,
"y": 7.285655737704918
},
"prevControl": null,
"nextControl": {
"x": 1.7142418032786886,
"y": 6.638319672131147
"x": 1.5703893442622952,
"y": 6.734221311475409
},
"isLocked": false,
"linkedName": "HP Left Position"

16
src/main/deploy/pathplanner/paths/L Backup.path
View File

@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 3.6202868852459016,
"y": 5.031967213114754
"x": 3.65625,
"y": 5.05594262295082
},
"prevControl": null,
"nextControl": {
"x": 3.4877871502451065,
"y": 5.243966789116026
"x": 3.523750264999205,
"y": 5.267942198952092
},
"isLocked": false,
"linkedName": "L"
},
{
"anchor": {
"x": 1.1987704918032787,
"y": 7.189754098360656
"x": 1.0549180327868852,
"y": 7.285655737704918
},
"prevControl": {
"x": 1.4025614754098363,
"y": 6.914036885245903
"x": 1.258709016393443,
"y": 7.009938524590164
},
"nextControl": null,
"isLocked": false,

54
src/main/deploy/pathplanner/paths/New Path.path Normal file
View File

@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 8.786987704919465,
"y": 4.348668032784689
},
"prevControl": null,
"nextControl": {
"x": 9.786987704919456,
"y": 4.348668032784689
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 11.256454918034219,
"y": 4.348668032784689
},
"prevControl": {
"x": 7.543546317006037,
"y": 6.492316813064786
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 124.695153531234
},
"useDefaultConstraints": true
}

4
src/main/deploy/pathplanner/paths/Start to 30 Right.path
View File

@ -46,7 +46,7 @@
],
"globalConstraints": {
"maxVelocity": 3.5,
"maxAcceleration": 1.75,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 400.0,
"nominalVoltage": 12.0,
@ -62,5 +62,5 @@
"velocity": 0,
"rotation": -90.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

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

@ -167,6 +167,15 @@ public class RobotContainer {
driver.start().whileTrue(drivetrain.resetToVision());
driver.povUp().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][0],
() -> VisionConstants.algaeSetpointsMap[closestTag.getAsInt()][1],
() -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180))
))
);
driver.rightBumper().whileTrue(
drivetrain.resetToVision().andThen(
drivetrain.goToPose(
@ -600,7 +609,7 @@ public class RobotContainer {
private Command shootAlgae(){
return manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>44).andThen(manipulator.runManipulator(() -> -1, false),
.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpoint(() -> ManipulatorPivotConstants.kBargeShotPosition)
.raceWith(elevator.maintainPosition()));
}

9
src/main/java/frc/robot/constants/ClimberRollersConstants.java
View File

@ -1,9 +1,18 @@
package frc.robot.constants;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
public class ClimberRollersConstants {
public static final int kRollerMotorID = 9;
public static final SparkMaxConfig motorConfig = new SparkMaxConfig();
static {
motorConfig
.smartCurrentLimit(40)
.idleMode(IdleMode.kBrake)
.inverted(true);
}
}

26
src/main/java/frc/robot/constants/VisionConstants.java
View File

@ -59,6 +59,32 @@ public class VisionConstants {
{4.993, 2.816, 5.272, 2.996}
};
public static final double[][] algaeSetpointsMap = {
{},
{},
{},
{},
{},
{},
{13.906, 2.658},//6
{14.661, 4.013},
{13.834, 5.428},
{12.263, 5.452},
{11.412, 4.025},
{12.191, 2.574},//11
{},
{},
{},
{},
{},
{3.649, 2.558},//17
{2.776, 4.005},
{3.644, 5.514},
{5.296, 5.522},//4.991, 5.246},
{6.225, 4.008},
{5.322, 2.511}//22
};
public static final double latencyFudge = 0.0;
}

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

@ -112,7 +112,7 @@ public class Drivetrain extends SubsystemBase {
},
new Pose2d(),
VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
VecBuilder.fill(1, 1, Units.degreesToRadians(360))
VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))
);
pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
@ -177,11 +177,11 @@ public class Drivetrain extends SubsystemBase {
gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.10, 0.10, Units.degreesToRadians(360)));
if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
if(vision.getOrangeDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
}
// if the detected tags match your alliances reef tags use their pose estimates
@ -199,7 +199,7 @@ public class Drivetrain extends SubsystemBase {
if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
if(vision.getBlackDist() < 60){
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360)));
m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.025, 0.025, Units.degreesToRadians(360)));
}
if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){

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

@ -84,8 +84,8 @@ public class Manipulator extends SubsystemBase {
*/
public Command runUntilCollected(DoubleSupplier speed) {
return run(() -> {
manipulatorMotor.set(
speed.getAsDouble()
manipulatorMotor.setVoltage(
speed.getAsDouble()*12
);
indexerMotor.set(1);