diff --git a/.vscode/launch.json b/.vscode/launch.json index 802498d..1eea0c5 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,58 +1,73 @@ { - "version": "0.2.0", - "configurations": [ + "version": "0.2.0", + "configurations": [ + { + "name": "CMSIS-DAP", + "cwd": "${workspaceRoot}", + "executable": "${workspaceRoot}${/}build${/}${input:robotProject}${/}${input:robotProject}.elf", + "request": "launch", + "type": "cortex-debug", + "svdFile": "STM32F407.svd", + "servertype": "openocd", + "serverpath": "/usr/bin/openocd", + "configFiles": [ + "${workspaceRoot}${/}config${/}openocd_cmsis_dap.cfg" + ], + "rtos": "FreeRTOS", + "liveWatch": { + "enabled": true, + "samplesPerSecond": 4 + }, + "preLaunchTask": "Build" + }, + { + "name": "ST-LINK", + "cwd": "${workspaceRoot}", + "executable": "${workspaceRoot}${/}build${/}${input:robotProject}${/}${input:robotProject}.elf", + "request": "launch", + "type": "cortex-debug", + "svdFile": "STM32F407.svd", + "servertype": "openocd", + "serverpath": "/usr/bin/openocd", + "configFiles": [ + "${workspaceRoot}${/}config${/}openocd_stlink.cfg" + ], + "rtos": "FreeRTOS", + "liveWatch": { + "enabled": true, + "samplesPerSecond": 8 + }, + "preLaunchTask": "Build" + }, + { + "name": "C/C++ Runner: Debug Session", + "type": "cppdbg", + "request": "launch", + "args": [], + "stopAtEntry": false, + "externalConsole": false, + "cwd": "/home/polarbearbo/RoboMaster/Embedded-Control/control-base/devices/src", + "program": "/home/polarbearbo/RoboMaster/Embedded-Control/control-base/devices/src/build/Debug/outDebug", + "MIMode": "gdb", + "miDebuggerPath": "gdb", + "setupCommands": [ { - "name": "CMSIS-DAP", - "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}${/}build${/}${input:robotProject}${/}${input:robotProject}.elf", - "request": "launch", - "type": "cortex-debug", - "svdFile": "STM32F407.svd", - "servertype": "openocd", //GDB server - "configFiles": [ - "${workspaceRoot}${/}config${/}openocd_cmsis_dap.cfg" // config - ], - //"runToEntryPoint": "main", // stop at main - //"showDevDebugOutput": "raw", // enable debug output - "rtos": "FreeRTOS", - "liveWatch": { - "enabled": true, - "samplesPerSecond": 4 - }, - "preLaunchTask": "Build" // build before debug - }, - { - "name": "ST-LINK", - "cwd": "${workspaceRoot}", - "executable": "${workspaceRoot}${/}build${/}${input:robotProject}${/}${input:robotProject}.elf", - "request": "launch", - "type": "cortex-debug", - "svdFile": "STM32F407.svd", - "servertype": "openocd", //GDB server - "configFiles": [ - "${workspaceRoot}${/}config${/}openocd_stlink.cfg" // config - ], - //"runToEntryPoint": "main", // stop at main - //"showDevDebugOutput": "raw", // enable debug output - "rtos": "FreeRTOS", - "liveWatch": { - "enabled": true, - "samplesPerSecond": 8 - }, - "preLaunchTask": "Build" // build before debug - } - ], - "inputs": [ - { - "id": "robotProject", - "type": "pickString", - "description": "Select the robot project to debug", - "options": [ - "Swerve-Standard", - "Sentry", - //"Template" - // Add your other robot projects here - ] + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true } - ] + ] + } + ], + "inputs": [ + { + "id": "robotProject", + "type": "pickString", + "description": "Select the robot project to debug", + "options": [ + "Swerve-Standard", + "Sentry" + ] + } + ] } \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..63e79fa --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ +![Project Logo](images/logo.png) + + diff --git a/Sentry/src/motor_task.c b/Sentry/src/motor_task.c index 83e1a7f..1a1d0ac 100644 --- a/Sentry/src/motor_task.c +++ b/Sentry/src/motor_task.c @@ -7,12 +7,12 @@ extern Supercap_t g_supercap; void Motor_Task_Loop() { - DJI_Motor_Send(); + // DJI_Motor_Send(); // MF_Motor_Send(); // DM_Motor_Send(); g_supercap.send_counter++; - if (g_supercap.send_counter >= 100) { + if (g_supercap.send_counter >= (1000 / SUPERCAP_SEND_FREQ)) { Supercap_Send(); g_supercap.send_counter = 0; } diff --git a/Sentry/src/robot.c b/Sentry/src/robot.c index 9a412e6..d3daa5c 100644 --- a/Sentry/src/robot.c +++ b/Sentry/src/robot.c @@ -49,8 +49,8 @@ void Handle_Starting_Up_State() Launch_Task_Init(); Remote_Init(&huart3); CAN_Service_Init(); - Referee_System_Init(&huart1); - Supercap_Init(&g_supercap); + // Referee_System_Init(&huart1); + Supercap_Init(&huart1); // Set robot state to disabled g_robot_state.state = DISABLED; diff --git a/control-base/devices/inc/supercap.h b/control-base/devices/inc/supercap.h index cf315c5..45281d5 100644 --- a/control-base/devices/inc/supercap.h +++ b/control-base/devices/inc/supercap.h @@ -5,22 +5,53 @@ #include "bsp_can.h" #include "referee_system.h" #include "jetson_orin.h" - -#define SUPERCAP_POWER (20) - +#include "bsp_uart.h" +// #define SUPERCAP_POWER (20) +#define SUPERCAP_SEND_FREQ (1.0f) // Hz +//todo ask why it is 128 +#define SUPERCAP_RX_BUFFER_SIZE (1) //number of bytes +#define SUPERCAP_BUFFER_SIZE (64) // new buffer +#define SUPERCAP_TX_BUFFER_SIZE (3) //todo finish this typedef struct { uint8_t can_bus; uint16_t tx_id; uint16_t rx_id; - uint8_t send_counter; + uint16_t send_counter; + + uint8_t receive_counter; + uint8_t final_receive_counter; uint8_t supercap_percent; uint8_t supercap_enabled_flag; + + float Vi; //input voltage + float Vo; //output voltage + float Pi; //input power + float Ii; //input current + float Ps; //reference power + float Io; //output current + + //formate of rx + //printf("Vi:%2.2f Vo:%2.2f Pi:%3.2f Ii:%2.2f Io:%2.2f Pa:%3.2f\r\n", VIPMR, VOFWR,POW_IN, IIPWR, IOPWR, pref); + uint8_t rx_buffer[SUPERCAP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[SUPERCAP_TX_BUFFER_SIZE]; // chnage this to other uint type if possible + uint8_t buffer_for_construction[SUPERCAP_BUFFER_SIZE]; //storing each byte unitl \r\n + uint8_t final_buffer_for_construction[SUPERCAP_BUFFER_SIZE]; + // struct { + // float input_voltage; // Vi + // float output_voltage; // Vo + // float input_power; //Pi + // float input_current; //Ii + // float output_currenIi; //Io + // float ref_power; //Pa + // } supercap_data; + } Supercap_t; -void Supercap_Init(Supercap_t *g_supercap); -void Supercap_Decode(CAN_Instance_t *can_instance); + +void Supercap_Init(UART_HandleTypeDef *huartx); // in robot the input element is g_supercap +void Supercap_Decode(UART_Instance_t *supercap_uart_instance_ptr, float *Vi, float *Vo, float *Pi, float *Ii, float *Io, float *Ps); void Supercap_Send(void); extern Supercap_t g_supercap; diff --git a/control-base/devices/src/supercap.c b/control-base/devices/src/supercap.c index 4e5b933..c2ea9d4 100644 --- a/control-base/devices/src/supercap.c +++ b/control-base/devices/src/supercap.c @@ -1,51 +1,107 @@ #include "supercap.h" +#include "bsp_uart.h" +#include "bsp_daemon.h" +#include "string.h" +#include "stdlib.h" +#include "bsp_serial.h" -#include "referee_system.h" - +#define SUPERCAP_TIMEOUT_MS (3000) Supercap_t g_supercap; -CAN_Instance_t *supercap_can_instance; +Daemon_Instance_t *g_supercap_daemon_ptr; +// CAN_Instance_t *supercap_can_instance; +UART_Instance_t *supercap_uart_instance_ptr; extern Jetson_Orin_Data_t g_orin_data; +char* middle_cpy; +char uart_buffer[SUPERCAP_RX_BUFFER_SIZE]; +uint8_t uart_byte; +uint8_t uart_index = 0; +uint8_t supercap_uart_instance_initialized; + +// Parsed values +//float Vi = 0, Vo = 0, Pi = 0, Io = 0, Ps = 0, Ii = 0; struct rx_data { - uint16_t cap_percentage; - uint16_t reserved1; - uint16_t reserved2; - uint16_t reserved3; -} g_supercap_rx_data; + uint16_t max_discharge; + uint16_t base_power; + int16_t cap_energy_percent; + uint16_t cap_state; +} g_supercap_data; -void Supercap_Init(Supercap_t *g_supercap) -{ - // Initialize supercap - g_supercap->can_bus = 1; - g_supercap->tx_id = 0x2C8; - g_supercap->rx_id = 0x2C7; - supercap_can_instance = - CAN_Device_Register(g_supercap->can_bus, g_supercap->tx_id, - g_supercap->rx_id, Supercap_Decode); + +// Function to extract float after a key like "Vi:" +// float extract_value(const char *src, const char *key) { +// const char *start = strstr(src, key); +// if (!start) return 0.0f; +// start += strlen(key); // move pointer after "Vi:" +// return atof(start); // convert to float +// } + +void Supercap_Timeout_Callback(void){ + UART_Service_Init(supercap_uart_instance_ptr); +} + +void Supercap_Decode_Callback(UART_Instance_t *uart_instance) { + // static float Vi, Vo, Pi, Io, Ps, Ii; + + // uart_instance->rx_buffer[0]; + if (uart_instance->rx_buffer[0] != '\n') { + + g_supercap.buffer_for_construction[g_supercap.receive_counter] = uart_instance->rx_buffer[0]; // store the byte in the buffer + g_supercap.receive_counter++; // R + } + else // if the end of frame is reached + { + g_supercap.buffer_for_construction[g_supercap.receive_counter] = '\0'; + g_supercap.receive_counter = 0; // reset counter if \n is received + Daemon_Reload(g_supercap_daemon_ptr); + + // Supercap decode logic + // g_supercap.Vi = extract_value((const char*)g_supercap.buffer_for_construction, "Vi:"); + // g_supercap.Vo = extract_value((const char*)g_supercap.buffer_for_construction, "Vo:"); + // g_supercap.Pi = extract_value((const char*)g_supercap.buffer_for_construction, "Pi:"); + // g_supercap.Ii = extract_value((const char*)g_supercap.buffer_for_construction, "Ii:"); + // g_supercap.Io = extract_value((const char*)g_supercap.buffer_for_construction, "Io:"); + // g_supercap.Ps = extract_value((const char*)g_supercap.buffer_for_construction, "Ps:"); + sscanf((char *)g_supercap.buffer_for_construction, "Vi:%f Vo:%f Pi:%f Ii:%f Io:%f Ps:%f", + &g_supercap.Vi, &g_supercap.Vo, &g_supercap.Pi, &g_supercap.Ii, &g_supercap.Io, &g_supercap.Ps); + } } -void Supercap_Decode(CAN_Instance_t *can_instance) +void Supercap_Init(UART_HandleTypeDef *huartx) { - // Recieve supercap data - uint16_t *supercap_rx = (uint16_t *) can_instance->rx_buffer; - - g_supercap_rx_data.cap_percentage = supercap_rx[0]; - g_supercap_rx_data.reserved1 = 0; - g_supercap_rx_data.reserved2 = 0; - g_supercap_rx_data.reserved3 = 0; - // ! do not read more than 8 bytes from the buffer + // Initialize supercap + // g_supercap->can_bus = 1; // can 2 + // g_supercap->tx_id = 0x2C8; + // g_supercap->rx_id = 0x2C7; + + //supercap_uart_instance_ptr = UART_Register(huartx, g_supercap.rx_buffer, SUPERCAP_RX_BUFFER_SIZE, Supercap_Decode); + supercap_uart_instance_ptr = UART_Register(huartx, g_supercap.rx_buffer, SUPERCAP_RX_BUFFER_SIZE, Supercap_Decode_Callback); // matches expected signature + uint16_t reload_value = SUPERCAP_TIMEOUT_MS / DAEMON_PERIOD; + uint16_t intial_counter = reload_value; + g_supercap_daemon_ptr = Daemon_Register(reload_value, intial_counter, Supercap_Timeout_Callback); + supercap_uart_instance_initialized = 1; //turn on supercap uart } + +// fomrate PXXXP - sending XXX as power +// PVONP - Turn on supercap +// PVOFFP - Turn off supercap void Supercap_Send(void) { + if (!supercap_uart_instance_initialized) { + return; + } + // Send supercap data - uint16_t *supercap_tx = (uint16_t *) supercap_can_instance->tx_buffer; - supercap_tx[0] = Referee_System.Robot_State.Chassis_Power_Max; // The power limit from the referee system - supercap_tx[1] = (uint16_t) Referee_System.Power_Heat.Chassis_Power; // the total power requested from the chassis - supercap_tx[2] = 0; - supercap_tx[3] = 0; + // uint16_t *supercap_tx = (uint16_t *) supercap_can_instance->tx_buffer; + // supercap_tx[0] = 0x003C; + // supercap_tx[1] = 0x003C; + // supercap_tx[2] = 0x2012; + // supercap_tx[3] = 0x0112; // ! do not write more than 8 bytes to the buffer - - CAN_Transmit(supercap_can_instance); + + // UART_Transmit(supercap_uart_instance_ptr, g_supercap.tx_buffer, sizeof(g_orin_data.tx_buffer), UART_DMA); + uint8_t max_power = 60; + DEBUG_PRINTF(supercap_uart_instance_ptr->uart_handle, "P%03dP\r\n", max_power); } \ No newline at end of file diff --git a/images/Locomotion_omni.png b/images/Locomotion_omni.png new file mode 100644 index 0000000..3bcad78 Binary files /dev/null and b/images/Locomotion_omni.png differ