Add Controls of 8 Thrusters for Crush

onboard/src/controls/config/crush.yaml

@@ -52,8 +52,8 @@ pid:
       error_ramp_rate: 1000
     yaw:
       Kp: 0.09
-      Ki: 0.003
-      Kd: 0
+      Ki: 0
+      Kd: 0.02
       Ff: 0
       control_effort:
         min: -1
@@ -206,30 +206,40 @@ static_power_global:
   y: 0
   z: -0.206
 power_scale_factor: 1
+# all x positions assume the corner link is 10.41527MM +x than it actually is
+# see README.md for more details
 thrusters:
-  - name: middle_right
-    type: T200
-    pos: [-0.2404152701, -0.42425, -0.003175]
-    rpy: [0, 90, 0]
   - name: front_right
     type: T200
-    pos: [3.360246e-05, -0.3867988726, -0.003175]
+    pos: [0.001895, -0.38866, -0.003175]
     rpy: [0, 0, -135]
+  - name: middle_right_first
+    type: T200
+    pos: [-0.15501, -0.34805, -0.003175]
+    rpy: [0, 90, 0]
+  - name: middle_right_second
+    type: T200
+    pos: [-0.32582, -0.34805, -0.003175]
+    rpy: [0, 90, 0]
   - name: back_right
     type: T200
-    pos: [-0.4808641427, -0.3867988726, -0.003175]
+    pos: [-0.48273, -0.38866, -0.003175]
     rpy: [0, 0, 135]
   - name: back_left
     type: T200
-    pos: [-0.4808641427, 0.09409887258, -0.003175]
+    pos: [-0.48273, 0.09596, -0.003175]
     rpy: [0, 0, -135]
-  - name: middle_left
+  - name: middle_left_first
+    type: T200
+    pos: [-0.15501, 0.05535, -0.003175]
+    rpy: [0, 90, 0]
+  - name: middle_left_second
     type: T200
-    pos: [-0.2404152701, 0.13155, -0.003175]
+    pos: [-0.32582, 0.05535, -0.003175]
     rpy: [0, 90, 0]
   - name: front_left
     type: T200
-    pos: [3.360246e-05, 0.09409887258, -0.003175]
+    pos: [0.001895, 0.09596, -0.003175]
     rpy: [0, 0, 135]
 wrench_matrix_file_path: data/crush_wrench.csv
 wrench_matrix_pinv_file_path: data/crush_wrench_pinv.csv

onboard/src/controls/data/crush_wrench.csv

@@ -1,6 +1,6 @@
-0.0,-0.7071067812,-0.7071067812,-0.7071067812,0.0,-0.7071067812
-0.0,-0.7071067812,0.7071067812,-0.7071067812,0.0,0.7071067812
--1.0,0.0,0.0,0.0,-1.0,0.0
-0.2779,0.0104651804,-0.0104651804,0.0104651804,-0.2779,-0.0104651804
-0.0,-0.0104651804,-0.0104651804,-0.0104651804,0.0,-0.0104651804
-0.0,-0.3400460567,-0.3400460567,0.3400460567,0.0,0.3400460567
+-0.7071067812,0.0,0.0,-0.7071067812,-0.7071067812,0.0,0.0,-0.7071067812
+-0.7071067812,0.0,0.0,0.7071067812,-0.7071067812,0.0,0.0,0.7071067812
+0.0,-1.0,-1.0,0.0,0.0,-1.0,-1.0,0.0
+0.0104651804,0.2017,0.2017,-0.0104651804,0.0104651804,-0.2017,-0.2017,-0.0104651804
+-0.0104651804,0.0854052701,-0.0854047299,-0.0104651804,-0.0104651804,0.0854052701,-0.0854047299,-0.0104651804
+-0.3426782793,0.0,0.0,-0.3426814328,0.3426814328,0.0,0.0,0.3426782793

onboard/src/controls/data/crush_wrench_pinv.csv

