From 66e55738eba5458bb7fe60731f5d1e8c4b630dbd Mon Sep 17 00:00:00 2001 From: Irving Wang Date: Sat, 29 Mar 2025 12:46:05 -0400 Subject: [PATCH 01/13] Create hero from template --- Hero/inc/chassis_task.h | 8 +++ Hero/inc/debug_task.h | 9 +++ Hero/inc/gimbal_task.h | 18 +++++ Hero/inc/launch_task.h | 7 ++ Hero/inc/motor_task.h | 6 ++ Hero/inc/robot.h | 105 ++++++++++++++++++++++++++++++ Hero/inc/robot_tasks.h | 133 +++++++++++++++++++++++++++++++++++++ Hero/src/chassis_task.c | 19 ++++++ Hero/src/debug_task.c | 52 +++++++++++++++ Hero/src/gimbal_task.c | 18 +++++ Hero/src/launch_task.c | 21 ++++++ Hero/src/motor_task.c | 20 ++++++ Hero/src/robot.c | 141 ++++++++++++++++++++++++++++++++++++++++ 13 files changed, 557 insertions(+) create mode 100644 Hero/inc/chassis_task.h create mode 100644 Hero/inc/debug_task.h create mode 100644 Hero/inc/gimbal_task.h create mode 100644 Hero/inc/launch_task.h create mode 100644 Hero/inc/motor_task.h create mode 100644 Hero/inc/robot.h create mode 100644 Hero/inc/robot_tasks.h create mode 100644 Hero/src/chassis_task.c create mode 100644 Hero/src/debug_task.c create mode 100644 Hero/src/gimbal_task.c create mode 100644 Hero/src/launch_task.c create mode 100644 Hero/src/motor_task.c create mode 100644 Hero/src/robot.c diff --git a/Hero/inc/chassis_task.h b/Hero/inc/chassis_task.h new file mode 100644 index 0000000..3fb057b --- /dev/null +++ b/Hero/inc/chassis_task.h @@ -0,0 +1,8 @@ +#ifndef CHASSIS_TASK_H +#define CHASSIS_TASK_H + +// Function prototypes +void Chassis_Task_Init(void); +void Chassis_Ctrl_Loop(void); + +#endif // CHASSIS_TASK_H diff --git a/Hero/inc/debug_task.h b/Hero/inc/debug_task.h new file mode 100644 index 0000000..876f98c --- /dev/null +++ b/Hero/inc/debug_task.h @@ -0,0 +1,9 @@ +#ifndef DEBUG_TASK_H +#define DEBUG_TASK_H + +#define DEBUG_PERIOD (10) + +void Debug_Task_Init(void); +void Debug_Task_Loop(void); + +#endif // DEBUG_TASK_H \ No newline at end of file diff --git a/Hero/inc/gimbal_task.h b/Hero/inc/gimbal_task.h new file mode 100644 index 0000000..79220fe --- /dev/null +++ b/Hero/inc/gimbal_task.h @@ -0,0 +1,18 @@ +#ifndef GIMBAL_TASK_H +#define GIMBAL_TASK_H + +#define YAW_MID_POSITION +#define PITCH_MID_POSITION + +typedef struct +{ + float pitch; + float yaw_velocity; + float yaw_angle; +} Gimbal_Target_t; + +// Function prototypes +void Gimbal_Task_Init(void); +void Gimbal_Ctrl_Loop(void); + +#endif // GIMBAL_TASK_H diff --git a/Hero/inc/launch_task.h b/Hero/inc/launch_task.h new file mode 100644 index 0000000..5e82166 --- /dev/null +++ b/Hero/inc/launch_task.h @@ -0,0 +1,7 @@ +#ifndef LAUNCH_TASK_H +#define LAUNCH_TASK_H + +void Launch_Task_Init(void); +void Launch_Ctrl_Loop(void); + +#endif // LAUNCH_TASK_H diff --git a/Hero/inc/motor_task.h b/Hero/inc/motor_task.h new file mode 100644 index 0000000..e7d285a --- /dev/null +++ b/Hero/inc/motor_task.h @@ -0,0 +1,6 @@ +#ifndef MOTOR_TASK_H +#define MOTOR_TASK_H + +void Motor_Task_Loop(void); + +#endif // MOTOR_TASK_H diff --git a/Hero/inc/robot.h b/Hero/inc/robot.h new file mode 100644 index 0000000..4c6de26 --- /dev/null +++ b/Hero/inc/robot.h @@ -0,0 +1,105 @@ +#ifndef ROBOT_H +#define ROBOT_H + +#include "user_math.h" +#include + +typedef enum Robot_State_e { + // Primary Enable Modes + STARTING_UP, + DISABLED, + ENABLED +} Robot_State_e; + +typedef struct { + // chassis motion + float x_speed; + float y_speed; + float omega; + uint8_t IS_SPINTOP_ENABLED; + + // power management + uint16_t power_index; + uint16_t power_count; + float avg_power; + float total_power; + float power_increment_ratio; +} Chassis_State_t; + +typedef struct { + float pitch_angle; + float yaw_angle; +} Gimbal_State_t; + +typedef enum Fire_Mode_e { + NO_FIRE, + SINGLE_FIRE, + BURST_FIRE, + FULL_AUTO, + REJIGGLE, // primarily for busy mode + IDLE // primarily for busy mode +} Fire_Mode_e; + +typedef struct Shooter_State_t { + float prev_time; + float prev_vel; + float accum_angle; +} Shooter_State_t; + +typedef struct { + uint8_t IS_FIRING_ENABLED; + uint8_t IS_AUTO_AIMING_ENABLED; + uint8_t IS_FLYWHEEL_ENABLED; + uint8_t IS_BUSY; + + Fire_Mode_e fire_mode; // requested fire mode + Fire_Mode_e busy_mode; // current fire mode (in progress) + Shooter_State_t shooter_state; // used for position integration +} Launch_State_t; + +typedef struct { + // controller input + float vx; + float vy; + + // mouse input + float vx_keyboard; + float vy_keyboard; + + // previous switch states + uint8_t prev_left_switch; + uint8_t prev_right_switch; + + // previous key states + uint8_t prev_B; + uint8_t prev_G; + uint8_t prev_V; + uint8_t prev_Z; + uint8_t prev_Shift; +} Input_State_t; + +typedef struct { + Robot_State_e state; + Chassis_State_t chassis; + Gimbal_State_t gimbal; + Launch_State_t launch; + Input_State_t input; + + uint8_t IS_SUPER_CAPACITOR_ENABLED; + uint8_t UI_ENABLED; + uint8_t IS_SAFELY_STARTED; +} Robot_State_t; + +void Robot_Init(void); +void Robot_Command_Loop(void); +void Handle_Starting_Up_State(void); +void Handle_Enabled_State(void); +void Handle_Disabled_State(void); +void Process_Remote_Input(void); +void Process_Chassis_Control(void); +void Process_Gimbal_Control(void); +void Process_Launch_Control(void); + +extern Robot_State_t g_robot_state; + +#endif // ROBOT_H diff --git a/Hero/inc/robot_tasks.h b/Hero/inc/robot_tasks.h new file mode 100644 index 0000000..43eb862 --- /dev/null +++ b/Hero/inc/robot_tasks.h @@ -0,0 +1,133 @@ +#pragma once + +#include "FreeRTOS.h" +#include "task.h" +#include "main.h" +#include "cmsis_os.h" + +#include "robot.h" +#include "launch_task.h" +#include "motor_task.h" +#include "debug_task.h" +#include "jetson_orin.h" +#include "bsp_serial.h" +#include "bsp_daemon.h" + +extern void IMU_Task(void const *pvParameters); + +osThreadId imu_task_handle; +osThreadId robot_command_task_handle; +osThreadId motor_task_handle; +osThreadId ui_task_handle; +osThreadId debug_task_handle; +osThreadId jetson_orin_task_handle; +osThreadId daemon_task_handle; + +void Robot_Tasks_Robot_Command(void const *argument); +void Robot_Tasks_Motor(void const *argument); +void Robot_Tasks_IMU(void const *argument); +void Robot_Tasks_UI(void const *argument); +void Robot_Tasks_Debug(void const *argument); +void Robot_Tasks_Jetson_Orin(void const *argument); +void Robot_Tasks_Daemon(void const *argument); + +void Robot_Tasks_Start() +{ + osThreadDef(imu_task, Robot_Tasks_IMU, osPriorityAboveNormal, 0, 1024); + imu_task_handle = osThreadCreate(osThread(imu_task), NULL); + + osThreadDef(motor_task, Robot_Tasks_Motor, osPriorityAboveNormal, 0, 256); + motor_task_handle = osThreadCreate(osThread(motor_task), NULL); + + osThreadDef(robot_command_task, Robot_Tasks_Robot_Command, osPriorityAboveNormal, 0, 256); + robot_command_task_handle = osThreadCreate(osThread(robot_command_task), NULL); + + osThreadDef(ui_task, Robot_Tasks_UI, osPriorityAboveNormal, 0, 256); + ui_task_handle = osThreadCreate(osThread(ui_task), NULL); + + osThreadDef(debug_task, Robot_Tasks_Debug, osPriorityIdle, 0, 256); + debug_task_handle = osThreadCreate(osThread(debug_task), NULL); + + osThreadDef(jetson_orin_task, Robot_Tasks_Jetson_Orin, osPriorityAboveNormal, 0, 256); + jetson_orin_task_handle = osThreadCreate(osThread(jetson_orin_task), NULL); + + osThreadDef(daemon_task, Robot_Tasks_Daemon, osPriorityAboveNormal, 0, 256); + daemon_task_handle = osThreadCreate(osThread(daemon_task), NULL); +} + +void Robot_Tasks_Robot_Command(void const *argument) +{ + portTickType xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + const TickType_t TimeIncrement = pdMS_TO_TICKS(2); + while (1) + { + Robot_Command_Loop(); + vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + } +} + +__weak void Robot_Tasks_IMU(void const *argument) +{ + IMU_Task(argument); +} + +void Robot_Tasks_Motor(void const *argument) +{ + portTickType xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + const TickType_t TimeIncrement = pdMS_TO_TICKS(1); + while (1) + { + Motor_Task_Loop(); + vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + } +} + +void Robot_Tasks_UI(void const *argument) +{ + portTickType xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + const TickType_t TimeIncrement = pdMS_TO_TICKS(1); + while (1) + { + // UI_Task_Loop(); + vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + } +} + +void Robot_Tasks_Debug(void const *argument) +{ + portTickType xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + const TickType_t TimeIncrement = pdMS_TO_TICKS(DEBUG_PERIOD); + while (1) + { + Debug_Task_Loop(); + vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + } +} + +void Robot_Tasks_Jetson_Orin(void const *argument) +{ + portTickType xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + const TickType_t TimeIncrement = pdMS_TO_TICKS(JETSON_ORIN_PERIOD); + while (1) + { + Jetson_Orin_Send_Data(); + vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + } +} + +void Robot_Tasks_Daemon(void const *argument) +{ + portTickType xLastWakeTime; + xLastWakeTime = xTaskGetTickCount(); + const TickType_t TimeIncrement = pdMS_TO_TICKS(DAEMON_PERIOD); + while (1) + { + Daemon_Task_Loop(); + vTaskDelayUntil(&xLastWakeTime, TimeIncrement); + } +} diff --git a/Hero/src/chassis_task.c b/Hero/src/chassis_task.c new file mode 100644 index 0000000..0a74dd4 --- /dev/null +++ b/Hero/src/chassis_task.c @@ -0,0 +1,19 @@ +#include "chassis_task.h" + +#include "robot.h" +#include "remote.h" + +extern Robot_State_t g_robot_state; +extern Remote_t g_remote; + +float chassis_rad; + +void Chassis_Task_Init() +{ + // Init chassis hardware +} + +void Chassis_Ctrl_Loop() +{ + // Control loop for the chassis +} \ No newline at end of file diff --git a/Hero/src/debug_task.c b/Hero/src/debug_task.c new file mode 100644 index 0000000..ad5fb4a --- /dev/null +++ b/Hero/src/debug_task.c @@ -0,0 +1,52 @@ +#include "debug_task.h" + +#include "bsp_serial.h" +#include "remote.h" +#include "user_math.h" +#include "imu_task.h" +#include "robot.h" +#include "referee_system.h" +#include "jetson_orin.h" +#include "bsp_daemon.h" +#include "launch_task.h" + +extern Robot_State_t g_robot_state; +extern IMU_t g_imu; +extern Remote_t g_remote; +extern Daemon_Instance_t *g_daemon_instances[3]; +extern Daemon_Instance_t *g_remote_daemon; +extern Daemon_Instance_t *g_referee_daemon_instance_ptr; +extern float test_tmd; +// #define PRINT_RUNTIME_STATS +#ifdef PRINT_RUNTIME_STATS +char g_debug_buffer[1024 * 2] = {0}; +#endif + +const char *top_border = "\r\n\r\n\r\n/***** System Info *****/\r\n"; +const char *bottom_border = "/***** End of Info *****/\r\n"; + +#define DEBUG_ENABLED + +void Debug_Task_Loop(void) +{ +#ifdef DEBUG_ENABLED +// static uint32_t counter = 0; +#ifdef PRINT_RUNTIME_STATS + if (counter % 100 == 0) // Print every 100 cycles + { + vTaskGetRunTimeStats(g_debug_buffer); + DEBUG_PRINTF(&huart6, "%s", top_border); + DEBUG_PRINTF(&huart6, "%s", g_debug_buffer); + DEBUG_PRINTF(&huart6, "%s", bottom_border); + } +#endif + // DEBUG_PRINTF(&huart6, ">time:%.1f\n>ref:%f\n",(float) counter / 1000.0f * DEBUG_PERIOD,Referee_Robot_State.Chassis_Power); + // DEBUG_PRINTF(&huart6, ">time:%.1f\n>yaw:%f\n>pitch:%f\n>roll:%f\n", (float) counter / 1000.0f * DEBUG_PERIOD, + // g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll); + // DEBUG_PRINTF(&huart6, ">remote_daemon:%d\n", g_remote_daemon->counter); + // counter++; + // if (counter > 0xFFFFFFFF) { + // counter = 0; + // } +#endif +} \ No newline at end of file diff --git a/Hero/src/gimbal_task.c b/Hero/src/gimbal_task.c new file mode 100644 index 0000000..4bebfff --- /dev/null +++ b/Hero/src/gimbal_task.c @@ -0,0 +1,18 @@ +#include "gimbal_task.h" + +#include "robot.h" +#include "remote.h" +#include "user_math.h" + +extern Robot_State_t g_robot_state; +extern Remote_t g_remote; + +void Gimbal_Task_Init() +{ + // Init Gimbal Hardware +} + +void Gimbal_Ctrl_Loop() +{ + // Control loop for gimbal +} diff --git a/Hero/src/launch_task.c b/Hero/src/launch_task.c new file mode 100644 index 0000000..3906b18 --- /dev/null +++ b/Hero/src/launch_task.c @@ -0,0 +1,21 @@ +#include "launch_task.h" + +#include "dji_motor.h" +#include "robot.h" +#include "remote.h" +#include "imu_task.h" +#include "user_math.h" +#include "referee_system.h" + +extern Robot_State_t g_robot_state; +extern Remote_t g_remote; + +void Launch_Task_Init() +{ + // Init Launch Hardware +} + +void Launch_Ctrl_Loop() +{ + // Control loop for launch +} diff --git a/Hero/src/motor_task.c b/Hero/src/motor_task.c new file mode 100644 index 0000000..83e1a7f --- /dev/null +++ b/Hero/src/motor_task.c @@ -0,0 +1,20 @@ +#include "motor_task.h" +#include "dji_motor.h" +#include "dm_motor.h" +#include "mf_motor.h" +#include "supercap.h" + +extern Supercap_t g_supercap; + +void Motor_Task_Loop() { + DJI_Motor_Send(); + // MF_Motor_Send(); + // DM_Motor_Send(); + + g_supercap.send_counter++; + if (g_supercap.send_counter >= 100) { + Supercap_Send(); + g_supercap.send_counter = 0; + } +} + diff --git a/Hero/src/robot.c b/Hero/src/robot.c new file mode 100644 index 0000000..06abc99 --- /dev/null +++ b/Hero/src/robot.c @@ -0,0 +1,141 @@ +#include "robot.h" + +#include "robot_tasks.h" +#include "chassis_task.h" +#include "gimbal_task.h" +#include "launch_task.h" +#include "remote.h" +#include "gimbal_task.h" +#include "imu_task.h" +#include "referee_system.h" +#include "buzzer.h" +#include "supercap.h" +#include "dji_motor.h" + +Robot_State_t g_robot_state = {0}; +extern Remote_t g_remote; +extern Supercap_t g_supercap; + +/** + * @brief This function initializes the robot. + * This means setting the state to STARTING_UP, + * initializing the buzzer, and calling the + * Robot_Task_Start() for the task scheduling + */ +void Robot_Init() +{ + g_robot_state.state = STARTING_UP; + + Buzzer_Init(); + Melody_t system_init_melody = { + .notes = SYSTEM_INITIALIZING, + .loudness = 0.5f, + .note_num = SYSTEM_INITIALIZING_NOTE_NUM, + }; + Buzzer_Play_Melody(system_init_melody); // TODO: Change to non-blocking + + // Initialize all tasks + Robot_Tasks_Start(); +} + +/** + * @brief This function handles the starting up state of the robot, initializing all hardware. + */ +void Handle_Starting_Up_State() +{ + // Initialize all hardware + Chassis_Task_Init(); + Gimbal_Task_Init(); + Launch_Task_Init(); + Remote_Init(&huart3); + CAN_Service_Init(); + Referee_System_Init(&huart1); + Supercap_Init(&g_supercap); + + // Set robot state to disabled + g_robot_state.state = DISABLED; +} + +/** + * @brief This function handles the enabled state of the robot. + * This means processing remote input, and subsystem control. + */ +void Handle_Enabled_State() +{ + if (g_remote.online_flag == REMOTE_OFFLINE || g_remote.controller.right_switch == DOWN) + { + g_robot_state.state = DISABLED; + } + else + { + // Process movement and components in enabled state + Referee_Set_Robot_State(); + Process_Remote_Input(); + Process_Chassis_Control(); + Process_Gimbal_Control(); + Process_Launch_Control(); + } +} + +/** + * @brief This function handles the disabled state of the robot. + * This means disabling all motors and components + */ +void Handle_Disabled_State() +{ + DJI_Motor_Disable_All(); + // Disable all major components + g_robot_state.launch.IS_FLYWHEEL_ENABLED = 0; + g_robot_state.chassis.x_speed = 0; + g_robot_state.chassis.y_speed = 0; + + if (g_remote.online_flag == REMOTE_ONLINE && g_remote.controller.right_switch != DOWN) + { + g_robot_state.state = ENABLED; + DJI_Motor_Enable_All(); + } +} + +void Process_Remote_Input() +{ + // USER CODE HERE +} + +void Process_Chassis_Control() +{ + // USER CODE HERE +} + +void Process_Gimbal_Control() +{ + // USER CODE HERE +} + +void Process_Launch_Control() +{ + // USER CODE HERE +} + +/** + * This function is called periodically by the Robot Task. + * It serves as the top level state machine for the robot based on the current state. + * Appropriate functions are called. + */ +void Robot_Command_Loop() +{ + switch (g_robot_state.state) + { + case STARTING_UP: + Handle_Starting_Up_State(); + break; + case DISABLED: + Handle_Disabled_State(); + break; + case ENABLED: + Handle_Enabled_State(); + break; + default: + Error_Handler(); + break; + } +} \ No newline at end of file From c689f4c275fb269e29fc369c2fefaa099725d028 Mon Sep 17 00:00:00 2001 From: Irving Wang Date: Sat, 29 Mar 2025 15:49:33 -0400 Subject: [PATCH 02/13] hero chassis draft --- .vscode/launch.json | 1 + .vscode/tasks.json | 1 + Hero/inc/chassis_task.h | 8 +++++ Hero/inc/robot.h | 1 + Hero/src/chassis_task.c | 79 ++++++++++++++++++++++++++++++++++++++++- Hero/src/robot.c | 23 +++++++++--- Sentry/src/robot.c | 6 ++-- Template/src/robot.c | 6 ++-- 8 files changed, 114 insertions(+), 11 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 6154703..3a22f26 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -50,6 +50,7 @@ "options": [ "Swerve-Standard", "Sentry", + "Hero", //"Template" // Add your other robot projects here ] diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 37aeabc..572e521 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -118,6 +118,7 @@ "options": [ "Swerve-Standard", "Sentry", + "Hero" //"Template" // Add your other robot projects here ] diff --git a/Hero/inc/chassis_task.h b/Hero/inc/chassis_task.h index 3fb057b..e021bf0 100644 --- a/Hero/inc/chassis_task.h +++ b/Hero/inc/chassis_task.h @@ -1,6 +1,14 @@ #ifndef CHASSIS_TASK_H #define CHASSIS_TASK_H +#define SPIN_TOP_OMEGA 2 // rad / s +#define MAX_ANGLUAR_SPEED 3.14 // rad / s +#define CHASSIS_WHEEL_DIAMETER (0.15f) // m +#define CHASSIS_RADIUS (0.305f) // center to wheel, m +#define CHASSIS_MAX_SPEED (2.0f) // m/s +#define CHASSIS_MOUNTING_ANGLE (PI / 4) // rad (45deg) +#define MAX_ABC (200.0f) // rad/s // TODO rename this macro + // Function prototypes void Chassis_Task_Init(void); void Chassis_Ctrl_Loop(void); diff --git a/Hero/inc/robot.h b/Hero/inc/robot.h index 4c6de26..260dc9c 100644 --- a/Hero/inc/robot.h +++ b/Hero/inc/robot.h @@ -61,6 +61,7 @@ typedef struct { // controller input float vx; float vy; + float vomega; // mouse input float vx_keyboard; diff --git a/Hero/src/chassis_task.c b/Hero/src/chassis_task.c index 0a74dd4..4a6127e 100644 --- a/Hero/src/chassis_task.c +++ b/Hero/src/chassis_task.c @@ -1,19 +1,96 @@ #include "chassis_task.h" +#include "omni_locomotion.h" +#include "rate_limiter.h" #include "robot.h" #include "remote.h" +#include "dji_motor.h" + extern Robot_State_t g_robot_state; extern Remote_t g_remote; -float chassis_rad; +DJI_Motor_Handle_t *motors[4]; +uint8_t drive_esc_id_array[4] = {1, 2, 3, 4}; +Motor_Reversal_t drive_motor_reversal_array[4] = { + MOTOR_REVERSAL_REVERSED, + MOTOR_REVERSAL_REVERSED, + MOTOR_REVERSAL_REVERSED, + MOTOR_REVERSAL_REVERSED +}; + +omni_physical_constants_t physical_constants; +omni_chassis_state_t chassis_state; + +rate_limiter_t wheel_rate_limiters[4]; void Chassis_Task_Init() { // Init chassis hardware + Motor_Config_t drive_motor_config = { + .can_bus = 1, + .control_mode = VELOCITY_CONTROL, + .velocity_pid = { + .kp = 500.0f, + .kf = 100.0f, + .output_limit = M3508_MAX_CURRENT, + .integral_limit = 3000.0f, + }}; + + for (int i = 0; i < 4; i++) { + // configure drive motor + drive_motor_config.speed_controller_id = drive_esc_id_array[i]; + drive_motor_config.motor_reversal = drive_motor_reversal_array[i]; + motors[i] = DJI_Motor_Init(&drive_motor_config, M3508); + DJI_Motor_Set_Control_Mode(motors[i], VELOCITY_CONTROL); + } + + // Initialize the omni chassis state + physical_constants = omni_init( + CHASSIS_WHEEL_DIAMETER, + CHASSIS_RADIUS, + CHASSIS_MOUNTING_ANGLE, + CHASSIS_MAX_SPEED + ); + + chassis_state.v_x = 0.0f; + chassis_state.v_y = 0.0f; + chassis_state.omega = 0.0f; + + for (int i = 0; i < 4; i++) { + // configure rate limiters + rate_limiter_init(&wheel_rate_limiters[i], MAX_ABC); + } } void Chassis_Ctrl_Loop() { + if (g_robot_state.chassis.IS_SPINTOP_ENABLED) { + chassis_state.omega = SPIN_TOP_OMEGA; + } else { + //chassis_state.omega = g_robot_state.chassis.omega * MAX_ANGLUAR_SPEED; + chassis_state.omega = g_robot_state.input.vomega; + } + + chassis_state.v_x = g_robot_state.input.vy; // x and y are swapped due to joytick orientation + chassis_state.v_y = -g_robot_state.input.vx; + + // Control loop for the chassis + omni_calculate_kinematics(&chassis_state, &physical_constants); + omni_desaturate_wheel_speeds(&chassis_state, &physical_constants); + omni_convert_to_rpm(&chassis_state); + + // use rate limiter to limit acceleration of the wheels + chassis_state.phi_dot_1 = rate_limiter_iterate(&wheel_rate_limiters[0], chassis_state.phi_dot_1); + chassis_state.phi_dot_2 = rate_limiter_iterate(&wheel_rate_limiters[1], chassis_state.phi_dot_2); + chassis_state.phi_dot_3 = rate_limiter_iterate(&wheel_rate_limiters[2], chassis_state.phi_dot_3); + chassis_state.phi_dot_4 = rate_limiter_iterate(&wheel_rate_limiters[3], chassis_state.phi_dot_4); + + // set the velocities of the wheels + DJI_Motor_Set_Velocity(motors[0], chassis_state.phi_dot_1); + DJI_Motor_Set_Velocity(motors[1], chassis_state.phi_dot_2); + DJI_Motor_Set_Velocity(motors[2], chassis_state.phi_dot_3); + DJI_Motor_Set_Velocity(motors[3], chassis_state.phi_dot_4); + } \ No newline at end of file diff --git a/Hero/src/robot.c b/Hero/src/robot.c index 06abc99..f7296a0 100644 --- a/Hero/src/robot.c +++ b/Hero/src/robot.c @@ -98,22 +98,37 @@ void Handle_Disabled_State() void Process_Remote_Input() { - // USER CODE HERE + g_robot_state.input.vx = g_remote.controller.left_stick.x; + g_robot_state.input.vy = g_remote.controller.left_stick.y; + g_robot_state.input.vomega = g_remote.controller.right_stick.x; + + if (__IS_TRANSITIONED(g_remote.controller.left_switch, g_robot_state.input.prev_left_switch, MID)) + { + g_robot_state.chassis.IS_SPINTOP_ENABLED = 1; + } + if (__IS_TRANSITIONED(g_remote.controller.left_switch, g_robot_state.input.prev_left_switch, DOWN) || + __IS_TRANSITIONED(g_remote.controller.left_switch, g_robot_state.input.prev_left_switch, UP)) + { + g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; + } + + g_robot_state.input.prev_left_switch = g_remote.controller.left_switch; } void Process_Chassis_Control() { - // USER CODE HERE + // USER CODE HERE + Chassis_Ctrl_Loop(); } void Process_Gimbal_Control() { - // USER CODE HERE + // USER CODE HERE } void Process_Launch_Control() { - // USER CODE HERE + // USER CODE HERE } /** diff --git a/Sentry/src/robot.c b/Sentry/src/robot.c index 9a412e6..7117f62 100644 --- a/Sentry/src/robot.c +++ b/Sentry/src/robot.c @@ -117,17 +117,17 @@ void Process_Remote_Input() void Process_Chassis_Control() { - Chassis_Ctrl_Loop(); + Chassis_Ctrl_Loop(); } void Process_Gimbal_Control() { - // USER CODE HERE + // USER CODE HERE } void Process_Launch_Control() { - // USER CODE HERE + // USER CODE HERE } /** diff --git a/Template/src/robot.c b/Template/src/robot.c index 06abc99..ea3b7f3 100644 --- a/Template/src/robot.c +++ b/Template/src/robot.c @@ -103,17 +103,17 @@ void Process_Remote_Input() void Process_Chassis_Control() { - // USER CODE HERE + // USER CODE HERE } void Process_Gimbal_Control() { - // USER CODE HERE + // USER CODE HERE } void Process_Launch_Control() { - // USER CODE HERE + // USER CODE HERE } /** From 6e56a53ecf9e7ad71da81b79cb7ede0ca3d7b1ea Mon Sep 17 00:00:00 2001 From: ThomasP850 Date: Sat, 29 Mar 2025 16:16:36 -0400 Subject: [PATCH 03/13] Co-authored-by: Sanjith Cherumandanda Co-authored-by: Ben Wehnert --- Hero/inc/launch_task.h | 18 ++++ Hero/inc/robot.h | 2 + Hero/src/launch_task.c | 158 ++++++++++++++++++++++++++++-- Hero/src/robot.c | 54 ++++++++-- control-base/algo/inc/user_math.h | 1 + 5 files changed, 218 insertions(+), 15 deletions(-) diff --git a/Hero/inc/launch_task.h b/Hero/inc/launch_task.h index 5e82166..ee36e75 100644 --- a/Hero/inc/launch_task.h +++ b/Hero/inc/launch_task.h @@ -1,7 +1,25 @@ #ifndef LAUNCH_TASK_H #define LAUNCH_TASK_H +#include "dji_motor.h" + + +// TODO: Adjust these values once we have an actual bot +#define NUM_SHOTS 8 +#define SHOT_ANGLE_OFFSET_RAD (TAU / NUM_SHOTS) +#define FEED_TOLERANCE (5 * PI / 180) // 5 degree tolerance +#define FEED_RATE 60 / NUM_SHOTS * 60 // 60 HZ + void Launch_Task_Init(void); void Launch_Ctrl_Loop(void); +void handleSingleFire(void); +void startFlywheel(void); +void stopFlywheel(void); +void handleFullAuto(void); + +/** + * @brief Rejiggle the feed motor to prevent jams + */ +void rejiggle(void); #endif // LAUNCH_TASK_H diff --git a/Hero/inc/robot.h b/Hero/inc/robot.h index 260dc9c..3b93df9 100644 --- a/Hero/inc/robot.h +++ b/Hero/inc/robot.h @@ -4,6 +4,8 @@ #include "user_math.h" #include +#define KEYBOARD_RAMP_COEF (0.004f) + typedef enum Robot_State_e { // Primary Enable Modes STARTING_UP, diff --git a/Hero/src/launch_task.c b/Hero/src/launch_task.c index 3906b18..5c00a74 100644 --- a/Hero/src/launch_task.c +++ b/Hero/src/launch_task.c @@ -3,19 +3,165 @@ #include "dji_motor.h" #include "robot.h" #include "remote.h" -#include "imu_task.h" #include "user_math.h" #include "referee_system.h" +#include "laser.h" +#include + +#define ticksToDegrees(ticks) ((ticks) * 360.0f / DJI_MAX_TICKS) +#define ticksToRad(ticks) ((ticks) * TAU / DJI_MAX_TICKS) +#define degreesToTicks(degrees) ((degrees) * DJI_MAX_TICKS / 360.0f) extern Robot_State_t g_robot_state; extern Remote_t g_remote; -void Launch_Task_Init() -{ +DJI_Motor_Handle_t *g_flywheel_left, *g_flywheel_right, *g_feed_motor; + +void Launch_Task_Init() { // Init Launch Hardware + Motor_Config_t flywheel_left_config = { + .can_bus = 1, + .speed_controller_id = 4, + .offset = 0, + .control_mode = VELOCITY_CONTROL, + .motor_reversal = MOTOR_REVERSAL_NORMAL, + .velocity_pid = + { + .kp = 500.0f, + .output_limit = M3508_MAX_CURRENT, + }, + }; + + Motor_Config_t flywheel_right_config = { + .can_bus = 1, + .speed_controller_id = 5, + .offset = 0, + .control_mode = VELOCITY_CONTROL, + .motor_reversal = MOTOR_REVERSAL_REVERSED, + .velocity_pid = + { + .kp = 500.0f, + .output_limit = M3508_MAX_CURRENT, + }, + }; + + Motor_Config_t feed_speed_config = { + .can_bus = 1, + .speed_controller_id = 2, + .offset = 0, + .control_mode = VELOCITY_CONTROL | POSITION_CONTROL_TOTAL_ANGLE, + .motor_reversal = MOTOR_REVERSAL_NORMAL, + .velocity_pid = + { + .kp = 500.0f, + .kd = 200.0f, + .kf = 100.0f, + .output_limit = M2006_MAX_CURRENT, + }, + .angle_pid = + { + .kp = 450000.0f, + .kd = 15000000.0f, + .ki = 0.1f, + .output_limit = M2006_MAX_CURRENT, + .integral_limit = 1000.0f, + } + }; + + g_flywheel_left = DJI_Motor_Init(&flywheel_left_config,M3508); + g_flywheel_right = DJI_Motor_Init(&flywheel_right_config,M3508); + g_feed_motor = DJI_Motor_Init(&feed_speed_config,M2006); + + Laser_Init(); +} + + +void Launch_Ctrl_Loop() { + if (!g_robot_state.launch.IS_FIRING_ENABLED){ + stopFlywheel(); + Laser_Off(); + g_robot_state.launch.IS_FLYWHEEL_ENABLED = 0; + return; + } else { + g_robot_state.launch.IS_FLYWHEEL_ENABLED = 1; + startFlywheel(); + Laser_On(); + } + + if (g_robot_state.launch.IS_BUSY) { // check if we are in middle of a fire mode + switch (g_robot_state.launch.busy_mode) { + case REJIGGLE: + rejiggle(); + break; + case SINGLE_FIRE: + handleSingleFire(); + break; + case FULL_AUTO: + // NOT USED + break; + default: + break; + } + } else { + // Control loop for launch to see if new mode is set + switch (g_robot_state.launch.fire_mode) { + case SINGLE_FIRE: + handleSingleFire(); + break; + case FULL_AUTO: + // NOT USED + break; + default: + break; + } + } +} + +float g_curr_angle = 0; +// TODO check if at ref +void handleSingleFire() { + if (g_robot_state.launch.IS_BUSY) { + // if shots fired :O then rejiggle + if (DJI_Motor_Is_At_Angle(g_feed_motor, FEED_TOLERANCE)) { + g_robot_state.launch.IS_BUSY = 0; + g_robot_state.launch.busy_mode = IDLE; + rejiggle(); + } + } else { + g_robot_state.launch.IS_BUSY = 1; + g_robot_state.launch.busy_mode = SINGLE_FIRE; + + DJI_Motor_Set_Control_Mode(g_feed_motor, POSITION_CONTROL_TOTAL_ANGLE); + g_curr_angle = DJI_Motor_Get_Total_Angle(g_feed_motor); // rad + DJI_Motor_Set_Angle(g_feed_motor, g_curr_angle + SHOT_ANGLE_OFFSET_RAD); + } +} + +void rejiggle() { + //set a position reference slightly back to prevent jams + float curr_angle_rad = DJI_Motor_Get_Total_Angle(g_feed_motor); // rad + + if (g_robot_state.launch.IS_BUSY) { // if busy, means we already called so go back + if (DJI_Motor_Is_At_Angle(g_feed_motor, FEED_TOLERANCE)){ + g_robot_state.launch.busy_mode = IDLE; + g_robot_state.launch.IS_BUSY = 0; + DJI_Motor_Set_Control_Mode(g_feed_motor, POSITION_CONTROL_TOTAL_ANGLE); + DJI_Motor_Set_Angle(g_feed_motor, curr_angle_rad + SHOT_ANGLE_OFFSET_RAD); + } + } else { + g_robot_state.launch.busy_mode = REJIGGLE; + g_robot_state.launch.IS_BUSY = 1; + DJI_Motor_Set_Control_Mode(g_feed_motor, POSITION_CONTROL_TOTAL_ANGLE); + DJI_Motor_Set_Angle(g_feed_motor, curr_angle_rad - SHOT_ANGLE_OFFSET_RAD); + } +} + +void startFlywheel() { + DJI_Motor_Set_Velocity(g_flywheel_left, 100); + DJI_Motor_Set_Velocity(g_flywheel_right, 100); } -void Launch_Ctrl_Loop() -{ - // Control loop for launch +void stopFlywheel() { + DJI_Motor_Set_Velocity(g_flywheel_left, 0); + DJI_Motor_Set_Velocity(g_flywheel_right, 0); } diff --git a/Hero/src/robot.c b/Hero/src/robot.c index f7296a0..d6f1102 100644 --- a/Hero/src/robot.c +++ b/Hero/src/robot.c @@ -16,6 +16,8 @@ Robot_State_t g_robot_state = {0}; extern Remote_t g_remote; extern Supercap_t g_supercap; +Input_State_t g_input_state = {0}; + /** * @brief This function initializes the robot. * This means setting the state to STARTING_UP, @@ -98,16 +100,51 @@ void Handle_Disabled_State() void Process_Remote_Input() { - g_robot_state.input.vx = g_remote.controller.left_stick.x; - g_robot_state.input.vy = g_remote.controller.left_stick.y; + // Process remote input + g_robot_state.input.vy_keyboard = ((1.0f - KEYBOARD_RAMP_COEF) * g_robot_state.input.vy_keyboard + g_remote.keyboard.W * KEYBOARD_RAMP_COEF - g_remote.keyboard.S * KEYBOARD_RAMP_COEF); + g_robot_state.input.vx_keyboard = ((1.0f - KEYBOARD_RAMP_COEF) * g_robot_state.input.vx_keyboard - g_remote.keyboard.A * KEYBOARD_RAMP_COEF + g_remote.keyboard.D * KEYBOARD_RAMP_COEF); + float temp_x = g_robot_state.input.vx_keyboard + g_remote.controller.left_stick.x / REMOTE_STICK_MAX; + float temp_y = g_robot_state.input.vy_keyboard + g_remote.controller.left_stick.y / REMOTE_STICK_MAX; + g_robot_state.input.vx = temp_x; + g_robot_state.input.vy = temp_y; g_robot_state.input.vomega = g_remote.controller.right_stick.x; - - if (__IS_TRANSITIONED(g_remote.controller.left_switch, g_robot_state.input.prev_left_switch, MID)) + + + // Calculate Gimbal Oriented Control + + // TODO: Add back in + // float theta = DJI_Motor_Get_Absolute_Angle(g_yaw); + // g_robot_state.chassis.x_speed = -g_robot_state.input.vy * sin(theta) + g_robot_state.input.vx * cos(theta); + // g_robot_state.chassis.y_speed = g_robot_state.input.vy * cos(theta) + g_robot_state.input.vx * sin(theta); + + // g_robot_state.gimbal.yaw_angle -= (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 10000.0f); // controller and mouse + // g_robot_state.gimbal.pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); + + // keyboard toggles + if (__IS_TOGGLED(g_remote.keyboard.B, g_input_state.prev_B)) + { + g_robot_state.launch.IS_FIRING_ENABLED ^= 0x01; // Toggle firing + } + if (__IS_TOGGLED(g_remote.keyboard.B, g_input_state.prev_B)) + { + g_robot_state.chassis.IS_SPINTOP_ENABLED ^= 0x01; // Toggle spintop + } + if (__IS_TOGGLED(g_remote.keyboard.B, g_input_state.prev_B)) + { + g_robot_state.UI_ENABLED ^= 0x01; // Toggle UI + } + if (__IS_TOGGLED(g_remote.keyboard.Shift, g_input_state.prev_Shift)) + { + g_robot_state.IS_SUPER_CAPACITOR_ENABLED ^= 0x01; // Toggle supercap + } + + // controller toggles + if (__IS_TRANSITIONED(g_remote.controller.left_switch, g_input_state.prev_left_switch, MID)) { g_robot_state.chassis.IS_SPINTOP_ENABLED = 1; } - if (__IS_TRANSITIONED(g_remote.controller.left_switch, g_robot_state.input.prev_left_switch, DOWN) || - __IS_TRANSITIONED(g_remote.controller.left_switch, g_robot_state.input.prev_left_switch, UP)) + if (__IS_TRANSITIONED(g_remote.controller.left_switch, g_input_state.prev_left_switch, DOWN) || + __IS_TRANSITIONED(g_remote.controller.left_switch, g_input_state.prev_left_switch, UP)) { g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; } @@ -117,18 +154,17 @@ void Process_Remote_Input() void Process_Chassis_Control() { - // USER CODE HERE Chassis_Ctrl_Loop(); } void Process_Gimbal_Control() { - // USER CODE HERE + Gimbal_Ctrl_Loop(); } void Process_Launch_Control() { - // USER CODE HERE + Launch_Ctrl_Loop(); } /** diff --git a/control-base/algo/inc/user_math.h b/control-base/algo/inc/user_math.h index 1ac4a56..af6fedf 100644 --- a/control-base/algo/inc/user_math.h +++ b/control-base/algo/inc/user_math.h @@ -2,6 +2,7 @@ #define USER_MATH_H #define PI (3.1415926f) +#define TAU (6.2831852f) #define PI_OVER_2 (PI / 2.0f) #define __MAX_LIMIT(val, min, max) \ From a14aa7079838ab7e913f6534a4b03f47f4e62edf Mon Sep 17 00:00:00 2001 From: nicolasshul <74377515+nicolasshul@users.noreply.github.com> Date: Sat, 29 Mar 2025 17:43:29 -0400 Subject: [PATCH 04/13] added keyboard bidings for launch --- Swerve-Standard/src/robot.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Swerve-Standard/src/robot.c b/Swerve-Standard/src/robot.c index f317148..8306991 100644 --- a/Swerve-Standard/src/robot.c +++ b/Swerve-Standard/src/robot.c @@ -184,6 +184,13 @@ void Process_Remote_Input() g_robot_state.launch.fire_mode = NO_FIRE; } + if (__IS_TOGGLED(g_remote.keyboard.V, g_input_state.prev_V)) { // switch to burst later + g_robot_state.launch.fire_mode = SINGLE_FIRE; + } + if (__IS_TOGGLED(g_remote.keyboard.G, g_input_state.prev_G)) { + g_robot_state.launch.fire_mode = FULL_AUTO; + } + // cycle burst flags with keyboard // TODO: assign a key for this // if (__IS_TOGGLED(g_remote.keyboard.G, g_input_state.prev_G)) From 18d59898502bcb65d5714fda8e3d7db76bf773e1 Mon Sep 17 00:00:00 2001 From: Irving Wang Date: Sat, 29 Mar 2025 19:39:55 -0400 Subject: [PATCH 05/13] fix prelaunch task --- .vscode/launch.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 3a22f26..f6a7ad5 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -19,7 +19,7 @@ "enabled": true, "samplesPerSecond": 4 }, - "preLaunchTask": "Build Robot" // build before debug + "preLaunchTask": "Build" // build before debug }, { "name": "ST-LINK", @@ -39,7 +39,7 @@ "enabled": true, "samplesPerSecond": 8 }, - "preLaunchTask": "Build Robot" // build before debug + "preLaunchTask": "Build" // build before debug } ], "inputs": [ From 4bdaefe1336a9b46038c4d373b4be768e496db37 Mon Sep 17 00:00:00 2001 From: Irving Wang Date: Fri, 4 Apr 2025 17:50:07 -0400 Subject: [PATCH 06/13] tested chassis --- Hero/inc/chassis_task.h | 4 ++-- Hero/src/chassis_task.c | 16 +++++++++------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/Hero/inc/chassis_task.h b/Hero/inc/chassis_task.h index e021bf0..59827f3 100644 --- a/Hero/inc/chassis_task.h +++ b/Hero/inc/chassis_task.h @@ -2,12 +2,12 @@ #define CHASSIS_TASK_H #define SPIN_TOP_OMEGA 2 // rad / s -#define MAX_ANGLUAR_SPEED 3.14 // rad / s +#define MAX_ANGLUAR_SPEED 6.28 // rad / s #define CHASSIS_WHEEL_DIAMETER (0.15f) // m #define CHASSIS_RADIUS (0.305f) // center to wheel, m #define CHASSIS_MAX_SPEED (2.0f) // m/s #define CHASSIS_MOUNTING_ANGLE (PI / 4) // rad (45deg) -#define MAX_ABC (200.0f) // rad/s // TODO rename this macro +#define MAX_ABC (400.0f) // TODO rename this macro and add units // Function prototypes void Chassis_Task_Init(void); diff --git a/Hero/src/chassis_task.c b/Hero/src/chassis_task.c index 4a6127e..86bf054 100644 --- a/Hero/src/chassis_task.c +++ b/Hero/src/chassis_task.c @@ -28,14 +28,16 @@ void Chassis_Task_Init() { // Init chassis hardware Motor_Config_t drive_motor_config = { - .can_bus = 1, + .can_bus = 2, .control_mode = VELOCITY_CONTROL, - .velocity_pid = { - .kp = 500.0f, - .kf = 100.0f, - .output_limit = M3508_MAX_CURRENT, - .integral_limit = 3000.0f, - }}; + .velocity_pid = + { + .kp = 500.0f, + .kf = 100.0f, + .output_limit = M3508_MAX_CURRENT, + .integral_limit = 3000.0f, + }, + }; for (int i = 0; i < 4; i++) { // configure drive motor From 8f6df93fb6ddfe0e6342f6491d1a49a1b830901b Mon Sep 17 00:00:00 2001 From: ThomasP850 Date: Fri, 4 Apr 2025 19:59:20 -0400 Subject: [PATCH 07/13] Hero Gimbal changes --- .vscode/tasks.json | 5 +- Hero/inc/gimbal_task.h | 30 ++++++++++-- Hero/src/gimbal_task.c | 72 +++++++++++++++++++++++++++-- Hero/src/launch_task.c | 10 ++-- Hero/src/motor_task.c | 2 +- Hero/src/robot.c | 58 +++++++++++++++++++---- Makefile | 2 +- Swerve-Standard/src/motor_task.c | 2 +- Swerve-Standard/src/robot.c | 2 +- control-base/devices/inc/dm_motor.h | 2 + control-base/devices/src/dm_motor.c | 18 ++++++++ 11 files changed, 178 insertions(+), 25 deletions(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 572e521..203f367 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -4,7 +4,10 @@ { "label": "Clear Terminal", "type": "shell", - "command": "clear", + "command": "clear", // Default for non-Windows + "windows": { + "command": "cls" // Specific for Windows + }, "presentation": { "reveal": "never", "clear": true diff --git a/Hero/inc/gimbal_task.h b/Hero/inc/gimbal_task.h index 79220fe..d426d18 100644 --- a/Hero/inc/gimbal_task.h +++ b/Hero/inc/gimbal_task.h @@ -1,18 +1,40 @@ #ifndef GIMBAL_TASK_H #define GIMBAL_TASK_H -#define YAW_MID_POSITION -#define PITCH_MID_POSITION +// TODO: Find correct values + +// Note: these values are in degrees so that they are human-readable + +#define PITCH_LOWER_LIMIT (-0.8f) +#define PITCH_UPPER_LIMIT (0.7f) + +#define PITCH_OFFSET (0.06f) +#define YAW_OFFSET (0.0f) + +#define PITCH_CONTROLLER_VELOCITY_COEF (4e-5f) +#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 (16.0f) +#define YAW_RATE_LIMIT (1.0f) typedef struct { - float pitch; + float pitch_velocity; + float pitch_angle; float yaw_velocity; float yaw_angle; } Gimbal_Target_t; + // Function prototypes void Gimbal_Task_Init(void); void Gimbal_Ctrl_Loop(void); -#endif // GIMBAL_TASK_H +#endif // GIMBAL_TASK_H \ No newline at end of file diff --git a/Hero/src/gimbal_task.c b/Hero/src/gimbal_task.c index 4bebfff..387de36 100644 --- a/Hero/src/gimbal_task.c +++ b/Hero/src/gimbal_task.c @@ -1,18 +1,84 @@ #include "gimbal_task.h" +#include "dji_motor.h" +#include "dm_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; + +DM_Motor_Handle_t* g_pitch_motor; +// 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() { - // Init Gimbal Hardware + // TODO: Work with gear ratios + + + DM_Motor_Config_t pitch_motor_config = { + .can_bus = 1, + .tx_id = 0x01, + .rx_id = 0x11, + .disable_behavior = DM_MOTOR_ZERO_CURRENT, + .pos_offset = PITCH_OFFSET, + .kp = 15.0f, + .kd = 2.0f + }; + + g_pitch_motor = DM_Motor_Init(&pitch_motor_config); + // DM_Motor_Enable_Motor(g_pitch_motor); + + Motor_Config_t yaw_motor_config = { + .can_bus = 2, + .speed_controller_id = 3, + .offset = YAW_OFFSET, + .control_mode = POSITION_CONTROL, + .motor_reversal = MOTOR_REVERSAL_REVERSED, + .angle_pid = { + .kp = 1000.0f, + .ki = 0.0f, + .kd = 100.0f, + .output_limit = GM6020_MAX_CURRENT, + } + }; + + g_yaw_motor = DJI_Motor_Init(&yaw_motor_config, GM6020); + + rate_limiter_init(&g_yaw_rate_limiter, YAW_RATE_LIMIT); + rate_limiter_init(&g_pitch_rate_limiter, PITCH_RATE_LIMIT); + } void Gimbal_Ctrl_Loop() { - // Control loop for gimbal -} + + 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, TAU); + __MAX_LIMIT(g_robot_state.gimbal.pitch_angle, PITCH_LOWER_LIMIT, PITCH_UPPER_LIMIT); + + g_robot_state.gimbal.yaw_angle = rate_limiter_iterate(&g_yaw_rate_limiter, g_robot_state.gimbal.yaw_angle); + g_robot_state.gimbal.pitch_angle = rate_limiter_iterate(&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); + DM_Motor_Ctrl_MIT(g_pitch_motor, g_robot_state.gimbal.pitch_angle, 0.0f, 0.0f); +} \ No newline at end of file diff --git a/Hero/src/launch_task.c b/Hero/src/launch_task.c index 5c00a74..ce5d331 100644 --- a/Hero/src/launch_task.c +++ b/Hero/src/launch_task.c @@ -21,7 +21,7 @@ void Launch_Task_Init() { // Init Launch Hardware Motor_Config_t flywheel_left_config = { .can_bus = 1, - .speed_controller_id = 4, + .speed_controller_id = 5, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_NORMAL, @@ -34,7 +34,7 @@ void Launch_Task_Init() { Motor_Config_t flywheel_right_config = { .can_bus = 1, - .speed_controller_id = 5, + .speed_controller_id = 4, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_REVERSED, @@ -68,9 +68,9 @@ void Launch_Task_Init() { } }; - g_flywheel_left = DJI_Motor_Init(&flywheel_left_config,M3508); - g_flywheel_right = DJI_Motor_Init(&flywheel_right_config,M3508); - g_feed_motor = DJI_Motor_Init(&feed_speed_config,M2006); + g_flywheel_left = DJI_Motor_Init(&flywheel_left_config, M3508); + g_flywheel_right = DJI_Motor_Init(&flywheel_right_config, M3508); + g_feed_motor = DJI_Motor_Init(&feed_speed_config, M2006); Laser_Init(); } diff --git a/Hero/src/motor_task.c b/Hero/src/motor_task.c index 83e1a7f..d35ab37 100644 --- a/Hero/src/motor_task.c +++ b/Hero/src/motor_task.c @@ -9,7 +9,7 @@ extern Supercap_t g_supercap; void Motor_Task_Loop() { DJI_Motor_Send(); // MF_Motor_Send(); - // DM_Motor_Send(); + DM_Motor_Send(); g_supercap.send_counter++; if (g_supercap.send_counter >= 100) { diff --git a/Hero/src/robot.c b/Hero/src/robot.c index d6f1102..3f5fb73 100644 --- a/Hero/src/robot.c +++ b/Hero/src/robot.c @@ -11,10 +11,12 @@ #include "buzzer.h" #include "supercap.h" #include "dji_motor.h" +#include "dm_motor.h" Robot_State_t g_robot_state = {0}; extern Remote_t g_remote; extern Supercap_t g_supercap; +extern DJI_Motor_Handle_t* g_yaw_motor; Input_State_t g_input_state = {0}; @@ -75,7 +77,7 @@ void Handle_Enabled_State() Process_Remote_Input(); Process_Chassis_Control(); Process_Gimbal_Control(); - Process_Launch_Control(); + // Process_Launch_Control(); } } @@ -86,6 +88,7 @@ void Handle_Enabled_State() void Handle_Disabled_State() { DJI_Motor_Disable_All(); + DM_Motor_Disable_All(); // Disable all major components g_robot_state.launch.IS_FLYWHEEL_ENABLED = 0; g_robot_state.chassis.x_speed = 0; @@ -95,6 +98,7 @@ void Handle_Disabled_State() { g_robot_state.state = ENABLED; DJI_Motor_Enable_All(); + DM_Motor_Enable_All(); } } @@ -107,18 +111,25 @@ void Process_Remote_Input() float temp_y = g_robot_state.input.vy_keyboard + g_remote.controller.left_stick.y / REMOTE_STICK_MAX; g_robot_state.input.vx = temp_x; g_robot_state.input.vy = temp_y; - g_robot_state.input.vomega = g_remote.controller.right_stick.x; + // g_robot_state.input.vomega = g_remote.controller.right_stick.x; // Calculate Gimbal Oriented Control // TODO: Add back in - // float theta = DJI_Motor_Get_Absolute_Angle(g_yaw); - // g_robot_state.chassis.x_speed = -g_robot_state.input.vy * sin(theta) + g_robot_state.input.vx * cos(theta); - // g_robot_state.chassis.y_speed = g_robot_state.input.vy * cos(theta) + g_robot_state.input.vx * sin(theta); - - // g_robot_state.gimbal.yaw_angle -= (g_remote.controller.right_stick.x / 50000.0f + g_remote.mouse.x / 10000.0f); // controller and mouse - // g_robot_state.gimbal.pitch_angle -= (g_remote.controller.right_stick.y / 100000.0f - g_remote.mouse.y / 50000.0f); + float theta = DJI_Motor_Get_Absolute_Angle(g_yaw_motor); + g_robot_state.chassis.x_speed = -g_robot_state.input.vy * sin(theta) + g_robot_state.input.vx * cos(theta); + g_robot_state.chassis.y_speed = g_robot_state.input.vy * cos(theta) + g_robot_state.input.vx * sin(theta); + + // 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 + ); // keyboard toggles if (__IS_TOGGLED(g_remote.keyboard.B, g_input_state.prev_B)) @@ -149,6 +160,37 @@ void Process_Remote_Input() g_robot_state.chassis.IS_SPINTOP_ENABLED = 0; } + if (g_remote.controller.left_switch == UP) + { + g_robot_state.launch.IS_FIRING_ENABLED = 1; + } + else + { + g_robot_state.launch.IS_FIRING_ENABLED = 0; + } + + if ((g_remote.controller.right_switch == UP) || (g_remote.mouse.right == 1)) // mouse right button auto aim + { + g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 1; + } + else + { + g_robot_state.launch.IS_AUTO_AIMING_ENABLED = 0; + } + + if (g_remote.controller.wheel < -50.0f) + { // dial wheel forward single fire + g_robot_state.launch.fire_mode = SINGLE_FIRE; + } + else if (g_remote.controller.wheel > 50.0f) + { // dial wheel backward burst `fire + g_robot_state.launch.fire_mode = FULL_AUTO; + } + else + { // dial wheel mid stop fire + g_robot_state.launch.fire_mode = NO_FIRE; + } + g_robot_state.input.prev_left_switch = g_remote.controller.left_switch; } diff --git a/Makefile b/Makefile index 61e69e8..8981fa9 100644 --- a/Makefile +++ b/Makefile @@ -189,7 +189,7 @@ $(BUILD_DIR)/$(TARGET).bin: $(BUILD_DIR)/$(TARGET).elf | $(BUILD_DIR) @echo "${COLOR_CYAN}Generated binary: $@${COLOR_RESET}" $(BUILD_DIR): - @mkdir -p $@ + @if not exist "$(BUILD_DIR)" mkdir "$(BUILD_DIR)" # Clean build files clean: diff --git a/Swerve-Standard/src/motor_task.c b/Swerve-Standard/src/motor_task.c index a3835b9..75e710a 100644 --- a/Swerve-Standard/src/motor_task.c +++ b/Swerve-Standard/src/motor_task.c @@ -9,7 +9,7 @@ extern Supercap_t g_supercap; void Motor_Task_Loop() { DJI_Motor_Send(); // MF_Motor_Send(); - // DM_Motor_Send(); + DM_Motor_Send(); Supercap_Send(); } diff --git a/Swerve-Standard/src/robot.c b/Swerve-Standard/src/robot.c index 8306991..c837cc5 100644 --- a/Swerve-Standard/src/robot.c +++ b/Swerve-Standard/src/robot.c @@ -56,7 +56,7 @@ void Handle_Starting_Up_State() Supercap_Init(&g_supercap); Chassis_Task_Init(); Gimbal_Task_Init(); - Launch_Task_Init(); + // Launch_Task_Init(); Jetson_Orin_Init(&huart1); // ! temp uart1 Remote_Init(&huart3); diff --git a/control-base/devices/inc/dm_motor.h b/control-base/devices/inc/dm_motor.h index 7fc989b..812f3e8 100644 --- a/control-base/devices/inc/dm_motor.h +++ b/control-base/devices/inc/dm_motor.h @@ -78,6 +78,8 @@ typedef struct _DM_Motor { void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor); void DM_Motor_Enable_Motor(DM_Motor_Handle_t *motor); +void DM_Motor_Disable_All(void); +void DM_Motor_Enable_All(void); DM_Motor_Handle_t* DM_Motor_Init(DM_Motor_Config_t *config); void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq); void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd); diff --git a/control-base/devices/src/dm_motor.c b/control-base/devices/src/dm_motor.c index 53cdca2..c107126 100644 --- a/control-base/devices/src/dm_motor.c +++ b/control-base/devices/src/dm_motor.c @@ -114,6 +114,24 @@ void DM_Motor_Disable_Motor(DM_Motor_Handle_t *motor) motor->send_pending_flag = 1; } +void DM_Motor_Enable_All() +{ + for (int i = 0; i < g_dm_motor_num; i++) + { + // set the motor state to enabled + DM_Motor_Enable_Motor(g_dm_motors[i]); + } +} + +void DM_Motor_Disable_All() +{ + for (int i = 0; i < g_dm_motor_num; i++) + { + // set the motor state to enabled + DM_Motor_Disable_Motor(g_dm_motors[i]); + } +} + void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq) { uint16_t pos_temp, vel_temp, kp_temp, kd_temp, torq_temp; From c455eae272aaf0769fd21c2c5df31fa87aee938a Mon Sep 17 00:00:00 2001 From: ThomasP850 Date: Sat, 5 Apr 2025 01:32:43 -0400 Subject: [PATCH 08/13] gimble yaw --- Hero/inc/chassis_task.h | 2 +- Hero/inc/gimbal_task.h | 4 ++-- Hero/src/gimbal_task.c | 49 +++++++++++++++++++++++++++++------------ 3 files changed, 38 insertions(+), 17 deletions(-) diff --git a/Hero/inc/chassis_task.h b/Hero/inc/chassis_task.h index 59827f3..6ee9505 100644 --- a/Hero/inc/chassis_task.h +++ b/Hero/inc/chassis_task.h @@ -1,7 +1,7 @@ #ifndef CHASSIS_TASK_H #define CHASSIS_TASK_H -#define SPIN_TOP_OMEGA 2 // rad / s +#define SPIN_TOP_OMEGA 4 // rad / s #define MAX_ANGLUAR_SPEED 6.28 // rad / s #define CHASSIS_WHEEL_DIAMETER (0.15f) // m #define CHASSIS_RADIUS (0.305f) // center to wheel, m diff --git a/Hero/inc/gimbal_task.h b/Hero/inc/gimbal_task.h index d426d18..35e953b 100644 --- a/Hero/inc/gimbal_task.h +++ b/Hero/inc/gimbal_task.h @@ -12,7 +12,7 @@ #define YAW_OFFSET (0.0f) #define PITCH_CONTROLLER_VELOCITY_COEF (4e-5f) -#define YAW_CONTORLLER_VELOCITY_COEF (5e-5f) +#define YAW_CONTORLLER_VELOCITY_COEF (5e-6f) #define PITCH_MOUSE_VELOCITY_COEF (5e-5f) #define YAW_MOUSE_VELOCITY_COEF (1e-5f) @@ -22,7 +22,7 @@ // Degrees per second #define PITCH_RATE_LIMIT (16.0f) -#define YAW_RATE_LIMIT (1.0f) +#define YAW_RATE_LIMIT (16.0f) typedef struct { diff --git a/Hero/src/gimbal_task.c b/Hero/src/gimbal_task.c index 387de36..73f7381 100644 --- a/Hero/src/gimbal_task.c +++ b/Hero/src/gimbal_task.c @@ -18,7 +18,7 @@ extern Jetson_Orin_Data_t g_orin_data; DM_Motor_Handle_t* g_pitch_motor; // 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_yaw_rate_limiter; rate_limiter_t g_pitch_rate_limiter; void Gimbal_Task_Init() @@ -32,8 +32,9 @@ void Gimbal_Task_Init() .rx_id = 0x11, .disable_behavior = DM_MOTOR_ZERO_CURRENT, .pos_offset = PITCH_OFFSET, + // .kp = 130.0f, .kp = 15.0f, - .kd = 2.0f + .kd = 1.0f }; g_pitch_motor = DM_Motor_Init(&pitch_motor_config); @@ -43,19 +44,38 @@ void Gimbal_Task_Init() .can_bus = 2, .speed_controller_id = 3, .offset = YAW_OFFSET, - .control_mode = POSITION_CONTROL, + .control_mode = POSITION_VELOCITY_SERIES, + .use_external_feedback = 1, + .external_feedback_dir = 1, + .external_angle_feedback_ptr = &g_imu.rad.yaw, + .external_velocity_feedback_ptr = &(g_imu.bmi088_raw.gyro[2]), .motor_reversal = MOTOR_REVERSAL_REVERSED, - .angle_pid = { - .kp = 1000.0f, - .ki = 0.0f, - .kd = 100.0f, - .output_limit = GM6020_MAX_CURRENT, - } + .angle_pid = + { + .kp = 300.0f, + .kd = 30.0f, + .output_limit = GM6020_MAX_CURRENT, + }, + .velocity_pid = + { + .kp = 100.0f, + .ki = 0.0f, + .kf = 100.0f, + .feedforward_limit = 5000.0f, + .integral_limit = 1000.0f, + .output_limit = GM6020_MAX_CURRENT, + }, + // .angle_pid = { + // .kp = 100000.0f, + // .ki = 0.0f, + // .kd = 10000.0f, + // .output_limit = GM6020_MAX_CURRENT, + // } }; g_yaw_motor = DJI_Motor_Init(&yaw_motor_config, GM6020); - rate_limiter_init(&g_yaw_rate_limiter, YAW_RATE_LIMIT); + // rate_limiter_init(&g_yaw_rate_limiter, YAW_RATE_LIMIT); rate_limiter_init(&g_pitch_rate_limiter, PITCH_RATE_LIMIT); } @@ -71,13 +91,14 @@ void Gimbal_Ctrl_Loop() } } - g_robot_state.gimbal.yaw_angle = fmod(g_robot_state.gimbal.yaw_angle, TAU); + // g_robot_state.gimbal.yaw_angle = fmod(g_robot_state.gimbal.yaw_angle, TAU); __MAX_LIMIT(g_robot_state.gimbal.pitch_angle, PITCH_LOWER_LIMIT, PITCH_UPPER_LIMIT); - g_robot_state.gimbal.yaw_angle = rate_limiter_iterate(&g_yaw_rate_limiter, g_robot_state.gimbal.yaw_angle); - g_robot_state.gimbal.pitch_angle = rate_limiter_iterate(&g_pitch_rate_limiter, g_robot_state.gimbal.pitch_angle); + // g_robot_state.gimbal.yaw_angle = rate_limiter_iterate(&g_yaw_rate_limiter, g_robot_state.gimbal.yaw_angle); + g_robot_state.gimbal.pitch_angle = rate_limiter_iterate(&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_yaw_motor, g_robot_state.gimbal.yaw_angle); // DJI_Motor_Set_Angle(g_pitch_motor, g_robot_state.gimbal.pitch_angle); DM_Motor_Ctrl_MIT(g_pitch_motor, g_robot_state.gimbal.pitch_angle, 0.0f, 0.0f); From d18046bf1b76a8458c3c45fd3a27e5005b18e348 Mon Sep 17 00:00:00 2001 From: ThomasP850 Date: Fri, 11 Apr 2025 19:22:46 -0400 Subject: [PATCH 09/13] Gimbal-oriented control --- Hero/src/gimbal_task.c | 4 ++-- Hero/src/robot.c | 7 +++---- control-base/devices/inc/dm_motor.h | 1 + control-base/devices/src/dm_motor.c | 5 +++++ 4 files changed, 11 insertions(+), 6 deletions(-) diff --git a/Hero/src/gimbal_task.c b/Hero/src/gimbal_task.c index 73f7381..0db5d25 100644 --- a/Hero/src/gimbal_task.c +++ b/Hero/src/gimbal_task.c @@ -94,11 +94,11 @@ void Gimbal_Ctrl_Loop() // g_robot_state.gimbal.yaw_angle = fmod(g_robot_state.gimbal.yaw_angle, TAU); __MAX_LIMIT(g_robot_state.gimbal.pitch_angle, PITCH_LOWER_LIMIT, PITCH_UPPER_LIMIT); - // g_robot_state.gimbal.yaw_angle = rate_limiter_iterate(&g_yaw_rate_limiter, g_robot_state.gimbal.yaw_angle); + // g_robot_state.gimbal.yaw_angle = rate_limiter_iterate(&g_yaw_raste_limiter, g_robot_state.gimbal.yaw_angle); g_robot_state.gimbal.pitch_angle = rate_limiter_iterate(&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_yaw_motor, g_robot_state.gimbal.yaw_angle); // DJI_Motor_Set_Angle(g_pitch_motor, g_robot_state.gimbal.pitch_angle); DM_Motor_Ctrl_MIT(g_pitch_motor, g_robot_state.gimbal.pitch_angle, 0.0f, 0.0f); diff --git a/Hero/src/robot.c b/Hero/src/robot.c index 3f5fb73..c322408 100644 --- a/Hero/src/robot.c +++ b/Hero/src/robot.c @@ -107,17 +107,16 @@ void Process_Remote_Input() // Process remote input g_robot_state.input.vy_keyboard = ((1.0f - KEYBOARD_RAMP_COEF) * g_robot_state.input.vy_keyboard + g_remote.keyboard.W * KEYBOARD_RAMP_COEF - g_remote.keyboard.S * KEYBOARD_RAMP_COEF); g_robot_state.input.vx_keyboard = ((1.0f - KEYBOARD_RAMP_COEF) * g_robot_state.input.vx_keyboard - g_remote.keyboard.A * KEYBOARD_RAMP_COEF + g_remote.keyboard.D * KEYBOARD_RAMP_COEF); + g_robot_state.input.vomega = g_remote.controller.right_stick.x; + float temp_x = g_robot_state.input.vx_keyboard + g_remote.controller.left_stick.x / REMOTE_STICK_MAX; float temp_y = g_robot_state.input.vy_keyboard + g_remote.controller.left_stick.y / REMOTE_STICK_MAX; g_robot_state.input.vx = temp_x; g_robot_state.input.vy = temp_y; - // g_robot_state.input.vomega = g_remote.controller.right_stick.x; // Calculate Gimbal Oriented Control - - // TODO: Add back in - float theta = DJI_Motor_Get_Absolute_Angle(g_yaw_motor); + float theta = DM_Motor_Get_Absolute_Angle(g_yaw_motor); g_robot_state.chassis.x_speed = -g_robot_state.input.vy * sin(theta) + g_robot_state.input.vx * cos(theta); g_robot_state.chassis.y_speed = g_robot_state.input.vy * cos(theta) + g_robot_state.input.vx * sin(theta); diff --git a/control-base/devices/inc/dm_motor.h b/control-base/devices/inc/dm_motor.h index 812f3e8..fc4b156 100644 --- a/control-base/devices/inc/dm_motor.h +++ b/control-base/devices/inc/dm_motor.h @@ -83,6 +83,7 @@ void DM_Motor_Enable_All(void); DM_Motor_Handle_t* DM_Motor_Init(DM_Motor_Config_t *config); void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq); void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd); +float DM_Motor_Get_Absolute_Angle(DM_Motor_Handle_t *motor); /** * Global function to send the motor control data diff --git a/control-base/devices/src/dm_motor.c b/control-base/devices/src/dm_motor.c index c107126..694a097 100644 --- a/control-base/devices/src/dm_motor.c +++ b/control-base/devices/src/dm_motor.c @@ -165,6 +165,11 @@ void DM_Motor_Set_MIT_PD(DM_Motor_Handle_t *motor, float kp, float kd) motor->kd = kd; } +float DM_Motor_Get_Absolute_Angle(DM_Motor_Handle_t *motor) { + return motor->stats->pos; +} + + DM_Motor_Handle_t *DM_Motor_Init(DM_Motor_Config_t *config) { DM_Motor_Handle_t *motor = malloc(sizeof(DM_Motor_Handle_t)); From c370dfd60be354cc91dd3c91e2d5db25384f2c89 Mon Sep 17 00:00:00 2001 From: ThomasP850 Date: Fri, 11 Apr 2025 19:30:08 -0400 Subject: [PATCH 10/13] Clean up gimbal code --- Hero/src/gimbal_task.c | 8 -------- 1 file changed, 8 deletions(-) diff --git a/Hero/src/gimbal_task.c b/Hero/src/gimbal_task.c index 0db5d25..6a2cb68 100644 --- a/Hero/src/gimbal_task.c +++ b/Hero/src/gimbal_task.c @@ -65,12 +65,6 @@ void Gimbal_Task_Init() .integral_limit = 1000.0f, .output_limit = GM6020_MAX_CURRENT, }, - // .angle_pid = { - // .kp = 100000.0f, - // .ki = 0.0f, - // .kd = 10000.0f, - // .output_limit = GM6020_MAX_CURRENT, - // } }; g_yaw_motor = DJI_Motor_Init(&yaw_motor_config, GM6020); @@ -91,7 +85,6 @@ void Gimbal_Ctrl_Loop() } } - // g_robot_state.gimbal.yaw_angle = fmod(g_robot_state.gimbal.yaw_angle, TAU); __MAX_LIMIT(g_robot_state.gimbal.pitch_angle, PITCH_LOWER_LIMIT, PITCH_UPPER_LIMIT); // g_robot_state.gimbal.yaw_angle = rate_limiter_iterate(&g_yaw_raste_limiter, g_robot_state.gimbal.yaw_angle); @@ -100,6 +93,5 @@ void Gimbal_Ctrl_Loop() // 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); DM_Motor_Ctrl_MIT(g_pitch_motor, g_robot_state.gimbal.pitch_angle, 0.0f, 0.0f); } \ No newline at end of file From 307e5e1edb08cb7e9f81f6767729a1aa0dd959ba Mon Sep 17 00:00:00 2001 From: ThomasP850 Date: Sat, 19 Apr 2025 14:54:12 -0400 Subject: [PATCH 11/13] Gimbal bug fixes --- Hero/src/robot.c | 2 +- Swerve-Standard/src/motor_task.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Hero/src/robot.c b/Hero/src/robot.c index c322408..7ee55a0 100644 --- a/Hero/src/robot.c +++ b/Hero/src/robot.c @@ -116,7 +116,7 @@ void Process_Remote_Input() // Calculate Gimbal Oriented Control - float theta = DM_Motor_Get_Absolute_Angle(g_yaw_motor); + float theta = DJI_Motor_Get_Absolute_Angle(g_yaw_motor); g_robot_state.chassis.x_speed = -g_robot_state.input.vy * sin(theta) + g_robot_state.input.vx * cos(theta); g_robot_state.chassis.y_speed = g_robot_state.input.vy * cos(theta) + g_robot_state.input.vx * sin(theta); diff --git a/Swerve-Standard/src/motor_task.c b/Swerve-Standard/src/motor_task.c index 75e710a..0d7b9b1 100644 --- a/Swerve-Standard/src/motor_task.c +++ b/Swerve-Standard/src/motor_task.c @@ -1,6 +1,6 @@ #include "motor_task.h" #include "dji_motor.h" -// #include "dm_motor.h" +#include "dm_motor.h" // #include "mf_motor.h" #include "supercap.h" From cf5aef03dfdf4fb5e055acaebc962bf2f4941498 Mon Sep 17 00:00:00 2001 From: nicolasshul <74377515+nicolasshul@users.noreply.github.com> Date: Mon, 23 Jun 2025 22:11:15 -0700 Subject: [PATCH 12/13] fixed duplicate definition --- control-base/devices/src/dm_motor.c | 9 --------- 1 file changed, 9 deletions(-) diff --git a/control-base/devices/src/dm_motor.c b/control-base/devices/src/dm_motor.c index 2f37308..120e978 100644 --- a/control-base/devices/src/dm_motor.c +++ b/control-base/devices/src/dm_motor.c @@ -175,15 +175,6 @@ void DM_Motor_Enable_All() } } -void DM_Motor_Disable_All() -{ - for (int i = 0; i < g_dm_motor_num; i++) - { - // set the motor state to enabled - DM_Motor_Disable_Motor(g_dm_motors[i]); - } -} - void DM_Motor_Ctrl_MIT(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq) { uint16_t pos_temp, vel_temp, kp_temp, kd_temp, torq_temp; From 187efa8e292c270445233d0cbcf59aa0a79859f0 Mon Sep 17 00:00:00 2001 From: nicolasshul Date: Tue, 24 Jun 2025 13:46:55 -0700 Subject: [PATCH 13/13] changed cans --- Hero/src/chassis_task.c | 2 +- Hero/src/gimbal_task.c | 17 +++++++++++++++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/Hero/src/chassis_task.c b/Hero/src/chassis_task.c index 86bf054..4aa2e3f 100644 --- a/Hero/src/chassis_task.c +++ b/Hero/src/chassis_task.c @@ -28,7 +28,7 @@ void Chassis_Task_Init() { // Init chassis hardware Motor_Config_t drive_motor_config = { - .can_bus = 2, + .can_bus = 1, .control_mode = VELOCITY_CONTROL, .velocity_pid = { diff --git a/Hero/src/gimbal_task.c b/Hero/src/gimbal_task.c index 6a2cb68..f4bff5a 100644 --- a/Hero/src/gimbal_task.c +++ b/Hero/src/gimbal_task.c @@ -27,7 +27,7 @@ void Gimbal_Task_Init() DM_Motor_Config_t pitch_motor_config = { - .can_bus = 1, + .can_bus = 2, .tx_id = 0x01, .rx_id = 0x11, .disable_behavior = DM_MOTOR_ZERO_CURRENT, @@ -41,7 +41,7 @@ void Gimbal_Task_Init() // DM_Motor_Enable_Motor(g_pitch_motor); Motor_Config_t yaw_motor_config = { - .can_bus = 2, + .can_bus = 1, .speed_controller_id = 3, .offset = YAW_OFFSET, .control_mode = POSITION_VELOCITY_SERIES, @@ -67,6 +67,19 @@ void Gimbal_Task_Init() }, }; + // g_lower_yaw_motor = DJI_Motor_Init(&lower_yaw_motor_config, GM6020); + + // Motor_Config_t upper_yaw_motor_config = { + // .can_bus = 2, + // .tx_id = 0x01, + // .rx_id = 0x11, + // .disable_behavior = DM_MOTOR_ZERO_CURRENT, + // .pos_offset = PITCH_OFFSET, + // // .kp = 130.0f, + // .kp = 15.0f, + // .kd = 1.0f + // }; + g_yaw_motor = DJI_Motor_Init(&yaw_motor_config, GM6020); // rate_limiter_init(&g_yaw_rate_limiter, YAW_RATE_LIMIT);