Skip to content

Commit 5b4925f

Browse files
PhilBurt627PhilBurt627
committed
Drive fixed distance is relatively accurate
1 parent 0d9c09c commit 5b4925f

6 files changed

Lines changed: 104 additions & 17 deletions

File tree

‎src/main/deploy/pathplanner/autos/DriveFixedDistance.auto‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
{
2+
"version": "2025.0",
3+
"command": {
4+
"type": "sequential",
5+
"data": {
6+
"commands": [
7+
{
8+
"type": "path",
9+
"data": {
10+
"pathName": "DriveFixedDistancePath"
11+
}
12+
}
13+
]
14+
}
15+
},
16+
"resetOdom": true,
17+
"folder": null,
18+
"choreoAuto": false
19+
}

‎src/main/deploy/pathplanner/paths/DriveFixedDistancePath.path‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
{
2+
"version": "2025.0",
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 2.0,
7+
"y": 7.0
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 3.0,
12+
"y": 7.0
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 7.0,
20+
"y": 7.0
21+
},
22+
"prevControl": {
23+
"x": 6.0,
24+
"y": 7.0
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"pointTowardsZones": [],
34+
"eventMarkers": [],
35+
"globalConstraints": {
36+
"maxVelocity": 1.5,
37+
"maxAcceleration": 2.0,
38+
"maxAngularVelocity": 540.0,
39+
"maxAngularAcceleration": 720.0,
40+
"nominalVoltage": 12.0,
41+
"unlimited": false
42+
},
43+
"goalEndState": {
44+
"velocity": 0,
45+
"rotation": 0.0
46+
},
47+
"reversed": false,
48+
"folder": null,
49+
"idealStartingState": {
50+
"velocity": 0,
51+
"rotation": 0.0
52+
},
53+
"useDefaultConstraints": false
54+
}

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 9 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ public static final class DriveConstants {
5151

5252
// Should get overwritten in robotcontainer
5353
public static double maxSpeedMetersPerSec = 5.450;//5.450; //4.0; // was default
54-
public static final double angularVelocityMultiplier = 0.5;
54+
public static final double angularVelocityMultiplier = 0.3;
5555
public static final double trackWidth = Units.inchesToMeters(22.8);
5656
public static final double wheelBase = Units.inchesToMeters(25.6);
5757
public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0);
@@ -90,9 +90,9 @@ public static final class DriveConstants {
9090
public static final int backRightTurnCanId = 2;
9191

9292
// Drive motor configuration
93-
public static final int driveMotorCurrentLimit = 40;
94-
public static final double wheelRadiusMeters = Units.inchesToMeters(2);
95-
public static final double driveMotorReduction = 7.3; //(45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth
93+
public static final int driveMotorCurrentLimit = 60;
94+
public static final double wheelRadiusMeters = Units.inchesToMeters(1.925);
95+
public static final double driveMotorReduction = 5.9; //(45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth
9696
public static final DCMotor driveGearbox = DCMotor.getNeoVortex(1); // DCMotor.getNeoVortex(1);
9797

9898
// Individual drive motor inversions
@@ -130,18 +130,18 @@ public static final class DriveConstants {
130130
(2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec
131131

132132
// Drive PID configuration
133-
public static final double driveKp = 0.01;
133+
public static final double driveKp = 0.02;
134134
public static final double driveKd = 0.0;
135-
public static final double driveKs = 0.184445; // Got this value from characterization auto routine
136-
public static final double driveKv = 0.093025; // Got this value from characterization auto routine
135+
public static final double driveKs = 0.14243; //0.184445; // Got this value from characterization auto routine
136+
public static final double driveKv = 0.12603; //0.093025; // Got this value from characterization auto routine
137137
public static final double driveSimP = 0.01; //8.0;
138138
public static final double driveSimD = 0.0;
139139
public static final double driveSimKs = 0.184445; //0.0;
140140
public static final double driveSimKv = 0.093025; //0.0789;
141141

142142
// Turn motor configuration
143143
// public static final boolean turnInverted = false;
144-
public static final int turnMotorCurrentLimit = 20;
144+
public static final int turnMotorCurrentLimit = 40;
145145
public static final double turnMotorReduction = 21.0; //9424.0 / 203.0;
146146
public static final DCMotor turnGearbox = DCMotor.getNEO(1); //DCMotor.getNeo550(1);
147147

@@ -194,7 +194,6 @@ public static final class DriveConstants {
194194
} // End drive
195195

196196

197-
// NOTICE: None of these values have been tested. They are all arbitrary
198197
public static final class ElevatorConstants {
199198

200199
public static final int leftMotorID = 54;
@@ -206,7 +205,7 @@ public static final class ElevatorConstants {
206205
public static final double wristMotorReduction = 1.0;
207206

208207

209-
public static final double maxVelocityMetersPerSec = 2.0; // Arbitrary
208+
public static final double maxVelocityMetersPerSec = 2.0;
210209
public static final double maxAccelerationMetersPerSecSqrd = 4.0;
211210

212211

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
2727
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
2828
import edu.wpi.first.wpilibj2.command.button.Trigger;
29+
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
2930
import frc.robot.Constants.ElevatorConstants.ElevatorHeight;
3031
import frc.robot.Constants.WristConstants.WristAngleRad;
3132
import frc.robot.commands.DriveCommands;
@@ -97,8 +98,8 @@ public RobotContainer() {
9798

9899
// Should the constructor and auto chooser/builder stuff be called in this order?
99100

100-
driveConstructorStuff();
101101
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
102+
driveConstructorStuff();
102103

103104

104105

@@ -297,7 +298,21 @@ public void ResetProfiledSubsystemsOnEnable(){
297298
// ============= Constructor Stuff =============
298299

299300
private void driveConstructorStuff() {
300-
301+
//Set up SysId routines
302+
autoChooser.addOption(
303+
"Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive));
304+
autoChooser.addOption(
305+
"Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive));
306+
autoChooser.addOption(
307+
"Drive SysId (Quasistatic Forward)",
308+
drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
309+
autoChooser.addOption(
310+
"Drive SysId (Quasistatic Reverse)",
311+
drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
312+
autoChooser.addOption(
313+
"Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
314+
autoChooser.addOption(
315+
"Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
301316
}
302317

303318

‎src/main/java/frc/robot/subsystems/drive/Drive.java‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -272,9 +272,9 @@ public void periodic() {
272272
poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
273273

274274
Pose2d visionPose = getRobotVisionPose();
275-
// if(visionPose != null){
276-
// // poseEstimator.addVisionMeasurement(visionPose, sampleTimestamps[i]);
277-
// }
275+
// if(visionPose != null){
276+
// poseEstimator.addVisionMeasurement(visionPose, sampleTimestamps[i]);
277+
// }
278278
}
279279

280280
// Update gyro alert

‎src/main/java/frc/robot/subsystems/drive/Module/ModuleIOSpark.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -181,7 +181,7 @@ public ModuleIOSpark(
181181
.idleMode(IdleMode.kBrake)
182182
.smartCurrentLimit(Constants.DriveConstants.driveMotorCurrentLimit)
183183
.voltageCompensation(12.0)
184-
.closedLoopRampRate(0.1);
184+
.closedLoopRampRate(0.01);
185185
driveConfig
186186
.encoder
187187
.positionConversionFactor(Constants.DriveConstants.driveEncoderPositionFactor)
@@ -216,7 +216,7 @@ public ModuleIOSpark(
216216
.idleMode(IdleMode.kBrake)
217217
.smartCurrentLimit(Constants.DriveConstants.turnMotorCurrentLimit)
218218
.voltageCompensation(12.0)
219-
.closedLoopRampRate(0.1);
219+
.closedLoopRampRate(0.01);
220220
turnConfig
221221
.absoluteEncoder
222222
.inverted(turnEncoderInverted)

0 commit comments

Comments
 (0)