diff --git a/.vscode/launch.json b/.vscode/launch.json index 7a642c6..f6a7ad5 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -50,7 +50,7 @@ "options": [ "Swerve-Standard", "Sentry", - "Wheel-Legged-Chassis", + "Hero", //"Template" // Add your other robot projects here ] diff --git a/.vscode/tasks.json b/.vscode/tasks.json index b1fa1d4..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 @@ -22,7 +25,6 @@ "windows": { "command": "mingw32-make", "args": [ - "all_windows", "-j", "ROBOT_PROJECT=${input:robotProject}" ] @@ -119,7 +121,7 @@ "options": [ "Swerve-Standard", "Sentry", - "Wheel-Legged-Chassis", + "Hero" //"Template" // Add your other robot projects here ] diff --git a/Hero/inc/chassis_task.h b/Hero/inc/chassis_task.h new file mode 100644 index 0000000..6ee9505 --- /dev/null +++ b/Hero/inc/chassis_task.h @@ -0,0 +1,16 @@ +#ifndef CHASSIS_TASK_H +#define CHASSIS_TASK_H + +#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 +#define CHASSIS_MAX_SPEED (2.0f) // m/s +#define CHASSIS_MOUNTING_ANGLE (PI / 4) // rad (45deg) +#define MAX_ABC (400.0f) // TODO rename this macro and add units + +// 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..35e953b --- /dev/null +++ b/Hero/inc/gimbal_task.h @@ -0,0 +1,40 @@ +#ifndef GIMBAL_TASK_H +#define GIMBAL_TASK_H + +// 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-6f) + +#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 (16.0f) + +typedef struct +{ + 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 \ No newline at end of file diff --git a/Hero/inc/launch_task.h b/Hero/inc/launch_task.h new file mode 100644 index 0000000..ee36e75 --- /dev/null +++ b/Hero/inc/launch_task.h @@ -0,0 +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/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..3b93df9 --- /dev/null +++ b/Hero/inc/robot.h @@ -0,0 +1,108 @@ +#ifndef ROBOT_H +#define ROBOT_H + +#include "user_math.h" +#include + +#define KEYBOARD_RAMP_COEF (0.004f) + +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; + float vomega; + + // 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..4aa2e3f --- /dev/null +++ b/Hero/src/chassis_task.c @@ -0,0 +1,98 @@ +#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; + +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/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..f4bff5a --- /dev/null +++ b/Hero/src/gimbal_task.c @@ -0,0 +1,110 @@ +#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() +{ + // TODO: Work with gear ratios + + + DM_Motor_Config_t pitch_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_pitch_motor = DM_Motor_Init(&pitch_motor_config); + // DM_Motor_Enable_Motor(g_pitch_motor); + + Motor_Config_t yaw_motor_config = { + .can_bus = 1, + .speed_controller_id = 3, + .offset = YAW_OFFSET, + .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 = 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, + }, + }; + + // 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); + rate_limiter_init(&g_pitch_rate_limiter, PITCH_RATE_LIMIT); + +} + +void Gimbal_Ctrl_Loop() +{ + + 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; + } + } + + __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); + 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); + + 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 new file mode 100644 index 0000000..ce5d331 --- /dev/null +++ b/Hero/src/launch_task.c @@ -0,0 +1,167 @@ +#include "launch_task.h" + +#include "dji_motor.h" +#include "robot.h" +#include "remote.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; + +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 = 5, + .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 = 4, + .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 stopFlywheel() { + DJI_Motor_Set_Velocity(g_flywheel_left, 0); + DJI_Motor_Set_Velocity(g_flywheel_right, 0); +} diff --git a/Hero/src/motor_task.c b/Hero/src/motor_task.c new file mode 100644 index 0000000..d35ab37 --- /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..7ee55a0 --- /dev/null +++ b/Hero/src/robot.c @@ -0,0 +1,233 @@ +#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" +#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}; + +/** + * @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(); + DM_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(); + DM_Motor_Enable_All(); + } +} + +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; + + + // Calculate Gimbal Oriented Control + 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)) + { + 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_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; + } + + 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; +} + +void Process_Chassis_Control() +{ + Chassis_Ctrl_Loop(); +} + +void Process_Gimbal_Control() +{ + Gimbal_Ctrl_Loop(); +} + +void Process_Launch_Control() +{ + Launch_Ctrl_Loop(); +} + +/** + * 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 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/Swerve-Standard/src/motor_task.c b/Swerve-Standard/src/motor_task.c index a3835b9..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" @@ -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/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 } /** diff --git a/control-base/algo/inc/user_math.h b/control-base/algo/inc/user_math.h index 727d53d..6e4a82d 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) \ diff --git a/control-base/devices/inc/dm_motor.h b/control-base/devices/inc/dm_motor.h index d63c095..791223e 100644 --- a/control-base/devices/inc/dm_motor.h +++ b/control-base/devices/inc/dm_motor.h @@ -92,10 +92,13 @@ 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_Ctrl_MIT_PD(DM_Motor_Handle_t *motor, float target_pos, float target_vel, float torq, float kp, float kd); 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 6fa6d95..120e978 100644 --- a/control-base/devices/src/dm_motor.c +++ b/control-base/devices/src/dm_motor.c @@ -166,6 +166,15 @@ void DM_Motor_Disable_All() } } +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_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; @@ -234,6 +243,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));