Team4388/2025RidgeScape
1
0
Fork 0
mirror of https://github.com/Team4388/2025RidgeScape.git synced 2026-06-08 16:28:04 -06:00
Code Issues Packages Projects Releases Wiki Activity

Swerve calibration, L2, Autos

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-04 19:46:52 -07:00
parent 6762ed16e6
commit 7dc6105145
7 changed files with 34 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/autos/3 Coral Cage 5.auto
+7 -1
View File
@@ -4,6 +4,12 @@
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "prepare-l4"
}
},
{
"type": "path",
"data": {
@@ -13,7 +19,7 @@
{
"type": "named",
"data": {
"name": "place-coral-right-l4"
"name": "place-coral-left-l4"
}
},
{
src/main/deploy/pathplanner/paths/Cage 5 to Reef.path
+6 -6
View File
@@ -3,13 +3,13 @@
"waypoints": [
{
"anchor": {
"x": 7.585795454545455,
"y": 1.9413510101010092
"x": 7.129687499999999,
"y": 1.9409375
},
"prevControl": null,
"nextControl": {
"x": 7.044375,
"y": 1.8921875
"x": 6.072698863636364,
"y": 1.0434943181818186
},
"isLocked": false,
"linkedName": null
@@ -20,8 +20,8 @@
"y": 2.61125
},
"prevControl": {
"x": 5.3503125,
"y": 2.1603125
"x": 5.504318181818181,
"y": 2.1902272727272725
},
"nextControl": null,
"isLocked": false,
src/main/java/frc4388/robot/Constants.java
+8 -40
View File
@@ -113,7 +113,7 @@ public final class Constants {
private static final class ModuleSpecificConstants { //2025
//Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.4794921875+0.25);
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.22705078125+0.5);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
@@ -121,7 +121,7 @@ public final class Constants {
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.62841796875-0.5);
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.07666015625);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
@@ -129,7 +129,7 @@ public final class Constants {
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
//Back Left
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(-0.867431640625+0.25);
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.37646484375);
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
@@ -137,7 +137,7 @@ public final class Constants {
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Back Right
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.425537109375-0.25+0.25);
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.219970703125+0.5);
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
@@ -145,40 +145,6 @@ public final class Constants {
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
}
/* private static final class ModuleSpecificConstants { // 2024
//Front Left
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.36328125);
private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_LEFT_ENCODER_INVERTED = false;
private static final Distance FRONT_LEFT_XPOS = Inches.of(HALF_WIDTH);
private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Front Right
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.133056640625);
private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false;
private static final Distance FRONT_RIGHT_XPOS = Inches.of(HALF_WIDTH);
private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
//Back Left
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.47705078125 + 0.5);
private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_LEFT_ENCODER_INVERTED = false;
private static final Distance BACK_LEFT_XPOS = Inches.of(-HALF_WIDTH);
private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT);
//Back Right
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(-0.355224609375 + 0.5);
private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false;
private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true;
private static final boolean BACK_RIGHT_ENCODER_INVERTED = false;
private static final Distance BACK_RIGHT_XPOS = Inches.of(-HALF_WIDTH);
private static final Distance BACK_RIGHT_YPOS = Inches.of(-HALF_HEIGHT);
} */
public static final class IDs {
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 4);
public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 5);
@@ -328,10 +294,11 @@ public final class Constants {
}
public static final class AutoConstants {
public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
public static final Gains XY_GAINS = new Gains(5,0.4,0.0);
// public static final Gains XY_GAINS = new Gains(3,0.3,0.0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Gains ROT_GAINS = new Gains(0.05,0,0.007);
// public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE,0.5, 0);
public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
@@ -355,6 +322,7 @@ public final class Constants {
public static final double L3_DISTANCE_PREP = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(12);
public static final double L3_DISTANCE_SCORE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_PREP_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(6);
public static final double L2_SCORE_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
public static final double ALGAE_REMOVAL_DISTANCE = Y_SCORING_POSITION_OFFSET + Units.inchesToMeters(2);
src/main/java/frc4388/robot/RobotContainer.java
+3 -1
View File
@@ -90,7 +90,7 @@ public class RobotContainer {
public final LED m_robotLED = new LED();
public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
public final Lidar m_lidar = new Lidar();
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.basinLimitSwitch, m_robotLED);
public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotMap.IRIntakeBeam, m_robotLED);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
@@ -250,6 +250,7 @@ public class RobotContainer {
private Command AprilLidarAlignL2Left = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.LEFT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
@@ -265,6 +266,7 @@ public class RobotContainer {
private Command AprilLidarAlignL2Right = new SequentialCommandGroup(
new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_PREP_DISTANCE, Side.RIGHT),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
src/main/java/frc4388/robot/subsystems/Elevator.java
+6 -2
View File
@@ -306,6 +306,10 @@ public class Elevator extends Subsystem {
} else if (currentState == CoordinationState.Ready) {
periodicReady();
}
if(!intakeIR.get()){
led.setMode(LEDConstants.DOWN_PATTERN);
}
// } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) {
// periodicScoring();
// }
@@ -324,8 +328,8 @@ public class Elevator extends Subsystem {
}
public boolean readyToMove() {
// return !intakeIR.get() || hasCoral();
return hasCoral();
return !intakeIR.get() || hasCoral() || !endeffectorLimitSwitch.get();
// return hasCoral();
}
public void armShuffle(){
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+2
View File
@@ -184,10 +184,12 @@ public class SwerveDrive extends Subsystem {
public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
stopped = false;
// Create robot-relative speeds.
if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
.withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
.withRotationalRate(rightStick.getX() * rotSpeedAdjust));
}
src/main/java/frc4388/utility/ReefPositionHelper.java
+2
View File
@@ -56,6 +56,8 @@ public class ReefPositionHelper {
minDistance = dist;
}
}
System.out.println(minPos.getRotation().getDegrees());
return minPos;
}