From e32944d157bbbe654c6eade5a229e25544725288 Mon Sep 17 00:00:00 2001
From: Rick Xu <rickxu2@illinois.edu>
Date: Sat, 9 Nov 2024 19:50:20 -0600
Subject: [PATCH] disable arm_translate (physically unplugged)

---
 .../Engineer/chassis/include/arm_translate_task.h    |  4 ++--
 vehicles/Engineer/chassis/src/chassis_task.cc        | 12 +++++-------
 2 files changed, 7 insertions(+), 9 deletions(-)

diff --git a/vehicles/Engineer/chassis/include/arm_translate_task.h b/vehicles/Engineer/chassis/include/arm_translate_task.h
index 5201b992..cf15f889 100644
--- a/vehicles/Engineer/chassis/include/arm_translate_task.h
+++ b/vehicles/Engineer/chassis/include/arm_translate_task.h
@@ -34,8 +34,8 @@ void kill_arm_translate();
 void revive_arm_translate();
 
 
-/* M3508 params start */
-const int BASE_TRANSLATE_ID = 0x205;
+/* M3508 params start */ 
+const int BASE_TRANSLATE_ID = 0x209; // TODO: need to reconnect
 //const GPIO_TypeDef* BASE_TRANS_INIT_GPIO_PORT = GPIOI;
 //const uint16_t BASE_TRANS_INIT_GPIO_PIN = GPIO_PIN_7;  // PWM pin 1 on C board
 
diff --git a/vehicles/Engineer/chassis/src/chassis_task.cc b/vehicles/Engineer/chassis/src/chassis_task.cc
index c3a2c0ed..99113562 100644
--- a/vehicles/Engineer/chassis/src/chassis_task.cc
+++ b/vehicles/Engineer/chassis/src/chassis_task.cc
@@ -52,7 +52,7 @@ void chassisTask(void* arg){
     UNUSED(arg);
 
     control::MotorCANBase* wheel_motors[] = {motor5, motor6, motor7, motor8};
-    control::MotorCANBase* steer_motors[] = {motor2, motor3, motor4};
+    control::MotorCANBase* steer_motors[] = {motor2, motor3, motor4, motor1};
     chassis->SteerSetMaxSpeed(ALIGN_SPEED);
 	chassis->Calibrate();
 
@@ -165,8 +165,7 @@ void chassisTask(void* arg){
                        50);
 
 
-        control::MotorCANBase::TransmitOutput(steer_motors, 3);
-        control::MotorCANBase::TransmitOutput(&motor1, 1);
+        control::MotorCANBase::TransmitOutput(steer_motors, 4);
         control::MotorCANBase::TransmitOutput(wheel_motors, 4);
 
         // if(HAL_GetTick() - last_cap_send > 10){ // 100Hz
@@ -194,7 +193,7 @@ void chassisTask(void* arg){
 
 void init_chassis(){
     print("Starting chassis init\n");
-    motor1 = new control::Motor6020(can1, 0x209);
+    motor1 = new control::Motor6020(can1, 0x205);
     motor2 = new control::Motor6020(can1, 0x206);
     motor3 = new control::Motor6020(can1, 0x207);
     motor4 = new control::Motor6020(can1, 0x208);
@@ -258,7 +257,7 @@ void kill_chassis(){
     RM_EXPECT_TRUE(false, "Operation Killed!\r\n");
 
     control::MotorCANBase* wheel_motors[] = {motor5, motor6, motor7, motor8}; 
-    control::MotorCANBase* steer_motors[] = {motor2, motor3, motor4};
+    control::MotorCANBase* steer_motors[] = {motor2, motor3, motor4, motor1};
 
     // RGB->Display(display::color_blue);
    // set alignment status of each wheel to false
@@ -267,8 +266,7 @@ void kill_chassis(){
     motor7->SetOutput(0);
     motor8->SetOutput(0);
     control::MotorCANBase::TransmitOutput(wheel_motors, 4);
-    control::MotorCANBase::TransmitOutput(&motor1, 1); // id 0x209 has a different identifier
-    control::MotorCANBase::TransmitOutput(steer_motors, 3);
+    control::MotorCANBase::TransmitOutput(steer_motors, 4);
 }
 
 void revive_chassis(){