diff --git a/src/algo/inc/five_bar_leg.h b/src/algo/inc/five_bar_leg.h index c1df1c2..c2ac232 100644 --- a/src/algo/inc/five_bar_leg.h +++ b/src/algo/inc/five_bar_leg.h @@ -38,9 +38,11 @@ typedef struct leg float compensatioin_torq; float target_length; uint32_t current_tick, last_tick; + float normal_force; + float actual_virtual_torq; } Leg_t; void Leg_ForwardKinematics(Leg_t *leg, float phi1, float phi2, float phi1_dot, float phi2_dot); void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *leg_2); -void Leg_VMC(Leg_t *leg); +void Leg_VMC(Leg_t *leg, float torq1, float torq4); #endif diff --git a/src/algo/src/five_bar_leg.c b/src/algo/src/five_bar_leg.c index 230a760..039002b 100644 --- a/src/algo/src/five_bar_leg.c +++ b/src/algo/src/five_bar_leg.c @@ -60,7 +60,7 @@ void Leg_InverseKinematics(float height, float leg_angle, float *leg_1, float *l *leg_2 = -(2 * (*leg_2) - PI); } -void Leg_VMC(Leg_t *leg) +void Leg_VMC(Leg_t *leg, float torq1, float torq4) { // float leg_length = leg->length; // float theta = leg->phi0 - PI / 2; @@ -112,4 +112,12 @@ void Leg_VMC(Leg_t *leg) leg->torq1 = j11 * leg->target_leg_virtual_force + j12 * leg->target_leg_virtual_torq; leg->torq4 = j21 * leg->target_leg_virtual_force + j22 * leg->target_leg_virtual_torq; + + float det = j11 * j22 - j12 * j21; + float j11_inv = j22 / det; + float j12_inv = -j12 / det; + float j21_inv = -j21 / det; + float j22_inv = j11 / det; + leg->normal_force = leg->normal_force * 0.95f + 0.05f * (j11_inv * torq1 + j12_inv * torq4); // normal force calculated using the actual torq current + leg->actual_virtual_torq = leg->actual_virtual_torq * 0.95f + 0.05f * (j21_inv * torq1 + j22_inv * torq4); // virtual torq calculated using the actual torq current } \ No newline at end of file diff --git a/src/app/inc/board_comm_task.h b/src/app/inc/board_comm_task.h index e70b989..1614171 100644 --- a/src/app/inc/board_comm_task.h +++ b/src/app/inc/board_comm_task.h @@ -6,6 +6,8 @@ typedef struct Board_Comm_Package_s float robot_pitch_rate; float x_acceleration; float yaw; + float robot_roll; + float robot_roll_rate; } Board_Comm_Package_t; void Board_Comm_Task_Init(void); diff --git a/src/app/inc/chassis_task.h b/src/app/inc/chassis_task.h index fb10394..f4ef6b2 100644 --- a/src/app/inc/chassis_task.h +++ b/src/app/inc/chassis_task.h @@ -17,8 +17,18 @@ typedef struct chassis_s float angle_diff; float forward_speed; uint8_t chassis_killed_by_referee; + float roll_compensation_height; } Chassis_t; +typedef struct jump_state_machine_s +{ + uint8_t flag_jump_start; + uint8_t flag_retracted; + uint8_t flag_extended; + uint8_t flag_reretracted; + uint8_t flag_reextended; +} Jump_State_Machine_t; + // Function prototypes void Chassis_Task_Init(void); void Chassis_Ctrl_Loop(void); diff --git a/src/app/src/board_comm_task.c b/src/app/src/board_comm_task.c index 8ab27e2..513ccb6 100644 --- a/src/app/src/board_comm_task.c +++ b/src/app/src/board_comm_task.c @@ -9,23 +9,28 @@ Board_Comm_Package_t g_board_comm_package; CAN_Instance_t *g_board_comm_part1; CAN_Instance_t *g_board_comm_part2; +CAN_Instance_t *g_board_comm_part3; uint8_t g_board_comm_first_part_sending_pending = 0; uint8_t g_board_comm_second_part_sending_pending = 0; +uint8_t g_board_comm_third_part_sending_pending = 0; uint8_t g_board_comm_initialized = 0; uint8_t g_board_comm_first_part_established = 0; uint8_t g_board_comm_second_part_established = 0; void board_comm_recv_first_part(CAN_Instance_t *can_instance); void board_comm_recv_second_part(CAN_Instance_t *can_instance); +void board_comm_recv_third_part(CAN_Instance_t *can_instance); void Board_Comm_Task_Init() { #ifdef MASTER g_board_comm_part1 = CAN_Device_Register(1, 0x060, 0x050, board_comm_recv_first_part); g_board_comm_part2 = CAN_Device_Register(1, 0x061, 0x051, board_comm_recv_second_part); + g_board_comm_part3 = CAN_Device_Register(1, 0x062, 0x052, board_comm_recv_third_part); #else #pragma message "Board_Comm_Task_Init() is compiled" g_board_comm_part1 = CAN_Device_Register(1, 0x050, 0x060, board_comm_recv_first_part); g_board_comm_part2 = CAN_Device_Register(1, 0x051, 0x061, board_comm_recv_second_part); + g_board_comm_part3 = CAN_Device_Register(1, 0x052, 0x062, board_comm_recv_third_part); #endif } @@ -41,6 +46,12 @@ void board_comm_recv_second_part(CAN_Instance_t *can_instance) memcpy(&g_board_comm_package.x_acceleration, can_instance->rx_buffer, 8); } +void board_comm_recv_third_part(CAN_Instance_t *can_instance) +{ + g_board_comm_second_part_established = 1; + memcpy(&g_board_comm_package.robot_roll, can_instance->rx_buffer, 8); +} + void Board_Comm_Task_Loop(void) { #ifndef MASTER @@ -52,6 +63,10 @@ void Board_Comm_Task_Loop(void) memcpy(&(g_board_comm_part2->tx_buffer[0]), &(g_imu.accel_body[1]), 4); memcpy(&(g_board_comm_part2->tx_buffer[4]), &(g_imu.rad_fusion.yaw), 4); g_board_comm_second_part_sending_pending = 1; + + memcpy(&(g_board_comm_part3->tx_buffer[0]), &(g_imu.rad_fusion.pitch), 4); + memcpy(&(g_board_comm_part3->tx_buffer[4]), &(g_imu.bmi088_raw.gyro[1]), 4); + g_board_comm_third_part_sending_pending = 1; } #endif } \ No newline at end of file diff --git a/src/app/src/chassis_task.c b/src/app/src/chassis_task.c index 95af553..295d76a 100644 --- a/src/app/src/chassis_task.c +++ b/src/app/src/chassis_task.c @@ -43,17 +43,17 @@ const float vaEstimateKF_H[4] = {1.0f, 0.0f, /* Statistics */ #define UP_ANGLE_ODD (-48.0f) #define UP_ANGLE_EVEN (180.0f - (-48.0f)) -#define UP_1 (58396.0f) -#define UP_2 (17889.0f) -#define UP_3 (51440.0f) -#define UP_4 (22877.0f) +#define UP_1 (19806.0f) //(58396.0f) // 19806 +#define UP_2 (53819.0f - 65536.0f) //(17889.0f) // 53819 +#define UP_3 (32624.0f) //(51440.0f) // 32624 +#define UP_4 (36213.0f) //(22877.0f) // 36213 #define DOWN_ANGLE_ODD (87.0f) #define DOWN_ANGLE_EVEN (180.0f - 87.0f) -#define DOWN_1 (33540.0f) -#define DOWN_2 (42464.0f) -#define DOWN_3 (26586.0f) -#define DOWN_4 (47774.0f) +#define DOWN_1 (60722.0f - 65536.0f) // (33540.0f) // 60722 +#define DOWN_2 (12924.0f) // due to zero-crossing// (42464.0f) // 12924 +#define DOWN_3 (7939.0f) // (26586.0f) // 7939 +#define DOWN_4 (60973.0f) // (47774.0f) // 60973 #define FOOT_MOTOR_MAX_TORQ (2.4f) #define FOOT_MF9025_MAX_TORQ_INT ((FOOT_MOTOR_MAX_TORQ / MF9025_TORQ_CONSTANT) / 16.5f * 2048.0f) @@ -65,6 +65,7 @@ extern Robot_State_t g_robot_state; extern Remote_t g_remote; extern IMU_t g_imu; Chassis_t g_chassis; +Jump_State_Machine_t g_jump_state_machine; uint8_t g_left_foot_initialized = 0; uint8_t g_right_foot_initialized = 0; MF_Motor_Handle_t *g_motor_lf, *g_motor_rf, *g_motor_lb, *g_motor_rb; @@ -78,6 +79,7 @@ PID_t g_balance_angle_pid, g_balance_vel_pid; PID_t g_pid_yaw_angle; PID_t g_pid_anti_split; PID_t g_pid_follow_gimbal; +PID_t g_pid_roll_compensation; Leg_t test = { .phi1 = PI, .phi4 = 0, @@ -87,7 +89,7 @@ Leg_t test = { Daemon_Instance_t *g_daemon_chassis_power_guard; void _get_leg_statistics(); void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch_dot); -void _leg_length_controller(float chassis_height); +void _leg_length_controller(float chassis_height, float robot_roll); void _lqr_balancce_controller(); void _vmc_torq_calc(); float g_test_angle; @@ -170,6 +172,7 @@ void Chassis_Task_Init() PID_Init(&g_pid_follow_gimbal, 8.0f, 0.0f, 0.95f, 6.0f, 0.0f, 0.0f); g_robot_state.chassis_height = CHASSIS_DEFAULT_HEIGHT; + PID_Init(&g_pid_roll_compensation, 0.1f, 0.0006f, 0.0f, 0.20f, 0.15f, 0.0f); xvEstimateKF_Init(&vaEstimateKF); } @@ -180,10 +183,10 @@ uint8_t _is_turning() void _hip_motor_torq_ctrl(float torq_lf, float torq_lb, float torq_rb, float torq_rf) { - int16_t torq1 = torq_lf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); - int16_t torq2 = torq_lb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); - int16_t torq3 = torq_rb / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); - int16_t torq4 = torq_rf / MG8016_TORQ_CONSTANT * (2048.0f / 16.5f); + int16_t torq1 = torq_lf / MG8016_TORQ_CONSTANT * (2048.0f / 33.0f); + int16_t torq2 = torq_lb / MG8016_TORQ_CONSTANT * (2048.0f / 33.0f); + int16_t torq3 = torq_rb / MG8016_TORQ_CONSTANT * (2048.0f / 33.0f); + int16_t torq4 = torq_rf / MG8016_TORQ_CONSTANT * (2048.0f / 33.0f); __MAX_LIMIT(torq1, -2000, 2000); __MAX_LIMIT(torq2, -2000, 2000); __MAX_LIMIT(torq3, -2000, 2000); @@ -265,14 +268,29 @@ void _wheel_leg_estimation(float robot_yaw, float robot_pitch, float robot_pitch void _get_leg_statistics() { - g_leg_left.phi1 = ((g_motor_lb->stats->angle - DOWN_2) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN) / (UP_2 - DOWN_2)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; +# pragma message "we are too lazy to write the legit function, if you see this, feel free to correct the angle overflow :)" + float angle_lf = g_motor_lf->stats->angle; + float angle_lb = g_motor_lb->stats->angle; + float angle_rb = g_motor_rb->stats->angle; + float angle_rf = g_motor_rf->stats->angle; +#define MF8016_MAX_TICK (65536) + // 40000 is an arbitrary number, it is because the motor task space includes the zero-crosing point of the encoder + if (angle_lf > 40000) + { + angle_lf -= MF8016_MAX_TICK; + } + if (angle_lb > 40000) { + angle_lb -= MF8016_MAX_TICK; + } + + g_leg_left.phi1 = ((angle_lb - DOWN_2) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN) / (UP_2 - DOWN_2)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; // g_leg_left.phi1_dot = g_motor_lb->stats->velocity; - g_leg_left.phi4 = ((g_motor_lf->stats->angle - DOWN_1) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD) / (UP_1 - DOWN_1)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; + g_leg_left.phi4 = ((angle_lf - DOWN_1) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD) / (UP_1 - DOWN_1)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; // g_leg_left.phi4_dot = g_motor_lf->stats->velocity; - g_leg_right.phi1 = ((g_motor_rf->stats->angle - DOWN_4) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN) / (UP_4 - DOWN_4)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; + g_leg_right.phi1 = ((angle_rf - DOWN_4) * ((UP_ANGLE_EVEN - DOWN_ANGLE_EVEN) / (UP_4 - DOWN_4)) + DOWN_ANGLE_EVEN) * DEG_TO_RAD; // g_leg_right.phi1_dot = g_motor_rf->stats->velocity; - g_leg_right.phi4 = ((g_motor_rb->stats->angle - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD) / (UP_3 - DOWN_3)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; + g_leg_right.phi4 = ((angle_rb - DOWN_3) * ((UP_ANGLE_ODD - DOWN_ANGLE_ODD) / (UP_3 - DOWN_3)) + DOWN_ANGLE_ODD) * DEG_TO_RAD; // g_leg_right.phi4_dot = g_motor_rb->stats->velocity; } @@ -282,13 +300,14 @@ void _target_state_reset() g_lqr_right_state.target_x = g_lqr_right_state.x; g_robot_state.chassis_height = CHASSIS_DEFAULT_HEIGHT; g_chassis.target_yaw = g_chassis.current_yaw; - + g_chassis.roll_compensation_height = 0; PID_Reset(&g_pid_left_leg_length); PID_Reset(&g_pid_right_leg_length); PID_Reset(&g_pid_left_leg_angle); PID_Reset(&g_pid_right_leg_angle); PID_Reset(&g_balance_angle_pid); PID_Reset(&g_balance_vel_pid); + PID_Reset(&g_pid_roll_compensation); } void _target_state_update(float forward_speed, float turning_speed, float chassis_height) @@ -323,14 +342,72 @@ void _target_state_update(float forward_speed, float turning_speed, float chassi // __MAX_LIMIT(g_robot_state.chassis_height, 0.1f, 0.35f); } -void _leg_length_controller(float chassis_height) +void _leg_length_controller(float chassis_height, float robot_roll) { + float chassis_height_temp = chassis_height; float feedforward_weight = 90.0f; + + if (g_jump_state_machine.flag_jump_start) + { + feedforward_weight = 20.0f; + g_pid_left_leg_length.kp = 5000.0f * 5.0f; + g_pid_left_leg_length.kd = 150.0f * 5.0f; + g_pid_right_leg_length.kp = 5000.0f * 5.0f; + g_pid_right_leg_length.kd = 150.0f * 5.0f; + chassis_height_temp = 0.1f; + } else if (g_jump_state_machine.flag_retracted) + { + feedforward_weight = 200.0f; + chassis_height_temp = 0.35f; + } else if (g_jump_state_machine.flag_extended) + { + feedforward_weight = -220.0f; + chassis_height_temp = 0.1f; + } else if (g_jump_state_machine.flag_reretracted) + { + feedforward_weight = 80.0f; + chassis_height_temp = 0.35f; + g_pid_left_leg_length.kp = 5000.0f * 1.0f; + g_pid_left_leg_length.kd = 150.0f * 1.0f; + g_pid_right_leg_length.kp = 5000.0f * 1.0f; + g_pid_right_leg_length.kd = 150.0f * 1.0f; + } else if (g_jump_state_machine.flag_reextended) + { + // hi :) + feedforward_weight = 60.0f; + g_pid_left_leg_length.kp = 5000.0f * 1.0f; + g_pid_left_leg_length.kd = 150.0f * 1.0f; + g_pid_right_leg_length.kp = 5000.0f * 1.0f; + g_pid_right_leg_length.kd = 150.0f * 1.0f; + } + + if ((g_jump_state_machine.flag_jump_start) && (g_leg_left.length < 0.12f) && (g_leg_right.length < 0.12f)) + { + g_jump_state_machine.flag_jump_start = 0; + g_jump_state_machine.flag_retracted = 1; + } else if (g_jump_state_machine.flag_retracted && (g_leg_left.length > 0.33f) && (g_leg_right.length > 0.33f)) + { + g_jump_state_machine.flag_retracted = 0; + g_jump_state_machine.flag_extended = 1; + } else if (g_jump_state_machine.flag_extended && (g_leg_left.length < 0.16f) && (g_leg_right.length < 0.16f)) + { + g_jump_state_machine.flag_extended = 0; + g_jump_state_machine.flag_reretracted = 1; + } else if (g_jump_state_machine.flag_reretracted && (g_leg_left.length > 0.2f) && (g_leg_right.length > 0.2f)) + { + g_jump_state_machine.flag_reretracted = 0; + g_jump_state_machine.flag_reextended = 1; + } else if (g_jump_state_machine.flag_reextended && (g_leg_left.length < 0.12f) && (g_leg_right.length < 0.12f)) + { + g_jump_state_machine.flag_reextended = 0; + } + // g_leg_left.compensatioin_torq = -g_chassis.centripetal_force * 0.5f; // g_leg_right.compensatioin_torq = +g_chassis.centripetal_force * 0.5f; - g_leg_left.target_leg_virtual_force = PID_dt(&g_pid_left_leg_length, chassis_height - g_leg_left.length, TASK_TIME) + feedforward_weight; - g_leg_right.target_leg_virtual_force = PID_dt(&g_pid_right_leg_length, chassis_height - g_leg_right.length, TASK_TIME) + feedforward_weight; + g_chassis.roll_compensation_height = g_chassis.roll_compensation_height * 0.99f + 0.01f * PID(&g_pid_roll_compensation, robot_roll); + g_leg_left.target_leg_virtual_force = PID_dt(&g_pid_left_leg_length, chassis_height_temp + g_chassis.roll_compensation_height - g_leg_left.length, TASK_TIME) + feedforward_weight; + g_leg_right.target_leg_virtual_force = PID_dt(&g_pid_right_leg_length, chassis_height_temp - g_chassis.roll_compensation_height - g_leg_right.length, TASK_TIME) + feedforward_weight; // g_leg_left.target_leg_virtual_force += g_leg_left.compensatioin_torq; // g_leg_right.target_leg_virtual_force += g_leg_right.compensatioin_torq; g_leg_left.target_leg_virtual_torq = -g_u_left.T_B; @@ -354,8 +431,12 @@ void _lqr_balancce_controller() } void _vmc_torq_calc() { - Leg_VMC(&g_leg_left); - Leg_VMC(&g_leg_right); + Leg_VMC(&g_leg_left, + -g_motor_lb->stats->current * MG8016_CURRENT_INT_TO_TORQ_NM, \ + -g_motor_lf->stats->current * MG8016_CURRENT_INT_TO_TORQ_NM); + Leg_VMC(&g_leg_right, + -g_motor_rf->stats->current * MG8016_CURRENT_INT_TO_TORQ_NM, \ + -g_motor_rb->stats->current * MG8016_CURRENT_INT_TO_TORQ_NM); PID_dt(&g_pid_yaw_angle, g_chassis.current_yaw - g_chassis.target_yaw, TASK_TIME); g_u_left.T_A -= g_pid_yaw_angle.output; @@ -464,14 +545,15 @@ void Chassis_Ctrl_Loop() _chassis_cmd(); _wheel_leg_estimation(g_board_comm_package.yaw, g_board_comm_package.robot_pitch, g_board_comm_package.robot_pitch_rate); _target_state_update(g_chassis.forward_speed, g_chassis.target_yaw_speed, g_robot_state.chassis_height); - _leg_length_controller(g_robot_state.chassis_height); + _leg_length_controller(g_robot_state.chassis_height, g_board_comm_package.robot_roll); _lqr_balancce_controller(); _vmc_torq_calc(); if (last_spintop_mode == 1 && g_robot_state.spintop_mode == 0) { _target_state_reset(); } - if ((g_robot_state.enabled) && (!g_chassis.chassis_killed_by_referee)) // add wheel offline detection + // if ((g_robot_state.enabled) && (!g_chassis.chassis_killed_by_referee)) // add wheel offline detection + if ((g_robot_state.enabled))// add wheel offline detection { _hip_motor_torq_ctrl(-g_leg_left.torq4, -g_leg_left.torq1, -g_leg_right.torq4, -g_leg_right.torq1); _foot_motor_torq_ctrl(-g_u_left.T_A, g_u_right.T_A); diff --git a/src/app/src/gimbal_task.c b/src/app/src/gimbal_task.c index 7ec39b6..5dd813c 100644 --- a/src/app/src/gimbal_task.c +++ b/src/app/src/gimbal_task.c @@ -41,8 +41,8 @@ void Gimbal_Task_Init() { Motor_Config_t yaw_motor_config = { .can_bus = 1, - .speed_controller_id = 6, - .offset = 870, + .speed_controller_id = 3, + .offset = 7685, .control_mode = POSITION_VELOCITY_SERIES, .motor_reversal = MOTOR_REVERSAL_NORMAL, .use_external_feedback = 1, @@ -69,7 +69,7 @@ void Gimbal_Task_Init() Motor_Config_t pitch_motor_config = { .can_bus = 1, .speed_controller_id = 7, - .offset = 6170, + .offset = 3235, .use_external_feedback = 1, .external_feedback_dir = 1, .external_angle_feedback_ptr = &g_imu.rad.pitch, // pitch diff --git a/src/app/src/launch_task.c b/src/app/src/launch_task.c index a597a60..7b9c562 100644 --- a/src/app/src/launch_task.c +++ b/src/app/src/launch_task.c @@ -17,7 +17,7 @@ void Feed_Angle_Calc(void); void Launch_Task_Init() { Motor_Config_t flywheel_left_config = { .can_bus = 2, - .speed_controller_id = 7, + .speed_controller_id = 2, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_REVERSED, @@ -30,7 +30,7 @@ void Launch_Task_Init() { Motor_Config_t flywheel_right_config = { .can_bus = 2, - .speed_controller_id = 8, + .speed_controller_id = 1, .offset = 0, .control_mode = VELOCITY_CONTROL, .motor_reversal = MOTOR_REVERSAL_NORMAL, @@ -43,7 +43,7 @@ void Launch_Task_Init() { Motor_Config_t feed_speed_config = { .can_bus = 2, - .speed_controller_id = 6, + .speed_controller_id = 3, .offset = 0, .control_mode = VELOCITY_CONTROL | POSITION_CONTROL, .motor_reversal = MOTOR_REVERSAL_NORMAL, diff --git a/src/app/src/robot.c b/src/app/src/robot.c index a086f76..e0156e9 100644 --- a/src/app/src/robot.c +++ b/src/app/src/robot.c @@ -60,7 +60,7 @@ void Robot_Init() Launch_Task_Init(); Remote_Init(&huart3); Referee_System_Init(&huart1); - Jetson_Orin_Init(&huart6); + // Jetson_Orin_Init(&huart6); #else Melody_t system_init_melody = { .notes = SYSTEM_READY, diff --git a/src/devices/inc/mf_motor.h b/src/devices/inc/mf_motor.h index ca8dcea..3f8ad83 100644 --- a/src/devices/inc/mf_motor.h +++ b/src/devices/inc/mf_motor.h @@ -5,7 +5,8 @@ #define SINGLE_MOTOR_CTRL_STD 0x140 -#define MG8016_TORQ_CONSTANT (2.9f) +#define MG8016_TORQ_CONSTANT (1.44f) +#define MG8016_CURRENT_INT_TO_TORQ_NM (1 / 2048.0f * 33.0f * MG8016_TORQ_CONSTANT) #define MF9025_TORQ_CONSTANT (.32f) #define MF9025_HALF_MAX_TICKS (MF9025_MAX_TICKS/2) #define MF9025_MAX_TICKS (65536.0f) diff --git a/src/devices/src/jetson_orin.c b/src/devices/src/jetson_orin.c index 7c6f4b5..3a7f676 100644 --- a/src/devices/src/jetson_orin.c +++ b/src/devices/src/jetson_orin.c @@ -104,5 +104,5 @@ void Jetson_Orin_Send_Data(void) g_orin_data.tx_buffer[1] = g_orin_data.sending.enemy_color_flag << 1 | g_orin_data.sending.game_start_flag; memcpy(&g_orin_data.tx_buffer[2],&g_orin_data.sending.float_byte.data_bytes[0], 32*sizeof(uint8_t)); - UART_Transmit(g_orin_uart_instance_ptr, g_orin_data.tx_buffer, sizeof(g_orin_data.tx_buffer), UART_DMA); + // UART_Transmit(g_orin_uart_instance_ptr, g_orin_data.tx_buffer, sizeof(g_orin_data.tx_buffer), UART_DMA); } \ No newline at end of file