Skip to content

Commit

Permalink
used maple config file for chassis configuration storage
Browse files Browse the repository at this point in the history
  • Loading branch information
@catr1xLiu
catr1xLiu authored Jul 6, 2024
2 parents 4b62def + e19d1dd commit f6df696
Show file tree
Hide file tree
Showing 23 changed files with 375 additions and 257 deletions.
51 changes: 51 additions & 0 deletions src/main/deploy/configs/ChassisWheelsCalibration/5516-2024-OnSeason.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
<ChassisWheelsCalibration>
<GeneralInformation>
<gyroPort type="int">15</gyroPort>
<maxAngularVelocityRadiansPerSecond type="double">9.42477796076938</maxAngularVelocityRadiansPerSecond>
<maxAccelerationMetersPerSecondSquared type="double">10.184</maxAccelerationMetersPerSecondSquared>
<overallGearRatio type="double">6.12</overallGearRatio>
<wheelRadiusMeters type="double">0.051</wheelRadiusMeters>
<bumperWidthMeters type="double">0.876</bumperWidthMeters>
<maxVelocityMetersPerSecond type="double">4.172</maxVelocityMetersPerSecond>
<bumperLengthMeters type="double">0.876</bumperLengthMeters>

<horizontalWheelsMarginMeters type="double">0.53</horizontalWheelsMarginMeters>
<verticalWheelsMarginMeters type="double">0.53</verticalWheelsMarginMeters>
</GeneralInformation>
<FrontLeft>
<steeringEncoderReadingAtOrigin type="double">3.3195344249845276</steeringEncoderReadingAtOrigin>
<steeringMotorPortOnPDP type="int">-1</steeringMotorPortOnPDP>
<steeringEncoderID type="int">10</steeringEncoderID>
<drivingMotorPortOnPDP type="int">-1</drivingMotorPortOnPDP>
<drivingMotorID type="int">3</drivingMotorID>
<steeringMotorInverted type="int">1</steeringMotorInverted>
<steeringMotorID type="int">4</steeringMotorID>
</FrontLeft>
<FrontRight>
<steeringEncoderReadingAtOrigin type="double">1.7564080021290591</steeringEncoderReadingAtOrigin>
<steeringMotorPortOnPDP type="int">-1</steeringMotorPortOnPDP>
<steeringEncoderID type="int">11</steeringEncoderID>
<drivingMotorPortOnPDP type="int">-1</drivingMotorPortOnPDP>
<drivingMotorID type="int">6</drivingMotorID>
<steeringMotorInverted type="int">1</steeringMotorInverted>
<steeringMotorID type="int">5</steeringMotorID>
</FrontRight>
<BackLeft>
<steeringEncoderReadingAtOrigin type="double">0.34974761963792617</steeringEncoderReadingAtOrigin>
<steeringMotorPortOnPDP type="int">-1</steeringMotorPortOnPDP>
<steeringEncoderID type="int">9</steeringEncoderID>
<drivingMotorPortOnPDP type="int">-1</drivingMotorPortOnPDP>
<drivingMotorID type="int">1</drivingMotorID>
<steeringMotorInverted type="int">1</steeringMotorInverted>
<steeringMotorID type="int">2</steeringMotorID>
</BackLeft>
<BackRight>
<steeringEncoderReadingAtOrigin type="double">0.10737865515199488</steeringEncoderReadingAtOrigin>
<steeringMotorPortOnPDP type="int">-1</steeringMotorPortOnPDP>
<steeringEncoderID type="int">12</steeringEncoderID>
<drivingMotorPortOnPDP type="int">-1</drivingMotorPortOnPDP>
<drivingMotorID type="int">8</drivingMotorID>
<steeringMotorInverted type="int">1</steeringMotorInverted>
<steeringMotorID type="int">7</steeringMotorID>
</BackRight>
</ChassisWheelsCalibration>
44 changes: 37 additions & 7 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,8 @@

package frc.robot;