@@ -1,6 +1,8 @@
-0.0,0.0266282836,-0.5,1.7992083483,0.0,0.0
--0.3534759652,-0.3535533906,0.0,0.0,-0.0052314443,-0.7351945276
--0.3534759652,0.3535533906,0.0,0.0,-0.0052314443,-0.7351945276
--0.3534759652,-0.3535533906,0.0,0.0,-0.0052314443,0.7351945276
-0.0,-0.0266282836,-0.5,-1.7992083483,0.0,0.0
--0.3534759652,0.3535533906,0.0,0.0,-0.0052314443,0.7351945276
+-0.3535533906,-0.3535550174,0.0,0.0,0.0,-0.729543904
+-0.0433229905,0.0183440754,-0.2499992093,1.2394645513,2.9272290849,0.0
+0.0433229905,0.0183440754,-0.2500007907,1.2394645513,-2.9272290849,0.0
+-0.3535533906,0.3535517638,0.0,0.0,0.0,-0.729543904
+-0.3535533906,-0.3535517638,0.0,0.0,0.0,0.729543904
+-0.0433229905,-0.0183440754,-0.2499992093,-1.2394645513,2.9272290849,0.0
+0.0433229905,-0.0183440754,-0.2500007907,-1.2394645513,-2.9272290849,0.0
+-0.3535533906,0.3535550174,0.0,0.0,0.0,0.729543904

onboard/src/offboard_comms/config/crush.yaml

@@ -26,33 +26,41 @@ arduino:
         tag: HB
         topic: /sensors/humidity/battery
   thruster:
-    ftdi: A10LT2EI
+    ftdi: AQ02V0UK
     fqbn: arduino:avr:nano
     core: arduino:avr
     sketch: sketches/thruster
+    libraries:
+      - "'Adafruit BusIO'"
 gyro:
   ftdi: FT81SUWZ
-  zero_bias: 0.0023800993
+  zero_bias: 0.0024940463
   scale_factor: 15497811.0
   negate: true
 modem:
   ftdi: CP2102
 thrusters:
-  - name: middle_right
+  - name: front_right
     cw: false
     standard_esc: false
-  - name: front_right
+  - name: middle_right_first
     cw: false
+    standard_esc: true
+  - name: middle_right_second
+    cw: true
     standard_esc: false
   - name: back_right
     cw: false
     standard_esc: true
   - name: back_left
     cw: true
-    standard_esc: false
-  - name: middle_left
+    standard_esc: true
+  - name: middle_left_first
+    cw: false
+    standard_esc: true
+  - name: middle_left_second
     cw: true
     standard_esc: true
   - name: front_left
     cw: true
-    standard_esc: false
+    standard_esc: true

onboard/src/offboard_comms/offboard_comms/test_thrusters.py

@@ -32,7 +32,7 @@ class ThrusterTester(Node):
 
     CONFIG_PATH = f'package://controls/config/{os.getenv("ROBOT_NAME")}.yaml'
 
-    def __init__(self, speed: float, rate: float, log_allocs: bool) -> None:
+    def __init__(self, speed: float, rate: float, log_allocs: bool, tTestNum: int) -> None:
         """
         Initialize the thruster tester node.
 
@@ -46,7 +46,13 @@ def __init__(self, speed: float, rate: float, log_allocs: bool) -> None:
         self.publisher_ = self.create_publisher(ThrusterAllocs, '/controls/thruster_allocs', 10)
         self.num_thrusters = self.get_thruster_count()
         self.thrust_speed = speed
-        self.allocs = [self.thrust_speed] * self.num_thrusters
+        if (tTestNum == 0):
+            self.allocs = [self.thrust_speed] * self.num_thrusters
+        else:
+            sArr = [0.0]*self.num_thrusters
+            sArr[tTestNum - 1] = speed
+            self.allocs = sArr
+
         self.msg = ThrusterAllocs()
         self.msg.allocs = self.allocs
         self.log_allocs = log_allocs
@@ -86,10 +92,11 @@ def main(args: list[str] | None = None) -> None:
     parser.add_argument('-r', '--rate', type=float, default=DEFAULT_RATE,
                         help=f'Publishing rate in Hz (default: {DEFAULT_RATE}).')
     parser.add_argument('--log-allocs', action='store_true', help='Log each thruster allocs message published.')
+    parser.add_argument('-t', '--test-num', type=int, default=0, help='Thruster number to test (default: 0 for all thrusters).')
     parsed_args = parser.parse_args()
 
     rclpy.init(args=args)
-    node = ThrusterTester(parsed_args.speed, parsed_args.rate, parsed_args.log_allocs)
+    node = ThrusterTester(parsed_args.speed, parsed_args.rate, parsed_args.log_allocs, parsed_args.test_num)
     try:
         rclpy.spin(node)
     except KeyboardInterrupt: