Hero RMNA

.vscode/launch.json

@@ -50,7 +50,7 @@
             "options": [
                 "Swerve-Standard",
                 "Sentry",
-                "Wheel-Legged-Chassis",
+                "Hero",
                 //"Template"
                 // Add your other robot projects here
             ]

.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
             ]

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

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

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

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

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

Hero/inc/robot.h

@@ -0,0 +1,108 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#include "user_math.h"
+#include <stdint.h>
+
+#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

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);
+    }
+}