import frc.robot.utils.MechanismControlHelpers.MapleSimplePIDController;
import edu.wpi.first.math.util.Units;
import frc.robot.utils.MechanismControl.MapleSimplePIDController;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
Expand Down Expand Up @@ -48,39 +49,68 @@ public enum ChassisType {
}
public static final ChassisType chassisType = ChassisType.CTRE_ON_CANIVORE;

public static final String DEFAULT_CHASSIS_CANIVORE = "ChassisCanivore";
public static final String CHASSIS_CANBUS = "ChassisCanivore";

public static final int ODOMETRY_CACHE_CAPACITY = 10;
public static final double ODOMETRY_FREQUENCY = 250;
public static final double ODOMETRY_WAIT_TIMEOUT_SECONDS = 0.02;
}

public static final class CalibrationConfigs {
public static final class ChassisDefaultConfigs {
public static final int DEFAULT_GYRO_PORT = 0;
public static final double DEFAULT_GEAR_RATIO = 6.12;
public static final double DEFAULT_WHEEL_RADIUS_METERS = 0.051; // 2 inch
public static final double DEFAULT_BUMPER_WIDTH_METERS = 0.876; // 34.5 inch
public static final double DEFAULT_LEFT_RIGHT_WHEELS_DISTANCE_METERS = 0.53;
public static final double DEFAULT_FRONT_BACK_WHEELS_DISTANCE_METERS = 0.53;
public static final double DEFAULT_HORIZONTAL_WHEELS_MARGIN_METERS = 0.53;
public static final double DEFAULT_VERTICAL_WHEELS_MARGIN_METERS = 0.53;
public static final double DEFAULT_BUMPER_LENGTH_METERS = 0.876; // 34.5 inch
public static final double DEFAULT_MAX_VELOCITY_METERS_PER_SECOND = 4.172; // calculated from Choreo (Kraken x60 motor, 6.12 gear ratio, 55kg robot mass)
public static final double DEFAULT_MAX_ACCELERATION_METERS_PER_SQUARED_SECOND = 10.184; // calculated from Choreo (Kraken x60 motor, 6.12 gear ratio, 55kg robot mass)
public static final double DEFAULT_MAX_ANGULAR_VELOCITY_DEGREES_PER_SECOND = 540;
}

public static final class WheelCalibrationConfigs {
public static final class WheelToBeCalibrated {
public final String name;
public final int drivingMotorID, steeringMotorID, encoderID, drivingMotorPortOnPDP, steeringMotorPortOnPDP;
public boolean steeringMotorInverted;

public WheelToBeCalibrated(String name, int drivingMotorID, int steeringMotorID, int encoderID) {
this(name, drivingMotorID, steeringMotorID, encoderID, -1, -1);
}

private WheelToBeCalibrated(String name, int drivingMotorID, int steeringMotorID, int encoderID, int drivingMotorPortOnPDP, int steeringMotorPortOnPDP) {
this.name = name;
this.drivingMotorID = drivingMotorID;
this.steeringMotorID = steeringMotorID;
this.encoderID = encoderID;
this.drivingMotorPortOnPDP = drivingMotorPortOnPDP;
this.steeringMotorPortOnPDP = steeringMotorPortOnPDP;
this.steeringMotorInverted = false;
}
}
public static final WheelToBeCalibrated[] wheelsToBeCalibrated = new WheelToBeCalibrated[] {
new WheelToBeCalibrated("FrontLeft", 3, 4, 10),
new WheelToBeCalibrated("FrontRight", 6, 5, 11),
new WheelToBeCalibrated("BackLeft", 1, 2, 9),
new WheelToBeCalibrated("BackRight", 8, 7, 12)
};
}

public static final class SwerveModuleConfigs {
public static final double MINIMUM_USAGE_SPEED_METERS_PER_SECOND = CalibrationConfigs.DEFAULT_MAX_VELOCITY_METERS_PER_SECOND * 0.03;
public static final double MINIMUM_USAGE_SPEED_METERS_PER_SECOND = ChassisDefaultConfigs.DEFAULT_MAX_VELOCITY_METERS_PER_SECOND * 0.03;
public static final double NON_USAGE_TIME_RESET_SWERVE = 0.5;

public static final MapleSimplePIDController.SimplePIDProfile steerHeadingCloseLoopConfig = new MapleSimplePIDController.SimplePIDProfile(
0.9,
Math.toRadians(65),
0.01,
0.02,
Math.toRadians(1.5),
0.05,
true
);
public static final double STEERING_CURRENT_LIMIT = 20;
public static final double DRIVING_CURRENT_LIMIT = 60;
public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0);
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ public void robotInit() {

// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our autonomous chooser on the dashboard.
robotContainer = new RobotContainer();
robotContainer = new RobotContainer("5516-2024-OnSeason");

MapleSubsystem.subsystemsInit();
}
Expand Down
33 changes: 23 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,8 +14,11 @@
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.DriveCommands;
import frc.robot.subsystems.drive.*;
import frc.robot.utils.Config.MapleConfigFile;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

import java.io.IOException;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
Expand All @@ -35,23 +38,31 @@ public class RobotContainer {
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
public RobotContainer(String chassisName) {
final MapleConfigFile chassisCalibrationFile;
try {
chassisCalibrationFile = MapleConfigFile.fromDeployedConfig("ChassisWheelsCalibration", chassisName);
} catch (IOException e) {
throw new RuntimeException(e);
}
final MapleConfigFile.ConfigBlock generalConfigBlock = chassisCalibrationFile.getBlock("GeneralInformation");
switch (Robot.CURRENT_ROBOT_MODE) {
case REAL -> {
// Real robot, instantiate hardware IO implementations
// drive = new Drive(
// new GyroIOPigeon2(false),
// new GyroIOPigeon2(),
// new ModuleIOSparkMax(0),
// new ModuleIOSparkMax(1),
// new ModuleIOSparkMax(2),
// new ModuleIOSparkMax(3)
// );
drive = new Drive(
new GyroIOPigeon2(),
new ModuleIOTalonFX(0),
new ModuleIOTalonFX(1),
new ModuleIOTalonFX(2),
new ModuleIOTalonFX(3)
new GyroIOPigeon2(),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("FrontLeft"), generalConfigBlock),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("FrontRight"), generalConfigBlock),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("BackLeft"), generalConfigBlock),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("BackRight"), generalConfigBlock),
generalConfigBlock
);
}

