Change navX communication to SPI (why was it on serial since 2022?), …

…rough vet bot offsets, math to calculate max accel
blair-robot-project · Dec 6, 2023 · 290dc76 · 290dc76
1 parent 943ab91
commit 290dc76
Show file tree

Hide file tree

Showing 6 changed files with 20 additions and 19 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -94,7 +94,8 @@
       "guid": "Keyboard0"
     },
     {
-      "guid": "Keyboard1"
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
@@ -50,7 +50,6 @@
       },
       "/SmartDashboard/Field": {
         "height": 8.013679504394531,
-        "image": "C:\\Users\\jeffe\\Downloads\\bunnybotsfield2023--.png",
         "width": 16.541748046875,
         "window": {
           "visible": true

diff --git a/src/main/kotlin/frc/team449/robot2023/Robot.kt b/src/main/kotlin/frc/team449/robot2023/Robot.kt
@@ -1,7 +1,7 @@
 package frc.team449.robot2023
 
 import edu.wpi.first.wpilibj.PowerDistribution
-import edu.wpi.first.wpilibj.SerialPort
+import edu.wpi.first.wpilibj.SPI
 import edu.wpi.first.wpilibj.XboxController
 import frc.team449.RobotBase
 import frc.team449.control.holonomic.SwerveDrive
@@ -20,7 +20,7 @@ class Robot : RobotBase(), Logged {
 
   val mechController = XboxController(1)
 
-  val ahrs = AHRS(SerialPort.Port.kMXP)
+  val ahrs = AHRS(SPI.Port.kMXP)
 
   // Instantiate/declare PDP and other stuff here
 

diff --git a/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt
@@ -5,7 +5,6 @@ import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.system.plant.DCMotor
 import edu.wpi.first.math.util.Units
-import edu.wpi.first.wpilibj.RobotBase
 import frc.team449.robot2023.constants.drives.SwerveConstants
 import kotlin.math.PI
 
@@ -20,25 +19,27 @@ object RobotConstants {
   const val ROTATION_DEADBAND = .15
 
   /** In kilograms, include bumpers and battery and all */
-  const val ROBOT_WEIGHT = 60
+  const val ROBOT_WEIGHT = 55
 
   /** Drive configuration */
   const val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s
   const val MAX_ROT_SPEED = PI // rad/s
-  val MAX_ACCEL = if (RobotBase.isSimulation()) {
-    12.2625
-  } else {
-    4 * DCMotor(
+  val MAX_ACCEL = 4 * DCMotor(
       MotorConstants.NOMINAL_VOLTAGE,
       MotorConstants.STALL_TORQUE,
       MotorConstants.STALL_CURRENT,
       MotorConstants.FREE_CURRENT,
       MotorConstants.FREE_SPEED,
       1
-    ).getTorque(80.0) * SwerveConstants.DRIVE_GEARING / (Units.inchesToMeters(2.0) * ROBOT_WEIGHT) // m/s/s
-  }
+    ).getTorque(90.0)/
+    (Units.inchesToMeters(2.0) * ROBOT_WEIGHT * SwerveConstants.DRIVE_GEARING) // m/s/s
+
   val INITIAL_POSE = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0))
 
+
+  init {
+    println("Max Accel $MAX_ACCEL")
+  }
   const val LOOP_TIME = 0.020
 
   /** PID controller for Orthogonal turning */

diff --git a/src/main/kotlin/frc/team449/robot2023/constants/drives/SwerveConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/drives/SwerveConstants.kt
@@ -16,18 +16,18 @@ object SwerveConstants {
   /** Turning encoder channels */
   const val TURN_ENC_CHAN_FL = 6
   const val TURN_ENC_CHAN_FR = 9
-  const val TURN_ENC_CHAN_BL = 7
+  const val TURN_ENC_CHAN_BL = 5
   const val TURN_ENC_CHAN_BR = 8
 
   // BL 0.205474
   // BR 0.620525
   // FL 0.365650
   // FR 0.790104
   /** Offsets for the absolute encoders in rotations */
-  const val TURN_ENC_OFFSET_FL = 0.365650 + .5
-  const val TURN_ENC_OFFSET_FR = 0.540868
-  const val TURN_ENC_OFFSET_BL = 0.205474 + .5
-  const val TURN_ENC_OFFSET_BR = 0.620525 - .5
+  const val TURN_ENC_OFFSET_FL = 0.400578
+  const val TURN_ENC_OFFSET_FR = 0.395996
+  const val TURN_ENC_OFFSET_BL = 0.288496
+  const val TURN_ENC_OFFSET_BR = 0.215352
 
   /** PID gains for turning each module */
   const val TURN_KP = 0.8

diff --git a/src/main/kotlin/frc/team449/system/AHRS.kt b/src/main/kotlin/frc/team449/system/AHRS.kt
@@ -2,7 +2,7 @@ package frc.team449.system
 
 import edu.wpi.first.math.filter.LinearFilter
 import edu.wpi.first.math.geometry.Rotation2d
-import edu.wpi.first.wpilibj.SerialPort
+import edu.wpi.first.wpilibj.SPI
 import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.simulation.SimDeviceSim
 import frc.team449.util.simBooleanProp
@@ -50,7 +50,7 @@ class AHRS(
   }
 
   constructor(
-    port: SerialPort.Port = SerialPort.Port.kMXP
+    port: SPI.Port = SPI.Port.kMXP
   ) : this(
     com.kauailabs.navx.frc.AHRS(port)
   )