can bus connection change, now feedback frequency is 250Hz per motor …

…and all chassis motors are connected to the same bus.
illini-robomaster · Nov 9, 2024 · bfaf532 · bfaf532
1 parent 831e4ab
commit bfaf532
Showing 1 changed file with 13 additions and 22 deletions.
diff --git a/vehicles/Engineer/chassis/src/chassis_task.cc b/vehicles/Engineer/chassis/src/chassis_task.cc
@@ -51,10 +51,8 @@ static bool killed;
 void chassisTask(void* arg){
     UNUSED(arg);
 
-    control::MotorCANBase* wheel_motors_2[] = {motor6, motor8};
-    control::MotorCANBase* wheel_motors_1[] = {motor5, motor7};
-
-    control::MotorCANBase* steer_motors_2[] = {motor2, motor4};
+    control::MotorCANBase* wheel_motors[] = {motor5, motor6, motor7, motor8};
+    control::MotorCANBase* steer_motors[] = {motor2, motor3, motor4};
     chassis->SteerSetMaxSpeed(ALIGN_SPEED);
 	chassis->Calibrate();
 
@@ -167,17 +165,9 @@ void chassisTask(void* arg){
                        50);
 
 
-        control::MotorCANBase::TransmitOutput(steer_motors_2, 2);
-        control::MotorCANBase::TransmitOutput(&motor3, 1);
+        control::MotorCANBase::TransmitOutput(steer_motors, 3);
         control::MotorCANBase::TransmitOutput(&motor1, 1);
-
-
-        control::MotorCANBase::TransmitOutput(wheel_motors_1, 2);
-        control::MotorCANBase::TransmitOutput(wheel_motors_2, 2);
-
-        UNUSED(steer_motors_2);
-        UNUSED(wheel_motors_1);
-        UNUSED(wheel_motors_2);
+        control::MotorCANBase::TransmitOutput(wheel_motors, 4);
 
         // if(HAL_GetTick() - last_cap_send > 10){ // 100Hz
         //     last_cap_send = HAL_GetTick();
@@ -205,14 +195,14 @@ void chassisTask(void* arg){
 void init_chassis(){
     print("Starting chassis init\n");
     motor1 = new control::Motor6020(can1, 0x209);
-    motor2 = new control::Motor6020(can2, 0x206);
+    motor2 = new control::Motor6020(can1, 0x206);
     motor3 = new control::Motor6020(can1, 0x207);
-    motor4 = new control::Motor6020(can2, 0x208);
+    motor4 = new control::Motor6020(can1, 0x208);
 
     motor5 = new control::Motor3508(can1, 0x201);
-    motor6 = new control::Motor3508(can2, 0x202);
+    motor6 = new control::Motor3508(can1, 0x202);
     motor7 = new control::Motor3508(can1, 0x203);
-    motor8 = new control::Motor3508(can2, 0x204);
+    motor8 = new control::Motor3508(can1, 0x204);
 
     chassis_data = new control::engineer_steering_chassis_t();
 
@@ -267,17 +257,18 @@ void kill_chassis(){
     killed = true;
     RM_EXPECT_TRUE(false, "Operation Killed!\r\n");
 
-    control::MotorCANBase* wheel_motors_2[] = {motor6, motor8};
-    control::MotorCANBase* wheel_motors_1[] = {motor5, motor7};
+    control::MotorCANBase* wheel_motors[] = {motor5, motor6, motor7, motor8}; 
+    control::MotorCANBase* steer_motors[] = {motor2, motor3, motor4};
 
     // RGB->Display(display::color_blue);
    // set alignment status of each wheel to false
     motor5->SetOutput(0);
     motor6->SetOutput(0);
     motor7->SetOutput(0);
     motor8->SetOutput(0);
-    control::MotorCANBase::TransmitOutput(wheel_motors_1, 2);
-    control::MotorCANBase::TransmitOutput(wheel_motors_2, 2);
+    control::MotorCANBase::TransmitOutput(wheel_motors, 4);
+    control::MotorCANBase::TransmitOutput(&motor1, 1); // id 0x209 has a different identifier
+    control::MotorCANBase::TransmitOutput(steer_motors, 3);
 }
 
 void revive_chassis(){