add auto aim to state machine

src/algo/src/Swerve_Locomotion.c

@@ -163,28 +163,6 @@ Module_State_Array_t Chassis_Speeds_To_Module_States(Chassis_Speeds_t chassis_sp
     return calculated_module_states;
 }
 
-/* Set the desired modules state of each module */
-void Set_Desired_States(Module_State_Array_t desired_states)
-{
-    g_swerve_fl.module_state = desired_states.states[0];
-    g_swerve_rl.module_state = desired_states.states[1];
-    g_swerve_rr.module_state = desired_states.states[2];
-    g_swerve_fr.module_state = desired_states.states[3];
-}
-
-/* Commands modules to stop moving and reset angle to 0. Should be called on robot enable */
-void Reset_Modules()
-{
-    Module_State_Array_t desired_states = {
-        .states = {
-            {0.0f, 0.0f},
-            {0.0f, 0.0f},
-            {0.0f, 0.0f},
-            {0.0f, 0.0f}
-        }};
-    Set_Desired_States(desired_states);
-}
-
 /**
  * @brief Optimize Module Angle
  * Optimize the module angle and velocity to minimize the movement of the module
@@ -246,6 +224,15 @@ void Set_Module_Output(Swerve_Module_t *swerve_module, Module_State_t desired_st
     DJI_Motor_Set_Velocity(swerve_module->drive_motor, optimized_module_state.speed * 60 / (PI * Wheel_Diameter));
 }
 
+/* Set the desired modules state of each module */
+void Set_Desired_States(Module_State_Array_t desired_states)
+{
+    g_swerve_fl.module_state = desired_states.states[0];
+    g_swerve_rl.module_state = desired_states.states[1];
+    g_swerve_rr.module_state = desired_states.states[2];
+    g_swerve_fr.module_state = desired_states.states[3];
+}
+
 /**
  * Takes inputs from (-1 to 1) and sets the respective module outputs.
  *
@@ -267,6 +254,18 @@ void Swerve_Drive(float x, float y, float omega)
     }
 }
 
+/* Commands modules to stop moving and reset angle to 0. Should be called on robot enable */
+void Reset_Modules()
+{
+    Module_State_Array_t desired_states = {
+        .states = {
+            {0.0f, 0.0f},
+            {0.0f, 0.0f},
+            {0.0f, 0.0f},
+            {0.0f, 0.0f}}};
+    Set_Desired_States(desired_states);
+}
+
 /**
  * Disables all the motors used in the swerve.
  */
@@ -282,5 +281,4 @@ void Swerve_Disable()
     DJI_Motor_Disable(g_swerve_rr.drive_motor);
     DJI_Motor_Disable(g_swerve_rl.azimuth_motor);
     DJI_Motor_Disable(g_swerve_rl.drive_motor);
-
 }

src/app/src/gimbal_task.c

@@ -4,6 +4,7 @@
 #include "remote.h"
 #include "imu_task.h"
 #include "user_math.h"
+#include "jetson_orin.h"
 #include <math.h>
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
@@ -42,6 +43,8 @@ void Gimbal_Task_Init()
             },
     };
 
+    #pragma message "CHANGE PITCH MOTOR OFFSET DO NO TRUN BEFORE DOING THIS"
+    int a = 0;
     Motor_Config_t pitch_motor_config = {
         .can_bus = 2,
         .speed_controller_id = 2,
@@ -76,7 +79,6 @@ void Gimbal_Ctrl_Loop()
     if (g_robot_state.enabled)
     {
         __MAX_LIMIT(g_gimbal_target.pitch, -0.45f, 0.5f);
-
         DJI_Motor_Set_Angle(g_pitch, g_robot_state.gimbal_pitch_angle);
         DJI_Motor_Set_Angle(g_yaw, g_robot_state.gimbal_yaw_angle);
     }

src/app/src/robot.c

@@ -7,6 +7,7 @@
 #include "remote.h"
 #include "bsp_can.h"
 #include "gimbal_task.h"
+#include "imu_task.h"
 #include <math.h>
 
 extern DJI_Motor_Handle_t *g_yaw;
@@ -51,14 +52,15 @@ void Robot_Cmd_Loop() {
         } else if (g_remote.controller.left_switch == MID){
             g_robot_state.chassis_omega = SPIN_TOP_OMEGA;
         } else if (g_remote.controller.left_switch == UP) {
-            // RESERVED        }
+            // RESERVED
         }
 
         if (g_remote.controller.right_switch == MID) {
             g_robot_state.gimbal_yaw_angle -= (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x); // controller and mouse
             g_robot_state.gimbal_pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f + g_remote.mouse.y); // controller and mouse
         } else if (g_remote.controller.right_switch == UP) {
-            // TODO: Algorithm Auto Aiming
+            g_robot_state.gimbal_pitch_angle = g_orin_data.receiving.auto_aiming.pitch + g_imu.rad.roll;
+            g_robot_state.gimbal_yaw_angle = g_orin_data.receiving.auto_aiming.yaw + g_imu.rad.yaw;
         }
     }
 }