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

Swerve calibration

Browse Source
This commit is contained in:
Michael Mikovsky
2025-03-06 19:55:26 -07:00
parent 85965a892a
commit 5ae62c89e5
6 changed files with 112 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+12 -8
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.22705078125+0.5);
private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375);
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.07666015625);
private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375);
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.37646484375);
private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+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;
@@ -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.219970703125+0.5);
private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+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;
@@ -287,8 +287,12 @@ public final class Constants {
}
public static final class LiDARConstants {
public static final int REEF_LIDAR_DIO_CHANNEL = 7;
public static final int REVERSE_LIDAR_DIO_CHANNEL = 4;
public static final int HUMAN_PLAYER_STATION_DISTANCE = 25;
public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole
public static final int LIDAR_DIO_CHANNEL = 7;
public static final int LIDAR_MICROS_TO_CM = 10;
public static final int SECONDS_TO_MICROS = 1000000;
}
@@ -331,7 +335,7 @@ public final class Constants {
// public static final int L3_DRIVE_TIME = 500;
public static final int L2_DRIVE_TIME = 250; //Milliseconds
public static final int ALGAE_DRIVE_TIME = 500;
public static final double STOP_VELOCITY = 0.25;
public static final double STOP_VELOCITY = 0.0;
}
public static final class VisionConstants {
@@ -394,8 +398,8 @@ public final class Constants {
public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);
public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 0;
public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 999;
public static final double SAFETY_ENDEFFECTOR_MAX_TORQUE = 75;
public static final double SAFETY_ENDEFFECTOR_MIN_VELOCITY = 20;
// public static final int BASIN_LIMIT_SWITCH = 8; // TODO: FIND
src/main/java/frc4388/robot/RobotContainer.java
+18 -11
View File
@@ -28,6 +28,7 @@ import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.robot.Constants.FieldConstants;
import frc4388.robot.Constants.LiDARConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.AutoConstants;
@@ -44,6 +45,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.robot.commands.DriveUntilLiDAR;
import frc4388.robot.commands.GotoLastApril;
import frc4388.robot.commands.LidarAlign;
import frc4388.robot.commands.MoveForTimeCommand;
@@ -91,7 +93,8 @@ public class RobotContainer {
/* Subsystems */
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 Lidar m_reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
public final Lidar m_reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
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);
@@ -143,7 +146,7 @@ public class RobotContainer {
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -178,7 +181,7 @@ public class RobotContainer {
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator),
@@ -211,7 +214,7 @@ public class RobotContainer {
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -238,7 +241,7 @@ public class RobotContainer {
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
// new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -255,7 +258,7 @@ public class RobotContainer {
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),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -271,7 +274,7 @@ public class RobotContainer {
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),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
@@ -287,7 +290,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -301,7 +304,7 @@ public class RobotContainer {
new waitEndefectorRefrence(m_robotElevator),
new waitElevatorRefrence(m_robotElevator),
new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT),
new LidarAlign(m_robotSwerveDrive, m_lidar),
new LidarAlign(m_robotSwerveDrive, m_reefLidar),
new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator),
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true),
@@ -339,7 +342,8 @@ public class RobotContainer {
NamedCommands.registerCommand("grab-coral", waitFeedStation.asProxy());
NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive,
new Translation2d(0,1), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, false));
NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.lastOdomSpeed < AutoConstants.STOP_VELOCITY));
NamedCommands.registerCommand("align-feed", new SequentialCommandGroup(
@@ -478,11 +482,14 @@ public class RobotContainer {
), m_robotSwerveDrive))
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new LidarAlign(m_robotSwerveDrive, m_reefLidar));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(thrustIntake.asProxy());
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar));
.onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_reefLidar));
// ? /* Operator Buttons */
src/main/java/frc4388/robot/commands/DriveUntilLiDAR.java
+50
View File
@@ -0,0 +1,50 @@
package frc4388.robot.commands;
import java.time.Instant;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.Lidar;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.TimesNegativeOne;
// Command to repeat a joystick movement for a specific time.
public class DriveUntilLiDAR extends Command {
private final SwerveDrive swerveDrive;
private final Translation2d leftStick;
private final Translation2d rightStick;
private final Lidar m_lidar;
private final double mindistance;
private final boolean robotRelative;
public DriveUntilLiDAR(
SwerveDrive swerveDrive,
Translation2d leftStick,
Translation2d rightStick,
Lidar lidar,
double mindistance,
boolean robotRelative) {
addRequirements(swerveDrive);
this.swerveDrive = swerveDrive;
this.leftStick = leftStick;
this.rightStick = rightStick;
this.m_lidar = lidar;
this.mindistance = mindistance;
this.robotRelative = robotRelative;
}
@Override
public void initialize() {
}
@Override
public void execute() {
swerveDrive.driveFine(leftStick, rightStick, 0.1);
}
@Override
public boolean isFinished() {
return Math.abs(m_lidar.getDistance()) < mindistance;
}
}
src/main/java/frc4388/robot/subsystems/Elevator.java
+1 -1
View File
@@ -302,7 +302,7 @@ public class Elevator extends Subsystem {
if(endeffectorVelocity < ElevatorConstants.SAFETY_ENDEFFECTOR_MIN_VELOCITY && endeffectorTorque > ElevatorConstants.SAFETY_ENDEFFECTOR_MAX_TORQUE ){
PIDPosition(elevatorMotor, endeffectorMotor.getPosition().getValueAsDouble());
PIDPosition(endeffectorMotor, endeffectorMotor.getPosition().getValueAsDouble());
}
// This method will be called once per scheduler run
src/main/java/frc4388/robot/subsystems/Lidar.java
+25 -17
View File
@@ -15,13 +15,31 @@ import frc4388.utility.Status.ReportLevel;
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
public class Lidar extends Subsystem {
private double distance = -1;
Counter LidarPWM = new Counter(LiDARConstants.LIDAR_DIO_CHANNEL);
private Counter LidarPWM;
private String name = "Lidar";
public Lidar() {
private double distance = -1;
public Lidar(int port, String name) {
this.name = name;
LidarPWM = new Counter(port);
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
LidarPWM.reset();
subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
sbDistance = subsystemLayout
.add("Distance", 0)
.withWidget(BuiltInWidgets.kGraph)
.getEntry();
sbWithinDistance = subsystemLayout
. add("Within Distance", 0)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
}
@Override
@@ -41,23 +59,13 @@ public class Lidar extends Subsystem {
return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
}
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
.withSize(2, 2);
GenericEntry sbDistance = subsystemLayout
.add("Distance", 0)
.withWidget(BuiltInWidgets.kGraph)
.getEntry();
GenericEntry sbWithinDistance = subsystemLayout
.add("Within Distance", 0)
.withWidget(BuiltInWidgets.kBooleanBox)
.getEntry();
ShuffleboardLayout subsystemLayout;
GenericEntry sbDistance;
GenericEntry sbWithinDistance;
@Override
public String getSubsystemName() {
return "Lidar";
return "Lidar " + name;
}
@Override
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -6
View File
@@ -237,11 +237,11 @@ public class SwerveDrive extends Subsystem {
.withVelocityX(leftStick.getX() * speedAdjust)
.withVelocityY(leftStick.getY() * speedAdjust)
.withTargetDirection(heading);
ctrl.HeadingController.setPID(
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
);
// ctrl.HeadingController.setPID(
// SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
// SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
// SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
// );
swerveDriveTrain.setControl(ctrl);
}
@@ -349,7 +349,7 @@ public class SwerveDrive extends Subsystem {
rotTarget += deltaAngle;
}
swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, vision.getLastLatency());
swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time);
//back to the ~~future~~ *past*
}