Working on KitBot!

FRC2713 · Jan 9, 2025 · 73672db · 73672db
1 parent 193a848
commit 73672db
Show file tree

Hide file tree

Showing 6 changed files with 190 additions and 101 deletions.
diff --git a/src/main/deploy/choreo/Example Auto.traj b/src/main/deploy/choreo/Example Auto.traj
@@ -3,7 +3,7 @@
  "version":1,
  "snapshot":{
   "waypoints":[
-    {"x":1.117739200592041, "y":6.639083385467529, "heading":0.0, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":1.117739200592041, "y":6.639083385467529, "heading":0.0, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":3.105804204940796, "y":6.619592666625977, "heading":0.0, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -13,7 +13,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"1.117739200592041 m", "val":1.117739200592041}, "y":{"exp":"6.639083385467529 m", "val":6.639083385467529}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":19, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"1.117739200592041 m", "val":1.117739200592041}, "y":{"exp":"6.639083385467529 m", "val":6.639083385467529}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"3.105804204940796 m", "val":3.105804204940796}, "y":{"exp":"6.619592666625977 m", "val":6.619592666625977}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
@@ -26,28 +26,21 @@
  },
  "trajectory":{
   "sampleType":"Swerve",
-  "waypoints":[0.0,0.94064],
+  "waypoints":[0.0,0.59841],
   "samples":[
-    {"t":0.0, "x":1.11774, "y":6.63908, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":9.01866, "ay":-0.09282, "alpha":0.0, "fx":[153.40487,153.40487,153.40487,153.40487], "fy":[-1.57882,-1.57877,-1.57877,-1.57882]},
-    {"t":0.04951, "x":1.12879, "y":6.63897, "heading":0.0, "vx":0.44649, "vy":-0.0046, "omega":0.0, "ax":9.01771, "ay":-0.09231, "alpha":0.0, "fx":[153.38865,153.38865,153.38865,153.38865], "fy":[-1.57019,-1.57014,-1.57014,-1.57019]},
-    {"t":0.09901, "x":1.16195, "y":6.63863, "heading":0.0, "vx":0.89293, "vy":-0.00917, "omega":0.0, "ax":9.01648, "ay":-0.0918, "alpha":0.0, "fx":[153.36777,153.36777,153.36777,153.36777], "fy":[-1.56147,-1.56142,-1.56142,-1.56147]},
-    {"t":0.14852, "x":1.2172, "y":6.63806, "heading":0.0, "vx":1.33932, "vy":-0.01371, "omega":0.0, "ax":9.01484, "ay":-0.09128, "alpha":0.0, "fx":[153.33989,153.33989,153.33989,153.33989], "fy":[-1.55262,-1.55257,-1.55257,-1.55262]},
-    {"t":0.19803, "x":1.29456, "y":6.63727, "heading":0.0, "vx":1.78562, "vy":-0.01823, "omega":0.0, "ax":9.01255, "ay":-0.09074, "alpha":0.0, "fx":[153.30084,153.30084,153.30084,153.30084], "fy":[-1.54354,-1.54349,-1.54349,-1.54354]},
-    {"t":0.24754, "x":1.394, "y":6.63626, "heading":0.0, "vx":2.2318, "vy":-0.02272, "omega":0.0, "ax":9.0091, "ay":-0.09019, "alpha":0.0, "fx":[153.24222,153.24222,153.24222,153.24222], "fy":[-1.53409,-1.53404,-1.53404,-1.53409]},
-    {"t":0.29704, "x":1.51553, "y":6.63502, "heading":0.0, "vx":2.67782, "vy":-0.02719, "omega":0.0, "ax":9.00335, "ay":-0.08959, "alpha":0.0, "fx":[153.14449,153.14449,153.14449,153.14449], "fy":[-1.5239,-1.52385,-1.52385,-1.5239]},
-    {"t":0.34655, "x":1.65914, "y":6.63357, "heading":0.0, "vx":3.12355, "vy":-0.03162, "omega":0.0, "ax":8.99187, "ay":-0.08888, "alpha":0.0, "fx":[152.94907,152.94907,152.94907,152.94907], "fy":[-1.51186,-1.51181,-1.51181,-1.51186]},
-    {"t":0.39606, "x":1.8248, "y":6.63189, "heading":0.0, "vx":3.56872, "vy":-0.03602, "omega":0.0, "ax":8.95747, "ay":-0.08774, "alpha":0.0, "fx":[152.36392,152.36392,152.36392,152.36392], "fy":[-1.49246,-1.49242,-1.49242,-1.49246]},
-    {"t":0.44557, "x":2.01245, "y":6.63, "heading":0.0, "vx":4.01218, "vy":-0.04037, "omega":0.0, "ax":0.00034, "ay":0.04107, "alpha":0.00005, "fx":[0.00573,0.00573,0.00598,0.00598], "fy":[0.69874,0.6985,0.6985,0.69874]},
-    {"t":0.49507, "x":2.21109, "y":6.62805, "heading":0.0, "vx":4.01219, "vy":-0.03833, "omega":0.0, "ax":-8.95746, "ay":0.08803, "alpha":0.00001, "fx":[-152.36377,-152.36377,-152.36377,-152.36377], "fy":[1.49742,1.49737,1.49737,1.49742]},
-    {"t":0.54458, "x":2.39874, "y":6.62626, "heading":0.0, "vx":3.56873, "vy":-0.03397, "omega":0.0, "ax":-8.99187, "ay":0.08757, "alpha":0.0, "fx":[-152.94919,-152.94919,-152.94919,-152.94919], "fy":[1.48952,1.48947,1.48947,1.48952]},
-    {"t":0.59409, "x":2.5644, "y":6.62469, "heading":0.0, "vx":3.12357, "vy":-0.02964, "omega":0.0, "ax":-9.00337, "ay":0.08709, "alpha":0.0, "fx":[-153.1448,-153.1448,-153.1448,-153.1448], "fy":[1.48133,1.48128,1.48128,1.48134]},
-    {"t":0.6436, "x":2.70801, "y":6.62333, "heading":0.0, "vx":2.67784, "vy":-0.02533, "omega":0.0, "ax":-9.00913, "ay":0.0866, "alpha":0.0, "fx":[-153.24271,-153.24271,-153.24271,-153.24271], "fy":[1.47309,1.47303,1.47303,1.47309]},
-    {"t":0.6931, "x":2.82954, "y":6.62218, "heading":0.0, "vx":2.23182, "vy":-0.02104, "omega":0.0, "ax":-9.01259, "ay":0.08612, "alpha":0.0, "fx":[-153.3015,-153.3015,-153.3015,-153.3015], "fy":[1.46482,1.46477,1.46477,1.46482]},
-    {"t":0.74261, "x":2.92898, "y":6.62125, "heading":0.0, "vx":1.78563, "vy":-0.01678, "omega":0.0, "ax":-9.01489, "ay":0.08563, "alpha":0.0, "fx":[-153.34072,-153.34072,-153.34072,-153.34072], "fy":[1.45654,1.45649,1.45649,1.45654]},
-    {"t":0.79212, "x":3.00634, "y":6.62052, "heading":0.0, "vx":1.33933, "vy":-0.01254, "omega":0.0, "ax":-9.01654, "ay":0.08514, "alpha":0.0, "fx":[-153.36876,-153.36876,-153.36876,-153.36876], "fy":[1.44826,1.44821,1.44821,1.44827]},
-    {"t":0.84163, "x":3.0616, "y":6.62, "heading":0.0, "vx":0.89294, "vy":-0.00832, "omega":0.0, "ax":-9.01778, "ay":0.08466, "alpha":0.0, "fx":[-153.38981,-153.38981,-153.38981,-153.38981], "fy":[1.43999,1.43993,1.43993,1.43999]},
-    {"t":0.89113, "x":3.09475, "y":6.61969, "heading":0.0, "vx":0.44649, "vy":-0.00413, "omega":0.0, "ax":-9.01875, "ay":0.08344, "alpha":-0.00004, "fx":[-153.40632,-153.40631,-153.40632,-153.40632], "fy":[1.41913,1.42009,1.41908,1.41913]},
-    {"t":0.94064, "x":3.1058, "y":6.61959, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+    {"t":0.0, "x":1.11774, "y":6.63908, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":24.86759, "ay":-0.2557, "alpha":0.0, "fx":[140.99685,140.99685,140.99685,140.99685], "fy":[-1.4498,-1.4498,-1.4498,-1.4498]},
+    {"t":0.04987, "x":1.14866, "y":6.63877, "heading":0.0, "vx":1.24008, "vy":-0.01275, "omega":0.0, "ax":24.8602, "ay":-0.25389, "alpha":0.0, "fx":[140.95498,140.95498,140.95498,140.95498], "fy":[-1.43953,-1.43953,-1.43953,-1.43953]},
+    {"t":0.09973, "x":1.24141, "y":6.63781, "heading":0.0, "vx":2.47979, "vy":-0.02541, "omega":0.0, "ax":24.84521, "ay":-0.25197, "alpha":0.0, "fx":[140.86998,140.86998,140.86998,140.86998], "fy":[-1.42863,-1.42863,-1.42863,-1.42863]},
+    {"t":0.1496, "x":1.39596, "y":6.63623, "heading":0.0, "vx":3.71875, "vy":-0.03798, "omega":0.0, "ax":24.79862, "ay":-0.24959, "alpha":0.0, "fx":[140.60581,140.60581,140.60581,140.60581], "fy":[-1.41514,-1.41514,-1.41514,-1.41514]},
+    {"t":0.19947, "x":1.61224, "y":6.63403, "heading":0.0, "vx":4.95538, "vy":-0.05042, "omega":0.0, "ax":1.4235, "ay":0.00895, "alpha":0.0, "fx":[8.0711,8.0711,8.0711,8.0711], "fy":[0.05073,0.05073,0.05073,0.05073]},
+    {"t":0.24934, "x":1.86112, "y":6.63153, "heading":0.0, "vx":5.02637, "vy":-0.04998, "omega":0.0, "ax":0.00016, "ay":0.01405, "alpha":0.0, "fx":[0.00092,0.00092,0.00092,0.00092], "fy":[0.07965,0.07965,0.07965,0.07965]},
+    {"t":0.2992, "x":2.11177, "y":6.62905, "heading":0.0, "vx":5.02638, "vy":-0.04928, "omega":0.0, "ax":0.00009, "ay":0.01403, "alpha":0.0, "fx":[0.00053,0.00053,0.00053,0.00053], "fy":[0.07957,0.07957,0.07957,0.07957]},
+    {"t":0.34907, "x":2.36242, "y":6.62661, "heading":0.0, "vx":5.02638, "vy":-0.04858, "omega":0.0, "ax":-1.42314, "ay":0.03678, "alpha":0.0, "fx":[-8.06908,-8.06908,-8.06908,-8.06908], "fy":[0.20856,0.20856,0.20856,0.20856]},
+    {"t":0.39894, "x":2.6113, "y":6.62423, "heading":0.0, "vx":4.95541, "vy":-0.04674, "omega":0.0, "ax":-24.7987, "ay":0.23665, "alpha":0.0, "fx":[-140.60628,-140.60628,-140.60628,-140.60628], "fy":[1.3418,1.3418,1.3418,1.3418]},
+    {"t":0.4488, "x":2.82758, "y":6.6222, "heading":0.0, "vx":3.71877, "vy":-0.03494, "omega":0.0, "ax":-24.84535, "ay":0.23519, "alpha":0.0, "fx":[-140.87076,-140.87076,-140.87076,-140.87076], "fy":[1.33353,1.33353,1.33353,1.33353]},
+    {"t":0.49867, "x":2.98213, "y":6.62075, "heading":0.0, "vx":2.4798, "vy":-0.02321, "omega":0.0, "ax":-24.86038, "ay":0.23357, "alpha":0.0, "fx":[-140.95598,-140.95598,-140.95598,-140.95598], "fy":[1.32433,1.32433,1.32433,1.32433]},
+    {"t":0.54854, "x":3.07488, "y":6.61988, "heading":0.0, "vx":1.24009, "vy":-0.01156, "omega":0.0, "ax":-24.8678, "ay":0.23191, "alpha":0.0, "fx":[-140.99807,-140.99807,-140.99807,-140.99807], "fy":[1.31493,1.31493,1.31493,1.31493]},
+    {"t":0.59841, "x":3.1058, "y":6.61959, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
   "splits":[0]
  },
  "events":[]

diff --git a/src/main/deploy/choreo/auto.chor b/src/main/deploy/choreo/auto.chor
@@ -9,64 +9,64 @@
  "config":{
   "frontLeft":{
    "x":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"10.375 in",
+    "val":0.263525
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"10.375 in",
+    "val":0.263525
    }
   },
   "backLeft":{
    "x":{
-    "exp":"-11 in",
-    "val":-0.2794
+    "exp":"-10.375 in",
+    "val":-0.263525
    },
    "y":{
-    "exp":"11 in",
-    "val":0.2794
+    "exp":"10.375 in",
+    "val":0.263525
    }
   },
   "mass":{
-   "exp":"150 lbs",
-   "val":68.0388555
+   "exp":"50 lbs",
+   "val":22.6796185
   },
   "inertia":{
    "exp":"6 kg m ^ 2",
    "val":6.0
   },
   "gearing":{
-   "exp":"6.5",
-   "val":6.5
+   "exp":"6.122448979591837",
+   "val":6.122448979591837
   },
   "radius":{
-   "exp":"2 in",
-   "val":0.0508
+   "exp":"1.93 in",
+   "val":0.049022
   },
   "vmax":{
    "exp":"6000 RPM",
    "val":628.3185307179587
   },
   "tmax":{
-   "exp":"1.2 N * m",
-   "val":1.2
+   "exp":"1.13 N * m",
+   "val":1.13
   },
   "cof":{
-   "exp":"1.5",
-   "val":1.5
+   "exp":"1.1",
+   "val":1.1
   },
   "bumper":{
    "front":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"15.875 in",
+    "val":0.403225
    },
    "side":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"15.875 in",
+    "val":0.403225
    },
    "back":{
-    "exp":"16 in",
-    "val":0.4064
+    "exp":"15.875 in",
+    "val":0.403225
    }
   },
   "differentialTrackWidth":{

diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "Robot2025";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 12;
-  public static final String GIT_SHA = "6bff4a00f08253dd287838a34b8f20fe45963926";
-  public static final String GIT_DATE = "2025-01-06 19:29:19 EST";
+  public static final int GIT_REVISION = 13;
+  public static final String GIT_SHA = "193a848e9859b69bcb15b258738466d4fb1d80c6";
+  public static final String GIT_DATE = "2025-01-07 19:15:49 EST";
   public static final String GIT_BRANCH = "main";
-  public static final String BUILD_DATE = "2025-01-07 18:57:29 EST";
-  public static final long BUILD_UNIX_TIME = 1736294249173L;
+  public static final String BUILD_DATE = "2025-01-08 20:25:06 EST";
+  public static final long BUILD_UNIX_TIME = 1736385906585L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -26,6 +26,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.drive.Drivetrain;
@@ -97,6 +98,24 @@ public RobotContainer() {
     // Add options to the chooser
     autoChooser.addRoutine("Example Auto Command", this::exampleAuto);
 
+    autoChooser.addCmd(
+        "Drive Simple FF Characterization",
+        () -> DriveCommands.feedforwardCharacterization(driveSubsystem));
+    autoChooser.addCmd(
+        "Drive SysId (Quasistatic Forward)",
+        () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    autoChooser.addCmd(
+        "Drive SysId (Quasistatic Backward)",
+        () -> driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addCmd(
+        "Drive SysId (Dynamic Forward)",
+        () -> driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    autoChooser.addCmd(
+        "Drive SysId (Dynamic Backward)",
+        () -> driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    autoChooser.addCmd(
+        "Wheel Radius", () -> DriveCommands.wheelRadiusCharacterization(driveSubsystem));
+
     // Put the auto chooser on the dashboard
     SmartDashboard.putData("Auto Chooser", autoChooser);