diff --git a/src/algo/src/Swerve_Locomotion.c b/src/algo/src/Swerve_Locomotion.c index 265b70b..d44c6b9 100644 --- a/src/algo/src/Swerve_Locomotion.c +++ b/src/algo/src/Swerve_Locomotion.c @@ -6,6 +6,7 @@ Swerve_Module_t g_swerve_fl, g_swerve_rl, g_swerve_rr, g_swerve_fr; Swerve_Module_t *swerve_modules[NUMBER_OF_MODULES] = {&g_swerve_fl, &g_swerve_rl, &g_swerve_rr, &g_swerve_fr}; +Module_State_t optimized_module_state; float last_swerve_angle[NUMBER_OF_MODULES] = {0.0f, 0.0f, 0.0f, 0.0f}; /** @@ -43,12 +44,12 @@ void Swerve_Init() // define constants for each module in an array [0] == fl, [1] == rl, [2] == rr, [3] == fr int azimuth_can_bus_array[NUMBER_OF_MODULES] = {2, 2, 2, 2}; int azimuth_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4}; - int azimuth_offset_array[NUMBER_OF_MODULES] = {2050, 1940, 1430, 150}; + int azimuth_offset_array[NUMBER_OF_MODULES] = {2050, 1940, 1430, 8150}; Motor_Reversal_t azimuth_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED}; int drive_can_bus_array[NUMBER_OF_MODULES] = {1, 2, 2, 2}; int drive_speed_controller_id_array[NUMBER_OF_MODULES] = {1, 2, 3, 4}; - Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_NORMAL}; + Motor_Reversal_t drive_motor_reversal_array[NUMBER_OF_MODULES] = {MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_NORMAL, MOTOR_REVERSAL_REVERSED, MOTOR_REVERSAL_REVERSED}; // init common PID configuration for azimuth motors Motor_Config_t azimuth_motor_config = { @@ -178,45 +179,22 @@ Module_State_t Optimize_Module_Angle(Module_State_t input_state, float measured_ { Module_State_t optimized_module_state = {0}; optimized_module_state.speed = input_state.speed; - float target_angle = fmodf(input_state.angle, 2.0f * PI); // get positive normalized target angle - if (target_angle < 0) - { - target_angle += 2.0f * PI; - } + optimized_module_state.angle = input_state.angle; - float curr_angle_normalized = fmodf(measured_angle, 2 * PI); // get normalized current angle - if (curr_angle_normalized < 0) - { - curr_angle_normalized += 2.0f * PI; - } + if(measured_angle > PI) + measured_angle -= 2*PI; - float delta = target_angle - curr_angle_normalized; // calculate the angle delta + float angle_diff = optimized_module_state.angle - measured_angle; - // these conditions essentially create the step function for angles to "snap" too offset from the measured angle to avoid flipping - if (PI / 2.0f < fabsf(delta) && fabsf(delta) < 3 * PI / 2.0f) - { - float beta = PI - fabsf(delta); - beta *= (delta > 0 ? 1.0f : -1.0f); - target_angle = measured_angle - beta; - optimized_module_state.speed = -1.0f * input_state.speed; // invert velocity if optimal angle 180 deg from target - } - else if (fabsf(delta) >= 3.0f * PI / 2.0f) - { - if (delta < 0) - { - target_angle = measured_angle + (2.0f * PI + delta); - } - else - { - target_angle = measured_angle - (2.0f * PI - delta); - } - } - else - { - target_angle = measured_angle + delta; - } + if((angle_diff >= PI/2 && angle_diff <= 3*PI/2) || (angle_diff <= -PI/2 && angle_diff >= -3*PI/2)) + { + optimized_module_state.angle += (angle_diff > 0) ? -PI:PI; + optimized_module_state.speed *= -1.0f; + } + + if (optimized_module_state.angle < 0) + optimized_module_state.angle += 2.0f * PI; - optimized_module_state.angle = target_angle; return optimized_module_state; } diff --git a/src/app/src/debug_task.c b/src/app/src/debug_task.c index 71add01..f39ee06 100644 --- a/src/app/src/debug_task.c +++ b/src/app/src/debug_task.c @@ -15,12 +15,13 @@ extern Robot_State_t g_robot_state; extern DJI_Motor_Handle_t *g_yaw; extern IMU_t g_imu; -extern Swerve_Module_t g_swerve_rr; +extern Swerve_Module_t g_swerve_rl, g_swerve_fr; extern Remote_t g_remote; extern Launch_Target_t g_launch_target; 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}; @@ -29,7 +30,7 @@ char g_debug_buffer[1024*2] = {0}; 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 +//#define DEBUG_ENABLED void Debug_Task_Loop(void) { @@ -52,7 +53,8 @@ void Debug_Task_Loop(void) if (counter > 0xFFFFFFFF) { counter = 0; } - DEBUG_PRINTF(&huart6, ">fl:%d\n>rl:%d\n>rr:%d\n>fr:%d\n",g_swerve_rr.drive_motor->velocity_pid->ref); - //DEBUG_PRINTF(&huart6, ">feed_angle:%f\n>total_angle:%f\n",g_launch_target.feed_angle,g_motor_feed->stats->total_angle_rad); + +// DEBUG_PRINTF(&huart1, ">fr_ref_speed:%f\n>fr_actual_speed:%f\n", +// g_swerve_fr.drive_motor->velocity_pid->ref,g_swerve_fr.drive_motor->stats->current_vel_rpm); #endif } \ No newline at end of file diff --git a/src/devices/inc/dji_motor.h b/src/devices/inc/dji_motor.h index 91df051..0466136 100644 --- a/src/devices/inc/dji_motor.h +++ b/src/devices/inc/dji_motor.h @@ -39,6 +39,7 @@ typedef struct DJI_Motor_Stats_s { uint16_t encoder_offset; int32_t total_round; float absolute_angle_rad; + float last_absolute_angle_rad; float total_angle_rad; float reduction_ratio; } DJI_Motor_Stats_t; diff --git a/src/devices/src/dji_motor.c b/src/devices/src/dji_motor.c index b983fe0..4f3cb19 100644 --- a/src/devices/src/dji_motor.c +++ b/src/devices/src/dji_motor.c @@ -7,7 +7,7 @@ DJI_Motor_Handle_t *g_dji_motors[MAX_DJI_MOTORS] = {NULL}; uint8_t g_dji_motor_count = 0; - +float test_tmd; DJI_Send_Group_t *g_dji_send_group[MAX_DJI_MOTOR_GROUPS] = {NULL}; uint8_t g_dji_motor_group_count = 0; @@ -195,7 +195,7 @@ float DJI_Motor_Get_Absolute_Angle(DJI_Motor_Handle_t *motor_handle) return motor_handle->stats->absolute_angle_rad; break; case MOTOR_REVERSAL_REVERSED: - return -motor_handle->stats->absolute_angle_rad; + return -motor_handle->stats->absolute_angle_rad + 2*PI; break; } return -1; @@ -239,7 +239,7 @@ void DJI_Motor_Set_Angle(DJI_Motor_Handle_t *motor_handle, float angle) motor_handle->angle_pid->ref = angle; break; case MOTOR_REVERSAL_REVERSED: - motor_handle->angle_pid->ref = -angle; + motor_handle->angle_pid->ref = -angle+2*PI; break; default: break; @@ -435,18 +435,28 @@ void DJI_Motor_Decode(CAN_Instance_t *can_instance) motor->current_torq = (int16_t)(data[4] << 8 | data[5]); motor->temp = data[6]; + motor->last_absolute_angle_rad = motor->absolute_angle_rad; motor->absolute_angle_rad = motor->current_tick - motor->encoder_offset; + if(motor->absolute_angle_rad >= 8192) + { + motor->absolute_angle_rad -= 8192; + } + else if(motor->absolute_angle_rad < 0) + { + motor->absolute_angle_rad += 8192; + } /* absolute angle */ __MAP(motor->absolute_angle_rad, 0, 8192, 0, 2 * PI); /* angle wrap */ - if (motor->current_tick - motor->last_tick > DJI_HALF_MAX_TICKS) + if (motor->absolute_angle_rad - motor->last_absolute_angle_rad > PI) { motor->total_round--; } - else if (motor->current_tick - motor->last_tick < -4096) + else if (motor->absolute_angle_rad - motor->last_absolute_angle_rad < -PI) { motor->total_round++; } + #pragma message "there are some problem with total_angle_rad" motor->total_angle_rad = ((motor->total_round) * 2 * PI + motor->absolute_angle_rad) * motor->reduction_ratio; } \ No newline at end of file