Expand All @@ -62,7 +73,8 @@ public RobotContainer() {
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim(),
new ModuleIOSim()
new ModuleIOSim(),
generalConfigBlock
);
}

Expand All @@ -73,7 +85,8 @@ public RobotContainer() {
(inputs) -> {},
(inputs) -> {},
(inputs) -> {},
(inputs) -> {}
(inputs) -> {},
generalConfigBlock
);
}
}
Expand All @@ -97,7 +110,7 @@ private void configureButtonBindings() {
() -> -controller.getLeftY(),
() -> -controller.getLeftX(),
() -> -controller.getRightX()));
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
controller.x().onTrue(Commands.runOnce(drive::requestStopWithX, drive));
controller.b().onTrue(Commands.runOnce(() -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive).ignoringDisable(true));
}

Expand Down
22 changes: 11 additions & 11 deletions src/main/java/frc/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

package frc.robot.commands;

import edu.wpi.first.hal.simulation.REVPHDataJNI;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand Down Expand Up @@ -62,17 +63,16 @@ public static Command joystickDrive(
.getTranslation();

// Convert to field relative speeds & send command
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
isFlipped
? drive.getRotation().plus(new Rotation2d(Math.PI))
: drive.getRotation()));
final Rotation2d driverStationFacing = switch (DriverStation.getAlliance().orElse(Alliance.Red)) {
case Red -> new Rotation2d(Math.PI);
case Blue -> new Rotation2d(0);
};
drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.maxModuleVelocityMetersPerSec,
linearVelocity.getY() * drive.maxModuleVelocityMetersPerSec,
omega * drive.maxAngularVelocityRadPerSec,
drive.getPose().getRotation().minus(driverStationFacing)
));
},
drive);
}
Expand Down
Loading

0 comments on commit f6df696

Please sign in to comment.