Skip to content

Commit

Permalink
fix SwerveModule code and driving constants
Browse files Browse the repository at this point in the history
  • Loading branch information
@MichaelLesirge
MichaelLesirge committed Jan 19, 2024
1 parent 1e19965 commit e15918a
Show file tree
Hide file tree
Showing 3 changed files with 268 additions and 7 deletions.
244 changes: 244 additions & 0 deletions shuffleboard.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,244 @@
{
"tabPane": [
{
"title": "SmartDashboard",
"autoPopulate": true,
"autoPopulatePrefix": "SmartDashboard/",
"widgetPane": {
"gridSize": 128.0,
"showGrid": true,
"hgap": 16.0,
"vgap": 16.0,
"titleType": 0,
"tiles": {
"0,0": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Speed Mode",
"_title": "Speed Mode",
"_glyph": 148,
"_showGlyph": false
}
},
"0,1": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///SmartDashboard/Field Relieve",
"_title": "Field Relieve",
"_glyph": 148,
"_showGlyph": false,
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"1,0": {
"size": [
2,
2
],
"content": {
"_type": "Gyro",
"_source0": "network_table:///SmartDashboard/Heading Degrees",
"_title": "Heading Degrees",
"_glyph": 148,
"_showGlyph": false,
"Visuals/Major tick spacing": 45.0,
"Visuals/Starting angle": 180.0,
"Visuals/Show tick mark ring": true,
"Visuals/Counter clockwise": false
}
},
"4,0": {
"size": [
2,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Target Speed X MPH",
"_title": "Target Speed X MPH",
"_glyph": 148,
"_showGlyph": false
}
},
"6,0": {
"size": [
2,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Target Speed Y MPH",
"_title": "Target Speed Y MPH",
"_glyph": 148,
"_showGlyph": false
}
},
"8,0": {
"size": [
2,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Target RPM",
"_title": "Target RPM",
"_glyph": 148,
"_showGlyph": false
}
},
"4,1": {
"size": [
2,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Real Speed X MPH",
"_title": "Real Speed X MPH",
"_glyph": 148,
"_showGlyph": false
}
},
"6,1": {
"size": [
2,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Real Speed Y MPH",
"_title": "Real Speed Y MPH",
"_glyph": 148,
"_showGlyph": false
}
},
"8,1": {
"size": [
2,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Real RPM",
"_title": "Real RPM",
"_glyph": 148,
"_showGlyph": false
}
},
"5,2": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Joystick X",
"_title": "Joystick X",
"_glyph": 148,
"_showGlyph": false
}
},
"7,2": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Joystick Y",
"_title": "Joystick Y",
"_glyph": 148,
"_showGlyph": false
}
},
"8,2": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/Joystick R",
"_title": "Joystick R",
"_glyph": 148,
"_showGlyph": false
}
},
"4,2": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///SmartDashboard/X Active",
"_title": "X Active",
"_glyph": 148,
"_showGlyph": false,
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"6,2": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///SmartDashboard/Y Active",
"_title": "Y Active",
"_glyph": 148,
"_showGlyph": false,
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"9,2": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///SmartDashboard/R Active",
"_title": "R Active",
"_glyph": 148,
"_showGlyph": false,
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
}
}
}
},
{
"title": "LiveWindow",
"autoPopulate": true,
"autoPopulatePrefix": "LiveWindow/",
"widgetPane": {
"gridSize": 128.0,
"showGrid": true,
"hgap": 16.0,
"vgap": 16.0,
"titleType": 0,
"tiles": {}
}
}
],
"windowGeometry": {
"x": -7.199999809265137,
"y": -7.199999809265137,
"width": 1550.4000244140625,
"height": 830.4000244140625
}
}
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@ public static class DriverConstants {
public static final String[] maxSpeedOptionsNames = {"Precise", "Normal", "Boost"};

// max forward/sideways velocities for drivetrain, in meters per second
public static final double[] maxSpeedOptionsTranslation = {0.44, 4.4, 11.6};
public static final double[] maxSpeedOptionsTranslation = {0.4, 4, 8};

// max angular velocity for drivetrain, in radians per second
public static final double[] maxSpeedOptionsRotation = {1, 2, 4};
public static final double[] maxSpeedOptionsRotation = {0.1, 0.2, 0.4};
}

public static class ControllerConstants {
Expand All @@ -49,7 +49,7 @@ public static class SwerveModuleConstants {

// Other settings
public static final double MAX_SPEED_LIMIT = 0.2;
public static final double SWERVE_MODULE_DRIVE_COSIGN_COEFFICIENT = 0;
public static final double SWERVE_MODULE_DRIVE_COSIGN_SCALE = 0.1;


// Steering PID values
Expand Down
25 changes: 21 additions & 4 deletions src/main/java/frc/robot/subsystems/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,6 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
steeringEncoder = new CANcoder(steeringAbsoluteEncoderId);

CANcoderConfiguration configuration = new CANcoderConfiguration();
configuration.MagnetSensor.MagnetOffset = steeringEncoderZero;
configuration.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1;
steeringEncoder.getConfigurator().apply(configuration);

Expand Down Expand Up @@ -182,17 +181,35 @@ public void setDesiredState(SwerveModuleState state, boolean shouldOptimize) {
this.desiredState = state;
}

/**
* Set the state of the swerve module. The state is the speed and angle of the swerve module.
* You can use {@code Rotation2d.fromDegrees()} to create angle.
* This version is meant for driving the robot, where a new state is set every 20ms.
* It tries to make driving smoother by
*
* @param state New state of swerve module, contains speed in meters per second and angle as {@link Rotation2d}
* @param shouldOptimize Whether to optimize the way the swerve module gets to the desired state
*/
public void setDesiredStateDrive(SwerveModuleState state) {
if (state != null) {
Rotation2d encoderRotation = getState().angle;

// get current angle of robot
Rotation2d encoderAngle = getState().angle;

// Optimize the reference state to avoid spinning further than 90 degrees
state = SwerveModuleState.optimize(state, encoderRotation);

state = SwerveModuleState.optimize(state, encoderAngle);

// Scale speed by cosine of angle error.
// This scales down movement perpendicular to the desired direction of travel that can occur when modules change directions.
// This results in smoother driving.
state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos() * SwerveModuleConstants.SWERVE_MODULE_DRIVE_COSIGN_COEFFICIENT;

final double speed = state.speedMetersPerSecond;
final double speedCosScaled = speed * state.angle.minus(encoderAngle).getCos();

final double scaleSplit = SwerveModuleConstants.SWERVE_MODULE_DRIVE_COSIGN_SCALE;

state.speedMetersPerSecond = (speed * scaleSplit) + (speedCosScaled * (1 - scaleSplit));
}

this.desiredState = state;
Expand Down

0 comments on commit e15918a

Please sign in to comment.