From a763accccb85ae17d48a6f2391d024ccfc6f7d01 Mon Sep 17 00:00:00 2001
From: ThomasP850 <tp8505583@gmail.com>
Date: Sat, 4 Jan 2025 15:35:13 -0500
Subject: [PATCH] Main gimbal control and limits

---
 app/inc/gimbal_task.h | 25 +++++++++++---
 app/src/gimbal_task.c | 80 ++++++++++++++++++++++++++++++++-----------
 app/src/robot.c       | 10 +++++-
 3 files changed, 90 insertions(+), 25 deletions(-)

diff --git a/app/inc/gimbal_task.h b/app/inc/gimbal_task.h
index 1f9b2a8..e629206 100644
--- a/app/inc/gimbal_task.h
+++ b/app/inc/gimbal_task.h
@@ -1,11 +1,28 @@
 #ifndef GIMBAL_TASK_H
 #define GIMBAL_TASK_H
 
-#define YAW_MID_POSITION
-#define PITCH_MID_POSITION
+// TODO: Find correct values
 
-#define PITCH_VELOCITY_SCALE (0.01f)
-#define YAW_VELOCITY_SCALE (0.01f)
+// Note: these values are in degrees so that they are human-readable
+
+#define PITCH_LOWER_LIMIT (0.0f)
+#define PITCH_UPPER_LIMIT (45.0f)
+
+#define PITCH_OFFSET (0.0f)
+#define YAW_OFFSET (0.0f)
+
+#define PITCH_CONTROLLER_VELOCITY_COEF (1e-6f)
+#define YAW_CONTORLLER_VELOCITY_COEF (5e-5f)
+
+#define PITCH_MOUSE_VELOCITY_COEF (5e-5f)
+#define YAW_MOUSE_VELOCITY_COEF (1e-5f)
+
+// TODO: Find correct values
+
+// Degrees per second
+
+#define PITCH_RATE_LIMIT (1.0f)
+#define YAW_RATE_LIMIT (1.0f)
 
 typedef struct
 {
diff --git a/app/src/gimbal_task.c b/app/src/gimbal_task.c
index 4e94f25..d84b6dd 100644
--- a/app/src/gimbal_task.c
+++ b/app/src/gimbal_task.c
@@ -1,39 +1,79 @@
 #include "gimbal_task.h"
 
+#include "dji_motor.h"
+#include "imu_task.h"
+#include "jetson_orin.h"
+#include "pid.h"
+#include "rate_limiter.h"
 #include "robot.h"
 #include "remote.h"
 #include "user_math.h"
 
 extern Robot_State_t g_robot_state;
 extern Remote_t g_remote;
+extern IMU_t g_imu;
+extern Jetson_Orin_Data_t g_orin_data;
 
-DJI_Motor_Handle_t *g_gimbal_motors[2];
+DJI_Motor_Handle_t* g_pitch_motor;
+DJI_Motor_Handle_t* g_yaw_motor;
+rate_limiter_t g_yaw_rate_limiter;
+rate_limiter_t g_pitch_rate_limiter;
 
 void Gimbal_Task_Init()
 {
-    for (int i = 0; i < 2; i++)
-    {
-        Motor_Config_t chassis_motor_config = {
+    // TODO: Work with gear ratios
+    Motor_Config_t pitch_motor_config = {
         .can_bus = 2,
-        .speed_controller_id = i+1,
-        .offset = 0,
-        .control_mode = POSITION,
+        .speed_controller_id = 0,
+        .offset = PITCH_OFFSET,
+        .control_mode = POSITION_CONTROL,
         .motor_reversal = MOTOR_REVERSAL_NORMAL,
-        .angle = 
-            {
-                .kp = 1000.0f,
-                .ki = 0.0f,
-                .kd = 100.0f,
-                .output_limit = M2006_MAX_CURRENT,
-            }};
-        g_chassis_motors[i] = DJI_Motor_Init(&chassis_motor_config, M2006);
-    }
+        .angle_pid = {
+            .kp = 1000.0f,
+            .ki = 0.0f,
+            .kd = 100.0f,
+            .output_limit = M2006_MAX_CURRENT,
+        }
+    };
+    g_pitch_motor = DJI_Motor_Init(&pitch_motor_config, M2006);
+
+    Motor_Config_t yaw_motor_config = {
+        .can_bus = 2,
+        .speed_controller_id = 1,
+        .offset = YAW_OFFSET,
+        .control_mode = POSITION_CONTROL,
+        .motor_reversal = MOTOR_REVERSAL_NORMAL,
+        .angle_pid = {
+            .kp = 1000.0f,
+            .ki = 0.0f,
+            .kd = 100.0f,
+            .output_limit = M2006_MAX_CURRENT,
+        }
+    };
+    g_pitch_motor = DJI_Motor_Init(&yaw_motor_config, M2006);
+
+    rate_limiter_init(&g_yaw_rate_limiter, YAW_RATE_LIMIT);
+    rate_limiter_init(&g_pitch_rate_limiter, PITCH_RATE_LIMIT);
+
 }
 
 void Gimbal_Ctrl_Loop()
 {
-    float yaw_velocity = g_remote.controller.right_stick.x;
-    float pitch_velocity = g_remote.controller.right_stick.y;
-    g_robot_state.gimbal.pitch_angle += pitch_velocity * 0.01f;
-    g_robot_state.gimbal.yaw_angle += yaw_velocity * 0.01f;
+
+    if (g_robot_state.launch.IS_AUTO_AIMING_ENABLED) {
+        if (g_orin_data.receiving.auto_aiming.pitch || g_orin_data.receiving.auto_aiming.yaw) {
+            // We can set these directly for now, they will be limited later in this function
+            g_robot_state.gimbal.yaw_angle = (g_imu.deg.yaw - g_orin_data.receiving.auto_aiming.yaw) * DEG_TO_RAD;
+            g_robot_state.gimbal.pitch_angle = (g_imu.deg.pitch - g_orin_data.receiving.auto_aiming.pitch) * DEG_TO_RAD;
+        }
+    }
+
+    g_robot_state.gimbal.yaw_angle = fmod(g_robot_state.gimbal.yaw_angle, 2 * PI);
+    __MAX_LIMIT(g_robot_state.gimbal.pitch_angle, PITCH_LOWER_LIMIT, PITCH_UPPER_LIMIT);
+
+    g_robot_state.gimbal.yaw_angle = rate_limiter(&g_yaw_rate_limiter, g_robot_state.gimbal.yaw_angle);
+    g_robot_state.gimbal.pitch_angle = rate_limiter(&g_pitch_rate_limiter, g_robot_state.gimbal.pitch_angle);
+
+    DJI_Motor_Set_Angle(g_yaw_motor, g_robot_state.gimbal.yaw_angle);
+    DJI_Motor_Set_Angle(g_pitch_motor, g_robot_state.gimbal.pitch_angle);
 }
diff --git a/app/src/robot.c b/app/src/robot.c
index 06abc99..86d29f1 100644
--- a/app/src/robot.c
+++ b/app/src/robot.c
@@ -98,7 +98,15 @@ void Handle_Disabled_State()
 
 void Process_Remote_Input()
 {
-    // USER CODE HERE
+    // Gimbal control
+    g_robot_state.gimbal.yaw_angle -= (
+        g_remote.controller.right_stick.x * YAW_CONTORLLER_VELOCITY_COEF +
+        g_remote.mouse.x * YAW_MOUSE_VELOCITY_COEF
+    );
+    g_robot_state.gimbal.pitch_angle -= (
+        g_remote.controller.right_stick.y * PITCH_CONTROLLER_VELOCITY_COEF - 
+        g_remote.mouse.y * PITCH_MOUSE_VELOCITY_COEF
+    );
 }
 
 void Process_Chassis_Control()