Skip to content

Commit

Permalink
bug fixed
Browse files Browse the repository at this point in the history
  • Loading branch information
@CuboiLeo
CuboiLeo committed Jun 5, 2024
1 parent 5544df4 commit ac0bafe
Show file tree
Hide file tree
Showing 4 changed files with 37 additions and 46 deletions.
52 changes: 15 additions & 37 deletions src/algo/src/Swerve_Locomotion.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};

/**
Expand Down Expand Up @@ -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 = {
Expand Down Expand Up @@ -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;
}

Expand Down
10 changes: 6 additions & 4 deletions src/app/src/debug_task.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand All @@ -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)
{
Expand All @@ -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
}
1 change: 1 addition & 0 deletions src/devices/inc/dji_motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
20 changes: 15 additions & 5 deletions src/devices/src/dji_motor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}

0 comments on commit ac0bafe

Please sign in to comment.