robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
main
rm_balance/Chassis_Task.c
Robofish e7aaf1b98f 提交
2025-09-08 20:05:22 +08:00

1359 lines
63 KiB
C
Raw Permalink Blame History

#include "Chassis_Task.h"
#include "bsp_buzzer.h"
#include "FreeRTOS.h"
#include "task.h"
#include "detect_task.h"
#include "remote_control.h"
#include "referee.h"
#include "can.h"
#include "bsp_can.h"
#include "arm_math.h"
#include "string.h"
#include "Kalman_Mix.h"
#include "INS_Task.h"
#include "math.h"
#include "VMC.h"
#include "mpc.h"
#include "lqr.h"
#include "user_lib.h"
#ifndef PI
#define PI 3.14159265358979f
#endif
#ifndef FOLLOW_DEADBAND
#define FOLLOW_DEADBAND 0.05f
#endif
#ifndef min
#define min(a,b) ((a)<(b)?(a):(b))
#endif
#ifndef square
#define square(x) ((x)*(x))
#endif
#define ABS(x) (((x) > 0) ? (x) : (-(x)))
#define LimitMax(input, max) \
{ \
if ((input) > max) \
{ \
input = max; \
} \
else if ((input) < -max) \
{ \
input = -max; \
} \
}
#define LimitOutput(input, min, max ) \
{ \
if( input < min ) \
input = min; \
else if( input > max ) \
input = max; \
}
#define SIGN(x) ((x)>0 ? 1 : ((x)<0? -1: 0))
uint8_t use_mpc = 0, abnormal_debug = 0;
chassis_control_t chassis_control;
kalman2_state Body_Speed_KF;
// ------------------ PID info ------------------
fp32 roll_PD[2] = {100.0, 20.0}; // 200, 45
fp32 coordinate_PD[2] = { 10.0f, 0.5f }; //15.0f,1.0f
fp32 yaw_PD_test[2] = { 20.0f, 180.0f };
fp32 stand_PD[2] = { 200.0f, 50.0f };
fp32 jump_stand_PD[2] = { 10000.0f, 150.0f };
fp32 suspend_stand_PD[2] = { 100.0f, 30.0f };
// fp32 stand_PD[2]={150.0f, 20.0f};//1N/cm
fp32 suspend_foot_speed_p = 10.0f;
// fp32 High_Offset = 0.20;
fp32 High_Offset = 0.06;
fp32 Moving_High_Offset = 0.0f;
uint8_t last_is_update;
fp32 IDEAL_PREPARING_STAND_JUMPING_ANGLE = 0.6f;
extern fp32 LQR[4][10];
fp32 suspend_LQR[2][6] = {
20.0f,5.0f, 0.0f, 0.0f, 0.0f, 0.0f,
0, 0, 0, 0, 0, 0
};
fp32 x_set[10];
fp32 debug_2 = 1,upper_bound, stepp = 0.2;
fp32 debug_flag, last_debug_flag;
fp32 HIGH_HIGH = 0.35f;
fp32 SIT_HIGH = 0.11f;
fp32 NORMAL_HIGH = 0.18f; //16
fp32 lower_dec_speed = 1.0f;
uint8_t mpc_cnt = 0, mpc_period = 9;
float alpha_dx = 0.95f;
fp32 rollP, rollD, roll_angle_deadband = 0.0f, roll_gyro_deadband = 0.0f, leg_dlength_deadband = 0.0f;
fp32 rotate_move_threshold = 45.0f, rotate_speed = 12.0f, fake_sign = 1.0f, rc_sign = 1.0f, rotate_move_scale = 1.0f,normal_move_scale = 2.0f, offset_k = 0.31f, fly_speed = 2.5f;
fp32 rc_angle, rc_angle_temp, X_speed, Y_speed, delta_theta, delta_theta_temp, rotate_move_offset, normalized_speed, temp_max_spd, acc_step = 1.3f, dec_step = 0.1f;
fp32 move_scale_list[11] = { 0.0, 1.0, 1.5, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
fp32 cap_move_scale_list[11] = { 0.0, 1.5, 2.0, 2.5, 2.5, 3.0, 3.0, 3.0, 3.0, 3.0, 3.0};
fp32 rotate_speed_list[11] = {0.0, 5.0, 5.3, 5.6, 6.0, 6.0, 7.0, 8.0, 9.0, 10.0, 12.0};
fp32 rotate_move_scale_list[11] = {0.0, 1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8};
int speed_sign = 0, target_speed_sign = 0;
uint8_t robot_level = 1;
ext_robot_status_t *robot;
fp32 TK_x_p = -10.0f, TK_y_p = 10.0f, TK_y_d = 3.0f, reducing_p = 120.0f;
fp32 stablize_foot_speed_threshold = 1.2f, stablize_yaw_speed_threshold = 1.5f;
uint8_t no_follow_flag, follow_angle;
enum Chassis_Mode chassis_mode;
// ------------------ function definition ------------------
void Chassis_Init( chassis_control_t *init );
void Chassis_Data_Update( chassis_control_t *fdb );
void Chassis_Status_Detect( chassis_control_t *detect );
void Chassis_Mode_Set( chassis_control_t *mode_set );
void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change );
void Target_Value_Set( chassis_control_t *target_value_set );
void Chassis_Torque_Calculation( chassis_control_t *bl_ctrl );
void Chassis_Torque_Combine( chassis_control_t *bl_ctrl );
void Motor_CMD_Send(chassis_control_t *CMD_Send);
void Joint_Motor_to_Init_Pos(void);
void Motor_Zero_CMD_Send(void);
void HT_Motor_zero_set(void);
int16_t set1, set2;
uint16_t launching_time = 0;
void Chassis_Task(void const * argument)
{
vTaskDelay(CHASSIS_TASK_INIT_TIME);
Chassis_Init(&chassis_control);
vTaskDelay(100);
while(1)
{
// ------------------ Update Data ------------------
Chassis_Data_Update(&chassis_control);
// ------------------ Chassis Control ------------------
Chassis_Status_Detect(&chassis_control);
// ------------------ Mode Set ------------------
Chassis_Mode_Set(&chassis_control);
Chassis_Mode_Change_Control_Transit(&chassis_control);
Target_Value_Set(&chassis_control);
Chassis_Torque_Calculation(&chassis_control);
Chassis_Torque_Combine(&chassis_control);
Motor_CMD_Send(&chassis_control);
vTaskDelay(1);
}
}
void Chassis_Init( chassis_control_t *init )
{
// ------------------ Set HT Zero Point ------------------
buzzer_on(75, 60);
vTaskDelay(100);
HT_Motor_zero_set();
buzzer_off();
Motor_Zero_CMD_Send();
kalman_second_order_init(&Body_Speed_KF);
// ------------------ Ptr Set ------------------
init->joint_motor_1.motor_measure = get_HT_motor_measure_point(0);
init->joint_motor_2.motor_measure = get_HT_motor_measure_point(1);
init->joint_motor_3.motor_measure = get_HT_motor_measure_point(2);
init->joint_motor_4.motor_measure = get_HT_motor_measure_point(3);
// init->foot_motor_L.motor_measure = get_BMmotor_measure_point(0);
// init->foot_motor_R.motor_measure = get_BMmotor_measure_point(1);
init->chassis_rc_ctrl = get_Gimabl_control_point();
init->chassis_posture_info.chassis_INS_angle_point = get_INS_angle_point();
init->chassis_posture_info.chassis_INS_gyro_point = get_gyro_data_point();
init->chassis_posture_info.chassis_INS_accel_point = get_acc_data_point();
/*
first_order_filter_init(&rc_filter, 0.002f, rc_filt_numb);
first_order_filter_init(&tl_filter, 0.002f, tl_numb);
first_order_filter_init(&tl_run_filter, 0.002f, tl_numb);
OLS_Init(&OLS_S0_L,2);
OLS_Init(&OLS_S0_R,2);
*/
// ------------------ Mode Set ------------------
init->mode.chassis_mode = init->mode.last_chassis_mode = DISABLE_CHASSIS;
init->mode.chassis_balancing_mode = init->mode.last_chassis_balancing_mode = NO_FORCE;
init->mode.sport_mode = init->mode.last_sport_mode = NORMAL_MOVING_MODE;
init->joint_motor_1.motor_mode = init->joint_motor_1.last_motor_mode = MOTOR_NO_FORCE;
init->joint_motor_2.motor_mode = init->joint_motor_2.last_motor_mode = MOTOR_NO_FORCE;
init->joint_motor_3.motor_mode = init->joint_motor_3.last_motor_mode = MOTOR_NO_FORCE;
init->joint_motor_4.motor_mode = init->joint_motor_4.last_motor_mode = MOTOR_NO_FORCE;
init->foot_motor_L.motor_mode = init->foot_motor_L.last_motor_mode = MOTOR_NO_FORCE;
init->foot_motor_R.motor_mode = init->foot_motor_R.last_motor_mode = MOTOR_NO_FORCE;
// ------------------ Initialize HT Motor ------------------
init->joint_motor_1.position_offset = init->joint_motor_1.motor_measure->ecd;
init->joint_motor_2.position_offset = init->joint_motor_2.motor_measure->ecd;
init->joint_motor_3.position_offset = init->joint_motor_3.motor_measure->ecd;
init->joint_motor_4.position_offset = init->joint_motor_4.motor_measure->ecd;
init->joint_motor_1.position = (init->joint_motor_1.motor_measure->ecd -init->joint_motor_1.position_offset) + LEG_OFFSET;
init->joint_motor_2.position = (init->joint_motor_2.motor_measure->ecd -init->joint_motor_2.position_offset) - LEG_OFFSET;
init->joint_motor_3.position = (init->joint_motor_3.motor_measure->ecd -init->joint_motor_3.position_offset) + LEG_OFFSET;
init->joint_motor_4.position = (init->joint_motor_4.motor_measure->ecd -init->joint_motor_4.position_offset) - LEG_OFFSET;
init->flag_info.init_flag = 1;
Chassis_Data_Update(init);
init->flag_info.init_flag = 0;
init->foot_motor_L.distance_offset = init->foot_motor_L.distance;
init->foot_motor_R.distance_offset = init->foot_motor_R.distance;
Power_Init(init);
// ------------------ MPC init ------------------
MPC_Init();
}
void Chassis_Data_Update( chassis_control_t *fdb )
{
robot = get_robot_status_point();
robot_level = robot->robot_level;
// robot_level = 1;
// -------------------- Update foot measure --------------------
fdb->foot_motor_L.motor_measure.ecd = (uint16_t)motor_BM[0].ecd;
fdb->foot_motor_L.motor_measure.last_ecd = (uint16_t)motor_BM[0].last_ecd;
fdb->foot_motor_L.motor_measure.speed_rpm = (int16_t)motor_BM[0].speed_rpm;
fdb->foot_motor_L.torque_get = motor_BM[0].given_current*55.0f/32767.0f;
fdb->foot_motor_R.motor_measure.ecd = (uint16_t)motor_BM[1].ecd;
fdb->foot_motor_R.motor_measure.last_ecd = (uint16_t)motor_BM[1].last_ecd;
fdb->foot_motor_R.motor_measure.speed_rpm = (int16_t)motor_BM[1].speed_rpm;
fdb->foot_motor_R.torque_get = motor_BM[1].given_current*55.0f/32767.0f;
// ------------------ Update mode info ------------------
fdb->mode.last_chassis_mode = fdb->mode.chassis_mode;
fdb->mode.last_chassis_balancing_mode = fdb->mode.chassis_balancing_mode;
fdb->mode.last_sport_mode = fdb->mode.sport_mode;
fdb->mode.last_jumping_mode = fdb->mode.jumping_mode;
fdb->mode.last_jumping_stage = fdb->mode.jumping_stage;
fdb->mode.last_chassis_high_mode = fdb->mode.chassis_high_mode;
fdb->flag_info.last_static_flag = fdb->flag_info.static_flag;
fdb->flag_info.last_moving_flag = fdb->flag_info.moving_flag;
last_debug_flag = debug_flag;
fdb->flag_info.last_overpower_warning_flag = fdb->flag_info.overpower_warning_flag;
fdb->flag_info.last_stablize_high_flag = fdb->flag_info.stablize_high_flag;
fdb->flag_info.last_suspend_flag_L = fdb->flag_info.suspend_flag_L;
fdb->flag_info.last_suspend_flag_R = fdb->flag_info.suspend_flag_R;
fdb->joint_motor_1.last_motor_mode = fdb->joint_motor_1.motor_mode;
fdb->joint_motor_2.last_motor_mode = fdb->joint_motor_2.motor_mode;
fdb->joint_motor_3.last_motor_mode = fdb->joint_motor_3.motor_mode;
fdb->joint_motor_4.last_motor_mode = fdb->joint_motor_4.motor_mode;
fdb->foot_motor_L.last_motor_mode = fdb->foot_motor_L.motor_mode;
fdb->foot_motor_R.last_motor_mode = fdb->foot_motor_R.motor_mode;
// ------------------ Update IMU info ------------------
fdb->chassis_posture_info.pitch_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_PITCH_ADDRESS_OFFSET);
fdb->chassis_posture_info.roll_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_ROLL_ADDRESS_OFFSET);
fdb->chassis_posture_info.yaw_angle = *(fdb->chassis_posture_info.chassis_INS_angle_point + INS_YAW_ADDRESS_OFFSET) + yaw_count * PI2;
fdb->chassis_posture_info.pitch_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_PITCH_ADDRESS_OFFSET);
fdb->chassis_posture_info.roll_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_ROLL_ADDRESS_OFFSET);
fdb->chassis_posture_info.yaw_gyro = *(fdb->chassis_posture_info.chassis_INS_gyro_point + INS_YAW_ADDRESS_OFFSET);
// ------------------ Update HT Motor info ------------------
fdb->joint_motor_1.position = (fdb->joint_motor_1.motor_measure->ecd - fdb->joint_motor_1.position_offset) + LEG_OFFSET;
fdb->joint_motor_2.position = (fdb->joint_motor_2.motor_measure->ecd - fdb->joint_motor_2.position_offset) - LEG_OFFSET;
fdb->joint_motor_3.position = (fdb->joint_motor_3.motor_measure->ecd - fdb->joint_motor_3.position_offset) + LEG_OFFSET;
fdb->joint_motor_4.position = (fdb->joint_motor_4.motor_measure->ecd - fdb->joint_motor_4.position_offset) - LEG_OFFSET;
fdb->joint_motor_1.velocity = fdb->joint_motor_1.motor_measure->speed_rpm;
fdb->joint_motor_2.velocity = fdb->joint_motor_2.motor_measure->speed_rpm;
fdb->joint_motor_3.velocity = fdb->joint_motor_3.motor_measure->speed_rpm;
fdb->joint_motor_4.velocity = fdb->joint_motor_4.motor_measure->speed_rpm;
fdb->joint_motor_1.torque_get = 0.95f * fdb->joint_motor_1.torque_get + 0.05f * fdb->joint_motor_1.motor_measure->real_torque;
fdb->joint_motor_2.torque_get = 0.95f * fdb->joint_motor_2.torque_get + 0.05f * fdb->joint_motor_2.motor_measure->real_torque;
fdb->joint_motor_3.torque_get = 0.95f * fdb->joint_motor_3.torque_get + 0.05f * fdb->joint_motor_3.motor_measure->real_torque;
fdb->joint_motor_4.torque_get = 0.95f * fdb->joint_motor_4.torque_get + 0.05f * fdb->joint_motor_4.motor_measure->real_torque;
// ------------------ Update LK Motor info ------------------
fdb->foot_motor_L.last_position = fdb->foot_motor_L.position;
fdb->foot_motor_R.last_position = fdb->foot_motor_R.position;
fdb->foot_motor_L.position = fdb->foot_motor_L.motor_measure.ecd * MOTOR_ECD_TO_RAD;// Angle (rad)
fdb->foot_motor_R.position = fdb->foot_motor_R.motor_measure.ecd * MOTOR_ECD_TO_RAD;
if( fdb->flag_info.init_flag != 1 ) // Init_flag = 0: after Init fdb 如果不是在init里面调用就<E794A8>?<3F>圈
{
if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) > HALF_POSITION_RANGE )
fdb->foot_motor_L.turns++;
else if( (fdb->foot_motor_L.last_position - fdb->foot_motor_L.position ) < -HALF_POSITION_RANGE )
fdb->foot_motor_L.turns--;
if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) > HALF_POSITION_RANGE )
fdb->foot_motor_R.turns--;
else if( ( fdb->foot_motor_R.last_position - fdb->foot_motor_R.position ) < -HALF_POSITION_RANGE )
fdb->foot_motor_R.turns++;
}
fdb->foot_motor_L.distance = ( fdb->foot_motor_L.position/PI2 + fdb->foot_motor_L.turns ) * WHEEL_PERIMETER - fdb->foot_motor_L.distance_offset;
fdb->foot_motor_R.distance = ( -fdb->foot_motor_R.position/PI2 + fdb->foot_motor_R.turns ) * WHEEL_PERIMETER - fdb->foot_motor_R.distance_offset;
fdb->chassis_posture_info.foot_distance = ( fdb->foot_motor_L.distance + fdb->foot_motor_R.distance ) /2.0f;
fdb->foot_motor_L.speed = fdb->foot_motor_L.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f; // rpm -> m/s
fdb->foot_motor_R.speed = -fdb->foot_motor_R.motor_measure.speed_rpm * PI2 * WHEEL_RADIUS / 10.0f / 60.0f;
fdb->chassis_posture_info.foot_speed = ( fdb->foot_motor_L.speed + fdb->foot_motor_R.speed ) / 2.0f;
// fdb->foot_motor_L.torque_get = fdb->foot_motor_L.motor_measure.given_current;
// fdb->foot_motor_R.torque_get = fdb->foot_motor_R.motor_measure.given_current;
// ------------------ Kalman Filter ------------------
kalman_second_order_update(&Body_Speed_KF, fdb->chassis_posture_info.foot_distance, fdb->chassis_posture_info.foot_speed, temp_a);
fdb->chassis_posture_info.foot_distance_K = Body_Speed_KF.x[0];
fdb->chassis_posture_info.foot_speed_K = Body_Speed_KF.x[1];
// ------------------ Five_Bars_to_Pendulum ------------------
Forward_kinematic_solution(fdb,-fdb->joint_motor_2.position,-fdb->joint_motor_2.velocity,
-fdb->joint_motor_1.position,-fdb->joint_motor_1.velocity,1);
Forward_kinematic_solution(fdb,fdb->joint_motor_3.position,fdb->joint_motor_3.velocity,
fdb->joint_motor_4.position, fdb->joint_motor_4.velocity,0);
// ------------------ Huanzhi information update ------------------
fdb->chassis_posture_info.leg_angle_L -= PI_2;
fdb->chassis_posture_info.leg_angle_L -= fdb->chassis_posture_info.pitch_angle;
// fdb->chassis_posture_info.leg_gyro_L -= fdb->chassis_posture_info.pitch_gyro;
fp32 temp_v_L = ( fdb->chassis_posture_info.leg_angle_L - fdb->chassis_posture_info.last_leg_angle_L ) / TASK_RUN_TIME;
fdb->chassis_posture_info.leg_gyro_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_L;
fdb->chassis_posture_info .last_leg_angle_L = fdb->chassis_posture_info .leg_angle_L;
fdb->chassis_posture_info.leg_angle_R -= PI_2;
fdb->chassis_posture_info.leg_angle_R -= fdb->chassis_posture_info.pitch_angle;
// fdb->chassis_posture_info.leg_gyro_R -= fdb->chassis_posture_info.pitch_gyro;
fp32 temp_v_R = ( fdb->chassis_posture_info.leg_angle_R - fdb->chassis_posture_info.last_leg_angle_R ) / TASK_RUN_TIME;
fdb->chassis_posture_info.leg_gyro_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_gyro_R;
fdb->chassis_posture_info.last_leg_angle_R = fdb->chassis_posture_info .leg_angle_R;
temp_v_L = ( fdb->chassis_posture_info.leg_length_L - fdb->chassis_posture_info.last_leg_length_L ) / TASK_RUN_TIME;
fdb->chassis_posture_info.leg_dlength_L = alpha_dx * temp_v_L + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_L;
fdb->chassis_posture_info.last_leg_length_L = fdb->chassis_posture_info.leg_length_L;
temp_v_R = ( fdb->chassis_posture_info.leg_length_R - fdb->chassis_posture_info.last_leg_length_R ) / TASK_RUN_TIME;
fdb->chassis_posture_info.leg_dlength_R = alpha_dx * temp_v_R + (1-alpha_dx) * fdb->chassis_posture_info.leg_dlength_R;
fdb->chassis_posture_info.last_leg_length_R = fdb->chassis_posture_info.leg_length_R;
// ---------- Rotate and Move Info update ------------
rotate_move_offset = offset_k * rotate_speed_list[robot_level];
rc_sign = 1.0f;
X_speed = fdb->chassis_rc_ctrl->X_speed;
Y_speed = fdb->chassis_rc_ctrl->Y_speed;
// Original rc angle
if (X_speed == 0){
if (Y_speed > 0)
rc_angle_temp = PI/2;
else if (Y_speed < 0)
rc_angle_temp = -PI/2;
else
rc_angle_temp = 0;
}
else{
Y_speed = fdb->chassis_rc_ctrl->Y_speed;
rc_angle_temp = atan_tl(Y_speed/X_speed);
}
// Normalized speed
if (abs(X_speed) < 0.1 ||abs(Y_speed) <0.1)
temp_max_spd = 1.0f;
else if (X_speed<Y_speed){
temp_max_spd = sqrt(square((1/Y_speed)*X_speed)+1);
}
else{
temp_max_spd = sqrt(square((1/X_speed)*Y_speed)+1);
}
normalized_speed = sqrt(X_speed*X_speed+Y_speed*Y_speed) / temp_max_spd;
// Ref angle and sign
if (X_speed<0) rc_sign *= -1.0f;
rc_angle_temp += rotate_move_offset;
if (rc_angle_temp>PI/2||rc_angle_temp<-PI/2){
rc_sign *= -1.0f;
}
while (rc_angle_temp>PI/2) {
rc_angle_temp -= PI;
}
while (rc_angle_temp<-PI/2){
rc_angle_temp += PI;
}
// if (X_speed < 0) rc_sign = -1.0f;
rc_angle = rc_angle_temp * 180.0f / PI;
if (fdb->chassis_rc_ctrl->fake_flag == 0) fake_sign = -1.0f;
else fake_sign = 1.0f;
}
void Chassis_Status_Detect( chassis_control_t *detect )
{
// ------------------ Off Ground Detect ------------------
if( detect->mode.chassis_balancing_mode == BALANCING_READY && detect->mode.sport_mode!=TK_MODE)
{
if( ABS(detect->chassis_posture_info.pitch_angle) >= DANGER_PITCH_ANGLE )
detect->flag_info.abnormal_flag = 1;
else if( ABS(detect->chassis_posture_info.foot_speed_K) < MOVE_LOWER_BOUND &&
ABS(detect->chassis_posture_info.pitch_angle) < 0.1f &&
detect->flag_info.abnormal_flag )
detect->flag_info.abnormal_flag = 0;
}
if (abnormal_debug){
detect->flag_info.abnormal_flag = 0;
}
Supportive_Force_Cal(detect, detect->joint_motor_1.position, detect->joint_motor_2.position, 1.0f );
Supportive_Force_Cal(detect, detect->joint_motor_3.position, detect->joint_motor_4.position, 0.0f );
if( detect->mode.jumping_stage == CONSTACTING_LEGS )
detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND;
else
{
if( detect->mode.sport_mode == JUMPING_MODE )
{
if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE_FOR_JUMP &&
detect->chassis_posture_info.leg_length_R > 0.13f )
detect->flag_info.suspend_flag_R = OFF_GROUND;
else
detect->flag_info.suspend_flag_L = ON_GROUND;
if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE_FOR_JUMP &&
detect->chassis_posture_info.leg_length_L > 0.13f )
detect->flag_info.suspend_flag_L = OFF_GROUND;
else
detect->flag_info.suspend_flag_R = ON_GROUND;
}
else
{
if( detect->chassis_posture_info.supportive_force_R <= LOWER_SUPPORT_FORCE &&
detect->chassis_posture_info.leg_length_R > 0.13f )
detect->flag_info.suspend_flag_R = OFF_GROUND;
else
detect->flag_info.suspend_flag_L = ON_GROUND;
if( detect->chassis_posture_info.supportive_force_L <= LOWER_SUPPORT_FORCE &&
detect->chassis_posture_info.leg_length_L > 0.13f )
detect->flag_info.suspend_flag_L = OFF_GROUND;
else
detect->flag_info.suspend_flag_R = ON_GROUND;
}
}
if( detect->flag_info.abnormal_flag == 1 &&
( detect->flag_info.last_suspend_flag_L == ON_GROUND || detect->flag_info.last_suspend_flag_R == ON_GROUND ) &&
( detect->flag_info.suspend_flag_L == OFF_GROUND || detect->flag_info.suspend_flag_R == OFF_GROUND ) )
detect->flag_info.Ignore_Off_Ground = 1;
else if( detect->flag_info.abnormal_flag != 1 )
detect->flag_info.Ignore_Off_Ground = 0;
if( detect->flag_info.Ignore_Off_Ground )
{
detect->flag_info.suspend_flag_R = ON_GROUND;
detect->flag_info.suspend_flag_L = ON_GROUND;
}
// detect->flag_info.suspend_flag_L = detect->flag_info.suspend_flag_R = ON_GROUND;
//Moving_High_Offset
if( ABS(detect->chassis_posture_info.foot_speed_K) > stablize_foot_speed_threshold &&
ABS(detect->chassis_posture_info.yaw_gyro ) > stablize_yaw_speed_threshold )
detect->flag_info.stablize_high_flag = 1;
}
void Chassis_Mode_Set( chassis_control_t *mode_set )
{
// ----------------- Chassis Mode Update -----------------
if( mode_set->chassis_rc_ctrl->mode_R == 0 || toe_is_error(GIMBAL_TOE)|| toe_is_error(BM1_TOE)||toe_is_error(BM2_TOE))
mode_set->mode.chassis_mode = DISABLE_CHASSIS;
else
mode_set->mode.chassis_mode = ENABLE_CHASSIS;
if( mode_set->mode.chassis_mode == ENABLE_CHASSIS )
{
mode_set->joint_motor_1.motor_mode = MOTOR_FORCE;
mode_set->joint_motor_2.motor_mode = MOTOR_FORCE;
mode_set->joint_motor_3.motor_mode = MOTOR_FORCE;
mode_set->joint_motor_4.motor_mode = MOTOR_FORCE;
mode_set->foot_motor_L.motor_mode = MOTOR_FORCE;
mode_set->foot_motor_R.motor_mode = MOTOR_FORCE;
}
else
{
if( mode_set->mode.chassis_balancing_mode == JOINT_REDUCING )
{
mode_set->joint_motor_1.motor_mode = MOTOR_FORCE;
mode_set->joint_motor_2.motor_mode = MOTOR_FORCE;
mode_set->joint_motor_3.motor_mode = MOTOR_FORCE;
mode_set->joint_motor_4.motor_mode = MOTOR_FORCE;
mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE;
mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE;
}
else
{
mode_set->joint_motor_1.motor_mode = MOTOR_NO_FORCE;
mode_set->joint_motor_2.motor_mode = MOTOR_NO_FORCE;
mode_set->joint_motor_3.motor_mode = MOTOR_NO_FORCE;
mode_set->joint_motor_4.motor_mode = MOTOR_NO_FORCE;
mode_set->foot_motor_L.motor_mode = MOTOR_NO_FORCE;
mode_set->foot_motor_R.motor_mode = MOTOR_NO_FORCE;
}
}
// ----------------- Sport Mode Update -----------------
if( mode_set->mode.chassis_balancing_mode == BALANCING_READY )
{
if( mode_set->chassis_rc_ctrl->tk_flag )
mode_set->mode.sport_mode = TK_MODE;
else if( mode_set->flag_info.abnormal_flag )
mode_set->mode.sport_mode = ABNORMAL_MOVING_MODE;
else if( mode_set->mode.sport_mode == JUMPING_MODE && mode_set->mode.jumping_stage != FINISHED )
mode_set->mode.sport_mode = JUMPING_MODE;
else if( mode_set->chassis_rc_ctrl->jump_flag )
mode_set->mode.sport_mode = JUMPING_MODE;
else if( mode_set->chassis_rc_ctrl->cap_flag )
mode_set->mode.sport_mode = CAP_MODE;
else if( mode_set->chassis_rc_ctrl->fly_flag )
mode_set->mode.sport_mode = FLY_MODE;
else
mode_set->mode.sport_mode = NORMAL_MOVING_MODE;
}
else
mode_set->mode.sport_mode = NONE;
// ----------------- Rotation Flag --------------------
static uint8_t last_rotation_flag = 0;
last_rotation_flag = mode_set->flag_info.rotation_flag;
mode_set->flag_info.rotation_flag = mode_set->chassis_rc_ctrl->mode_R == 4; //Rotate
if (!last_rotation_flag && mode_set->flag_info.rotation_flag){
for (int i = 0; i < 11; ++i)
rotate_speed_list[i] = - rotate_speed_list[i];
}
// ----------------- No Follow Flag ----------------------
if (toe_is_error(GIMBAL_TOE)) no_follow_flag = 1;
else no_follow_flag = mode_set->chassis_rc_ctrl->mode_R == 3; //No Follow
// ---------------- Moving Flag ----------------------------
if( mode_set->chassis_rc_ctrl->X_speed != 0.0f )
{
mode_set->flag_info.controlling_flag = 1;
mode_set->flag_info.set_pos_after_moving = 1;
}
else
mode_set->flag_info.controlling_flag = 0;
if( ABS(mode_set->chassis_posture_info.foot_speed_K) <= 0.05f &&
!mode_set->flag_info.controlling_flag &&
mode_set->flag_info.set_pos_after_moving ) // maybe bug
{
mode_set->flag_info.moving_flag = 0;
mode_set->flag_info.set_pos_after_moving = 0;
}
else
mode_set->flag_info.moving_flag = 1;
}
uint8_t reduce_flag = 0;
fp32 reduce_high, high_offset = 0.2f;
fp32 debug_1 = 0.995;
void Chassis_Mode_Change_Control_Transit( chassis_control_t *chassis_mode_change )
{
if( chassis_mode_change->mode.chassis_mode == ENABLE_CHASSIS && chassis_mode_change->mode.last_chassis_mode == DISABLE_CHASSIS )
chassis_mode_change->mode.chassis_balancing_mode = FOOT_LAUNCHING;
if( chassis_mode_change->mode.chassis_balancing_mode == FOOT_LAUNCHING &&
ABS(chassis_mode_change->chassis_posture_info.pitch_angle) < EXIT_PITCH_ANGLE )
chassis_mode_change->mode.chassis_balancing_mode = JOINT_LAUNCHING;
else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_LAUNCHING && (
(NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_L) < 0.03f ||
(NORMAL_HIGH - chassis_mode_change->chassis_posture_info.leg_length_R) < 0.03f) )
chassis_mode_change->mode.chassis_balancing_mode = BALANCING_READY;
else if( chassis_mode_change->mode.chassis_balancing_mode == BALANCING_READY &&
chassis_mode_change->mode.chassis_mode == DISABLE_CHASSIS ){
if (!reduce_flag) {
reduce_high = chassis_mode_change->chassis_posture_info.ideal_high + high_offset;
reduce_flag = 1;
}
chassis_mode_change->mode.chassis_balancing_mode = JOINT_REDUCING;
}
else if( chassis_mode_change->mode.chassis_balancing_mode == JOINT_REDUCING && (
ABS( chassis_mode_change->chassis_posture_info.leg_length_L - SIT_HIGH ) < 0.001f ||
ABS( chassis_mode_change->chassis_posture_info.leg_length_R - SIT_HIGH ) < 0.001f ) ){
chassis_mode_change->mode.chassis_balancing_mode = NO_FORCE;
reduce_flag = 0;
}
if( chassis_mode_change->mode.sport_mode == JUMPING_MODE && chassis_mode_change->mode.last_sport_mode != JUMPING_MODE )
{
chassis_mode_change->mode.jumping_mode = STANDING_JUMP;
}
else if( chassis_mode_change->mode.sport_mode != JUMPING_MODE )
chassis_mode_change->mode.jumping_mode = NOT_DEFINE;
if( chassis_mode_change->mode.jumping_mode == MOVING_JUMP )
{
if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP )
chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS;
else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS &&
chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f )
chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS;
else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS &&
chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f )
chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2;
else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 &&
chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f )
chassis_mode_change->mode.jumping_stage = PREPARING_LANDING;
else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING &&
chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND &&
chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND )
chassis_mode_change->mode.jumping_stage = FINISHED;
else if( chassis_mode_change->mode.jumping_stage == FINISHED )
chassis_mode_change->mode.jumping_stage = READY_TO_JUMP;
}
else if( chassis_mode_change->mode.jumping_mode == STANDING_JUMP )
{
if( chassis_mode_change->mode.jumping_stage == READY_TO_JUMP )
chassis_mode_change->mode.jumping_stage = PREPARING_STAND_JUMPING;
else if(chassis_mode_change->mode.jumping_stage == PREPARING_STAND_JUMPING &&
ABS(chassis_mode_change->chassis_posture_info.leg_angle_L + IDEAL_PREPARING_STAND_JUMPING_ANGLE ) < stepp )
chassis_mode_change->mode.jumping_stage = EXTENDING_LEGS;
else if( chassis_mode_change->mode.jumping_stage == EXTENDING_LEGS &&
chassis_mode_change->chassis_posture_info.leg_length_L >= 0.30f )
chassis_mode_change->mode.jumping_stage = CONSTACTING_LEGS_2;
else if( chassis_mode_change->mode.jumping_stage == CONSTACTING_LEGS_2 &&
chassis_mode_change->chassis_posture_info.leg_length_L <= 0.13f )
chassis_mode_change->mode.jumping_stage = PREPARING_LANDING;
else if( chassis_mode_change->mode.jumping_stage == PREPARING_LANDING &&
chassis_mode_change->flag_info.suspend_flag_R == ON_GROUND &&
chassis_mode_change->flag_info.suspend_flag_L == ON_GROUND )
chassis_mode_change->mode.jumping_stage = FINISHED;
else if( chassis_mode_change->mode.jumping_stage == FINISHED )
chassis_mode_change->mode.jumping_stage = READY_TO_JUMP;
}
else
chassis_mode_change->mode.jumping_stage = FINISHED;
if( chassis_mode_change->flag_info.stablize_high_flag == 1 &&
Moving_High_Offset >= 0.2 &&
chassis_mode_change->chassis_posture_info.yaw_gyro <= ( stablize_yaw_speed_threshold - 0.5f ) )
chassis_mode_change->flag_info.stablize_high_flag = 0;
}
fp32 HIGH_SWITCH = 36.0f;
void Target_Value_Set( chassis_control_t *target_value_set )
{
// ------------------------------------
if( target_value_set->flag_info.stablize_high_flag == 1 )
if( Moving_High_Offset < 0.2 )
Moving_High_Offset += 0.001f;
if( target_value_set->flag_info.stablize_high_flag == 0 )
Moving_High_Offset = 0.0f;
// --------- X Speed Set ----------
if( target_value_set->mode.sport_mode != NONE &&
target_value_set->flag_info.suspend_flag_L == ON_GROUND &&
target_value_set->flag_info.suspend_flag_R == ON_GROUND )
{
if (!target_value_set->flag_info.rotation_flag) {// Normal move
if(toe_is_error(REFEREE_TOE)){
target_value_set->chassis_posture_info.foot_speed_set = fake_sign * target_value_set->chassis_rc_ctrl->X_speed * normal_move_scale ;
} else {
// target_speed_sign = (fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]) > 0 ? 1: -1;
target_speed_sign = SIGN(fake_sign * target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]);
target_value_set->chassis_posture_info.foot_speed_set = target_value_set->chassis_posture_info.foot_speed_K + target_speed_sign * acc_step;
if (target_value_set->mode.sport_mode == FLY_MODE)
target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * fly_speed),ABS(target_value_set->chassis_posture_info.foot_speed_set));
else if (target_value_set->mode.sport_mode == CAP_MODE)
target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * cap_move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set));
else
target_value_set->chassis_posture_info.foot_speed_set = target_speed_sign * min(ABS(target_value_set->chassis_rc_ctrl->X_speed * move_scale_list[robot_level]),ABS(target_value_set->chassis_posture_info.foot_speed_set));
// if( target_value_set->chassis_posture_info.leg_length_L > 0.22f || target_value_set->chassis_posture_info.leg_length_R > 0.22f )
// {
// target_value_set->chassis_posture_info.foot_speed_set /= 2.0f;
// debug_2++;
// }
}
}
else // Rotate and move
{
delta_theta_temp = target_value_set->chassis_rc_ctrl->W_angle - rc_angle;
if (delta_theta_temp>PI/2) delta_theta_temp -= PI;
else if (delta_theta_temp<-PI/2) delta_theta_temp += PI;
delta_theta = abs(delta_theta_temp);
if (delta_theta<rotate_move_threshold)
target_value_set->chassis_posture_info.foot_speed_set
= ((rotate_move_threshold-delta_theta)/rotate_move_threshold)
* rc_sign * fake_sign * normalized_speed * rotate_move_scale_list[robot_level];
else target_value_set->chassis_posture_info.foot_speed_set = 0;
}
}
else
target_value_set->chassis_posture_info.foot_speed_set = 0;
// --------- Distance Set ---------
if( target_value_set->mode.chassis_balancing_mode == NO_FORCE || target_value_set->mode.sport_mode == TK_MODE)
target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
else if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE )
target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_set;
else if( target_value_set->flag_info.suspend_flag_R == OFF_GROUND ||
target_value_set->flag_info.suspend_flag_L == OFF_GROUND )
target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
else if( target_value_set->flag_info.controlling_flag )
target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
else if( !target_value_set->flag_info.moving_flag &&
target_value_set->mode.chassis_balancing_mode == BALANCING_READY &&
target_value_set->flag_info.last_moving_flag )
target_value_set->chassis_posture_info.foot_distance_set = target_value_set->chassis_posture_info.foot_distance_K;
// --------- Y Speed & Angle Set ---------
if( target_value_set->mode.sport_mode != NONE &&
target_value_set->flag_info.suspend_flag_L == ON_GROUND &&
target_value_set->flag_info.suspend_flag_R == ON_GROUND )
{
if( target_value_set->flag_info.rotation_flag == 1 )
{
target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle;
target_value_set->chassis_posture_info.yaw_gyro_set = rotate_speed_list[robot_level];
}
else
{
if (target_value_set->chassis_rc_ctrl->W_angle<-FOLLOW_DEADBAND)
target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle+FOLLOW_DEADBAND) / 180.0f * PI;
else if (target_value_set->chassis_rc_ctrl->W_angle>FOLLOW_DEADBAND)
target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle + (target_value_set->chassis_rc_ctrl->W_angle-FOLLOW_DEADBAND) / 180.0f * PI;
else
target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle;
target_value_set->chassis_posture_info.yaw_gyro_set = 0.0f;
}
}
else
{
target_value_set->chassis_posture_info.yaw_angle_sett = target_value_set->chassis_posture_info.yaw_angle;
target_value_set->chassis_posture_info.yaw_gyro_set = target_value_set->chassis_posture_info.yaw_gyro;
}
// --------- Side Angle Set ---------
// if( target_value_set->mode.sport_mode != NONE &&
// target_value_set->mode.sport_mode != ABNORMAL_MOVING_MODE &&
// target_value_set->flag_info.suspend_flag_R == ON_GROUND &&
// target_value_set->flag_info.suspend_flag_L == ON_GROUND )
// {
// if( target_value_set->mode.sport_mode == SIDE_MODE )
// target_value_set->chassis_posture_info.roll_angle_set = (fp32)(5.0 / 180.0 * PI) * 50.0f;
// // else if( target_value_set->chassis_rc_ctrl->side_flag == -1 )
// // target_value_set->chassis_posture_info.roll_angle_set = -(fp32)(5.0 / 180.0 * PI) * 50.0f;
// else
// target_value_set->chassis_posture_info.roll_angle_set = 0.0f;
// }
// else
target_value_set->chassis_posture_info.roll_angle_set = 0.0f;
// ---------------- Leg Angle Set ----------
if( target_value_set->mode.jumping_stage == PREPARING_STAND_JUMPING )
{
target_value_set->chassis_posture_info.leg_angle_L_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE;
target_value_set->chassis_posture_info.leg_angle_R_set = -IDEAL_PREPARING_STAND_JUMPING_ANGLE;
}
else
{
target_value_set->chassis_posture_info.leg_angle_L_set = 0.0f;
target_value_set->chassis_posture_info.leg_angle_R_set = 0.0f;
}
// ----------------- Chassis High Mode Update -----------------
if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE || target_value_set->mode.sport_mode == TK_MODE)
target_value_set->mode.chassis_high_mode = SIT_MODE;
else if( target_value_set->mode.chassis_balancing_mode == FOOT_LAUNCHING )
target_value_set->mode.chassis_high_mode = SIT_MODE;
else if( target_value_set->mode.chassis_balancing_mode == JOINT_LAUNCHING )
target_value_set->mode.chassis_high_mode = NORMAL_MODE;
else if( target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING )
target_value_set->mode.chassis_high_mode = CHANGING_HIGH;
else if( target_value_set->chassis_rc_ctrl->sit_flag )
target_value_set->mode.chassis_high_mode = SIT_MODE;
else if( target_value_set->chassis_rc_ctrl->high_flag )
target_value_set->mode.chassis_high_mode = HIGH_MODE;
else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS )
target_value_set->mode.chassis_high_mode = SIT_MODE;
else if( target_value_set->mode.jumping_stage == EXTENDING_LEGS )
target_value_set->mode.chassis_high_mode = HIGH_MODE;
else if( target_value_set->mode.jumping_stage == CONSTACTING_LEGS_2 )
target_value_set->mode.chassis_high_mode = SIT_MODE;
else if( target_value_set->mode.jumping_stage == PREPARING_LANDING )
target_value_set->mode.chassis_high_mode = NORMAL_MODE;
else
target_value_set->mode.chassis_high_mode = NORMAL_MODE;
// --------- Leg Length Set ---------
if( target_value_set->mode.chassis_high_mode == SIT_MODE )
target_value_set->chassis_posture_info.ideal_high = SIT_HIGH;
else if( target_value_set->mode.chassis_high_mode == NORMAL_MODE )
target_value_set->chassis_posture_info.ideal_high = NORMAL_HIGH + High_Offset - Moving_High_Offset;
else if( target_value_set->mode.chassis_high_mode == HIGH_MODE )
target_value_set->chassis_posture_info.ideal_high = HIGH_HIGH + High_Offset - Moving_High_Offset;
else if( target_value_set->mode.chassis_high_mode == CHANGING_HIGH )
{
reduce_high = reduce_high * debug_1;
target_value_set->chassis_posture_info.ideal_high = min(reduce_high,target_value_set->chassis_posture_info.ideal_high);
// target_value_set->chassis_posture_info.ideal_high =
// debug_1 * target_value_set->chassis_posture_info.ideal_high;
}
if( target_value_set->mode.sport_mode == ABNORMAL_MOVING_MODE ||
( target_value_set->flag_info.suspend_flag_L == 1 && target_value_set->flag_info.suspend_flag_R == 1 ) ||
target_value_set->chassis_posture_info.ideal_high == SIT_HIGH ||
target_value_set->mode.chassis_balancing_mode == JOINT_REDUCING )
{
target_value_set->chassis_posture_info.leg_length_L_set = target_value_set->chassis_posture_info.ideal_high;
target_value_set->chassis_posture_info.leg_length_R_set = target_value_set->chassis_posture_info.ideal_high;
}
else
{
target_value_set->chassis_posture_info.foot_roll_angle =
target_value_set->chassis_posture_info.roll_angle +
atan(( target_value_set->chassis_posture_info.leg_length_L - target_value_set->chassis_posture_info.leg_length_R ) / 0.50f );
target_value_set->chassis_posture_info.leg_length_L_set =
target_value_set->chassis_posture_info .ideal_high
+ 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle );
target_value_set->chassis_posture_info.leg_length_R_set =
target_value_set->chassis_posture_info .ideal_high
- 0.25f * arm_sin_f32( target_value_set->chassis_posture_info .foot_roll_angle ) / arm_cos_f32( target_value_set->chassis_posture_info .foot_roll_angle );
}
}
void Chassis_Torque_Calculation(chassis_control_t *bl_ctrl)
{
LQR_Data_Update(bl_ctrl);
// -----Roll Balance-----
if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 ||
bl_ctrl->mode.chassis_high_mode == SIT_MODE )
{
bl_ctrl->torque_info.joint_roll_torque_R = 0.0f;
bl_ctrl->torque_info.joint_roll_torque_L = 0.0f;
}
else
{
if (bl_ctrl->chassis_posture_info.roll_angle < -roll_angle_deadband) {
rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle+roll_angle_deadband));
}
else if (bl_ctrl->chassis_posture_info.roll_angle > roll_angle_deadband){
rollP = roll_PD[0] * (bl_ctrl->chassis_posture_info.roll_angle_set - (bl_ctrl->chassis_posture_info.roll_angle-roll_angle_deadband));
}
else {
rollP = 0.0f;
}
if (bl_ctrl->chassis_posture_info.roll_gyro < -roll_gyro_deadband) {
rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro + roll_gyro_deadband);
}
else if (bl_ctrl->chassis_posture_info.roll_gyro > roll_gyro_deadband){
rollD = roll_PD[1] * - (bl_ctrl->chassis_posture_info.roll_gyro - roll_gyro_deadband);
}
else {
rollD = 0.0f;
}
bl_ctrl->torque_info.joint_roll_torque_R = rollP + rollD;
bl_ctrl->torque_info.joint_roll_torque_L = -bl_ctrl->torque_info.joint_roll_torque_R;
}
// -----During turns: prevent displacement of two legs-----
bl_ctrl->torque_info.joint_prevent_splits_torque_L =
coordinate_PD[0] * (bl_ctrl->chassis_posture_info.leg_angle_L - bl_ctrl->chassis_posture_info.leg_angle_R)
+ coordinate_PD[1] * (bl_ctrl->chassis_posture_info.leg_gyro_L - bl_ctrl->chassis_posture_info.leg_gyro_R);
bl_ctrl->torque_info.joint_prevent_splits_torque_R = -bl_ctrl->torque_info.joint_prevent_splits_torque_L;
if( bl_ctrl->mode.jumping_stage == EXTENDING_LEGS ||
bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 )
{
bl_ctrl->torque_info.joint_stand_torque_L =
+ jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L )
+ jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L );
bl_ctrl->torque_info.joint_stand_torque_R =
+ jump_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R )
+ jump_stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R );
}
else if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE )
{
bl_ctrl->torque_info.joint_stand_torque_L =
+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L )
+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L );
bl_ctrl->torque_info.joint_stand_torque_R =
+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R )
+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R );
}
else if( bl_ctrl->flag_info.suspend_flag_R == 1 || bl_ctrl->flag_info.suspend_flag_L == 1 )
{
if( bl_ctrl->flag_info.suspend_flag_L == 1 )
{
bl_ctrl->torque_info.joint_stand_torque_L =
+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L )
+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L );
}
else{
bl_ctrl->torque_info.joint_stand_torque_L =
FEED_f
+ stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L )
+ stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_L );
}
if( bl_ctrl->flag_info.suspend_flag_R == 1 )
{
bl_ctrl->torque_info.joint_stand_torque_R =
+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R )
+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R );
} else {
bl_ctrl->torque_info.joint_stand_torque_R =
FEED_f
+ stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R )
+ stand_PD[1] * ( 0 - bl_ctrl->chassis_posture_info.leg_dlength_R );
}
}
else if( bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING )
{
bl_ctrl->torque_info.joint_stand_torque_L =
+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L )
+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_L );
bl_ctrl->torque_info.joint_stand_torque_R =
+ suspend_stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R )
+ suspend_stand_PD[1] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_dlength_R );
}
else
{
bl_ctrl->torque_info.joint_stand_torque_L = FEED_f;
bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_L_set - bl_ctrl->chassis_posture_info.leg_length_L );
if (bl_ctrl->chassis_posture_info.leg_dlength_L > leg_dlength_deadband){
bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L-leg_dlength_deadband) );
}
else if (bl_ctrl->chassis_posture_info.leg_dlength_L < -leg_dlength_deadband){
bl_ctrl->torque_info.joint_stand_torque_L += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_L+leg_dlength_deadband));
}
else {
bl_ctrl->torque_info.joint_stand_torque_L += 0.0f;
}
bl_ctrl->torque_info.joint_stand_torque_R = FEED_f;
bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[0] * ( bl_ctrl->chassis_posture_info.leg_length_R_set - bl_ctrl->chassis_posture_info.leg_length_R );
if (bl_ctrl->chassis_posture_info.leg_dlength_R > leg_dlength_deadband){
bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R-leg_dlength_deadband) );
}
else if (bl_ctrl->chassis_posture_info.leg_dlength_R < -leg_dlength_deadband){
bl_ctrl->torque_info.joint_stand_torque_R += stand_PD[1] * ( 0 - (bl_ctrl->chassis_posture_info.leg_dlength_R+leg_dlength_deadband));
}
else {
bl_ctrl->torque_info.joint_stand_torque_R += 0.0f;
}
}
// --------------------- joint motor calucaltion ---------------------
// TODDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDDO
if( bl_ctrl->mode.jumping_stage == CONSTACTING_LEGS_2 )
{
bl_ctrl->torque_info.joint_balancing_torque_L =
-suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_L )
-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L );
bl_ctrl->torque_info.joint_balancing_torque_R =
-suspend_LQR[0][0] * ( 0 - bl_ctrl->chassis_posture_info.leg_angle_R )
-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R );
bl_ctrl->torque_info.joint_moving_torque_L = 0.0f;
bl_ctrl->torque_info.joint_moving_torque_R = 0.0f;
}
else
{
if( bl_ctrl->mode.sport_mode == ABNORMAL_MOVING_MODE || bl_ctrl->mode.sport_mode == TK_MODE )
{
bl_ctrl->torque_info.joint_balancing_torque_L =
bl_ctrl->torque_info.joint_moving_torque_L =
bl_ctrl->torque_info.joint_balancing_torque_R =
bl_ctrl->torque_info.joint_moving_torque_R = 0;
}
else
{
if( bl_ctrl->mode.chassis_high_mode == SIT_MODE ||
bl_ctrl->mode.chassis_balancing_mode == JOINT_REDUCING )
{
bl_ctrl->torque_info.joint_balancing_torque_L =
bl_ctrl->torque_info.joint_moving_torque_L =
bl_ctrl->torque_info.joint_balancing_torque_R =
bl_ctrl->torque_info.joint_moving_torque_R = 0;
}
else
{
bl_ctrl->torque_info.joint_balancing_torque_L = (
-LQR[2][4] * ( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L )
-LQR[2][5] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L )
-LQR[2][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle )
-LQR[2][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) );
bl_ctrl->torque_info.joint_moving_torque_L = (
+LQR[2][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
+LQR[2][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K )
+LQR[2][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle )
+LQR[2][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro )
);
bl_ctrl->torque_info.joint_balancing_torque_R = (
-LQR[3][6] * ( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R )
-LQR[3][7] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R )
-LQR[3][8] * ( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle )
-LQR[3][9] * ( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro ) );
bl_ctrl->torque_info.joint_moving_torque_R = (
+LQR[3][0] * ( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
+LQR[3][1] * ( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K )
+LQR[3][2] * ( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle )
+LQR[3][3] * ( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro )
);
}
}
}
// --------------------- Foot motor LQR ---------------------
if ( bl_ctrl->mode.sport_mode == TK_MODE ) {
bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f;
bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f;
bl_ctrl->torque_info.foot_moving_torque_L = (int) (
-TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K )
-TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle )
-TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro )
)* TORQ_K;
bl_ctrl->torque_info.foot_moving_torque_R = (int)-(
-TK_x_p*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_speed_K )
+TK_y_p*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.yaw_angle )
+TK_y_d*( 0.0f - bl_ctrl->chassis_posture_info.yaw_gyro )
)* TORQ_K;
}
else {
bl_ctrl->torque_info.foot_balancing_torque_L = (int) (
-LQR[0][4]*( bl_ctrl->chassis_posture_info.leg_angle_L_set - bl_ctrl->chassis_posture_info.leg_angle_L)
-LQR[0][5]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_L)
+LQR[0][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle)
+LQR[0][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro)
)* TORQ_K;
bl_ctrl->torque_info.foot_moving_torque_L = (int) (
-LQR[0][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
-LQR[0][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K)
-LQR[0][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle)
-LQR[0][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro )
)* TORQ_K;
bl_ctrl->torque_info.foot_balancing_torque_R = (int) -(
-LQR[1][6]*( bl_ctrl->chassis_posture_info.leg_angle_R_set - bl_ctrl->chassis_posture_info.leg_angle_R)
-LQR[1][7]*( 0.0f - bl_ctrl->chassis_posture_info.leg_gyro_R)
+LQR[1][8]*( bl_ctrl->chassis_posture_info.pitch_angle_set - bl_ctrl->chassis_posture_info.pitch_angle)
+LQR[1][9]*( bl_ctrl->chassis_posture_info.pitch_gyro_set - bl_ctrl->chassis_posture_info.pitch_gyro)
)* TORQ_K;
bl_ctrl->torque_info.foot_moving_torque_R = (int) -(
-LQR[1][0]*( bl_ctrl->chassis_posture_info.foot_distance_set - bl_ctrl->chassis_posture_info.foot_distance_K + NORMAL_MODE_WEIGHT_DISTANCE_OFFSET)
-LQR[1][1]*( bl_ctrl->chassis_posture_info.foot_speed_set - bl_ctrl->chassis_posture_info.foot_speed_K)
+LQR[1][2]*( bl_ctrl->chassis_posture_info.yaw_angle_sett - bl_ctrl->chassis_posture_info.yaw_angle)
+LQR[1][3]*( bl_ctrl->chassis_posture_info.yaw_gyro_set - bl_ctrl->chassis_posture_info.yaw_gyro )
)* TORQ_K;
}
if( bl_ctrl->flag_info.suspend_flag_R == 1 )
{
bl_ctrl->torque_info.joint_balancing_torque_R =
-suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_R )
-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_R );
bl_ctrl->torque_info.foot_moving_torque_R =
-suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_R.speed );
bl_ctrl->torque_info.joint_moving_torque_R = 0.0f;
bl_ctrl->torque_info.foot_balancing_torque_R = 0.0f;
}
if( bl_ctrl->flag_info.suspend_flag_L == 1 )
{
bl_ctrl->torque_info.joint_balancing_torque_L =
-suspend_LQR[0][0] * ( 0.0f - bl_ctrl->chassis_posture_info.leg_angle_L )
-suspend_LQR[0][1] * ( 0 - bl_ctrl->chassis_posture_info.leg_gyro_L );
bl_ctrl->torque_info.foot_moving_torque_L =
+suspend_foot_speed_p * ( 0.0f - bl_ctrl->foot_motor_L.speed );
bl_ctrl->torque_info.joint_moving_torque_L = 0.0f;
bl_ctrl->torque_info.foot_balancing_torque_L = 0.0f;
}
// LimitMax( bl_ctrl->torque_info.foot_moving_torque_L, MAX_ACCL );
// LimitMax( bl_ctrl->torque_info.foot_moving_torque_R, MAX_ACCL );
LimitMax( bl_ctrl->torque_info.joint_moving_torque_L, MAX_ACCL_JOINT );
LimitMax( bl_ctrl->torque_info.joint_moving_torque_R, MAX_ACCL_JOINT );
}
void Chassis_Torque_Combine(chassis_control_t *bl_ctrl)
{
bl_ctrl->mapping_info .J1_L = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L );
bl_ctrl->mapping_info .J2_L = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L);
bl_ctrl->mapping_info .J3_L = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L);
bl_ctrl->mapping_info .J4_L = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_L ,bl_ctrl->chassis_posture_info .leg_length_L );
bl_ctrl->mapping_info .J1_R = VMC_solve_J1(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R );
bl_ctrl->mapping_info .J2_R = VMC_solve_J2(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R);
bl_ctrl->mapping_info .J3_R = VMC_solve_J3(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R);
bl_ctrl->mapping_info .J4_R = VMC_solve_J4(bl_ctrl->chassis_posture_info .leg_angle_R ,bl_ctrl->chassis_posture_info .leg_length_R );
bl_ctrl->torque_info.foot_horizontal_torque_L =
bl_ctrl->torque_info.foot_balancing_torque_L + bl_ctrl->torque_info.foot_moving_torque_L;
bl_ctrl->torque_info.foot_horizontal_torque_R =
bl_ctrl->torque_info.foot_balancing_torque_R + bl_ctrl->torque_info.foot_moving_torque_R;
bl_ctrl->foot_motor_L.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_L;
bl_ctrl->foot_motor_R.torque_out = bl_ctrl->torque_info.foot_horizontal_torque_R;
LimitMax(bl_ctrl->foot_motor_L.torque_out,16383);
LimitMax(bl_ctrl->foot_motor_R.torque_out,16383);
bl_ctrl->torque_info.joint_horizontal_torque_L =
bl_ctrl->torque_info.joint_balancing_torque_L + bl_ctrl->torque_info.joint_moving_torque_L + bl_ctrl->torque_info.joint_prevent_splits_torque_L;
bl_ctrl->torque_info.joint_horizontal_torque_R =
bl_ctrl->torque_info.joint_balancing_torque_R + bl_ctrl->torque_info.joint_moving_torque_R + bl_ctrl->torque_info.joint_prevent_splits_torque_R;
bl_ctrl->torque_info.joint_vertical_torque_L =
bl_ctrl->torque_info.joint_stand_torque_L + bl_ctrl->torque_info.joint_roll_torque_L;
bl_ctrl->torque_info.joint_vertical_torque_R =
bl_ctrl->torque_info.joint_stand_torque_R + bl_ctrl->torque_info.joint_roll_torque_R;
bl_ctrl->torque_info.joint_horizontal_torque_temp1_L =
(bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J3_L ;
bl_ctrl->torque_info.joint_horizontal_torque_temp2_L =
(bl_ctrl->torque_info.joint_horizontal_torque_L) * bl_ctrl->mapping_info .J4_L ;
bl_ctrl->torque_info.joint_horizontal_torque_temp1_R =
(bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J3_R ;
bl_ctrl->torque_info.joint_horizontal_torque_temp2_R =
(bl_ctrl->torque_info.joint_horizontal_torque_R) * bl_ctrl->mapping_info .J4_R ;
bl_ctrl->torque_info.joint_vertical_torque_temp1_L =
(bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J1_L;
bl_ctrl->torque_info.joint_vertical_torque_temp2_L =
(bl_ctrl->torque_info.joint_vertical_torque_L) * bl_ctrl->mapping_info .J2_L;
bl_ctrl->torque_info.joint_vertical_torque_temp1_R =
(bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J1_R;
bl_ctrl->torque_info.joint_vertical_torque_temp2_R =
(bl_ctrl->torque_info.joint_vertical_torque_R) * bl_ctrl->mapping_info .J2_R;
/****************************************/
fp32 MAX_balance = 2000.0f;
LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_L,MAX_balance);
LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_L,MAX_balance);
LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp1_R,MAX_balance);
LimitMax(bl_ctrl->torque_info.joint_horizontal_torque_temp2_R,MAX_balance);
LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_L,15);
LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_L,15);
LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp1_R,15);
LimitMax(bl_ctrl->torque_info.joint_vertical_torque_temp2_R,15);
// 分配到四<E588B0>?关节电机
bl_ctrl->joint_motor_1.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp1_L + bl_ctrl->torque_info.joint_vertical_torque_temp1_L;
bl_ctrl->joint_motor_2.torque_out = + bl_ctrl->torque_info.joint_horizontal_torque_temp2_L + bl_ctrl->torque_info.joint_vertical_torque_temp2_L;
bl_ctrl->joint_motor_3.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp1_R + bl_ctrl->torque_info.joint_vertical_torque_temp1_R;
bl_ctrl->joint_motor_4.torque_out = - bl_ctrl->torque_info.joint_horizontal_torque_temp2_R + bl_ctrl->torque_info.joint_vertical_torque_temp2_R;
if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
{
bl_ctrl->joint_motor_1.max_torque = LIMITED_TORQUE;
bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE;
}
else if( ABS(bl_ctrl->joint_motor_1.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
{
bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_1.min_torque = -1.0f * LIMITED_TORQUE;
}
else
{
bl_ctrl->joint_motor_1.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_1.min_torque = -1.0f * UNLIMITED_TORQUE;
}
if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
{
bl_ctrl->joint_motor_3.max_torque = LIMITED_TORQUE;
bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE;
}
else if( ABS(bl_ctrl->joint_motor_3.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
{
bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_3.min_torque = -1.0f * LIMITED_TORQUE;
}
else
{
bl_ctrl->joint_motor_3.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_3.min_torque = -1.0f * UNLIMITED_TORQUE;
}
if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
{
bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_2.min_torque = -1.0f * LIMITED_TORQUE;
}
else if( ABS(bl_ctrl->joint_motor_2.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
{
bl_ctrl->joint_motor_2.max_torque = LIMITED_TORQUE;
bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE;
}
else
{
bl_ctrl->joint_motor_2.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_2.min_torque = -1.0f * UNLIMITED_TORQUE;
}
if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) <= MOTOR_POS_UPPER_BOUND )
{
bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_4.min_torque = -1.0f * LIMITED_TORQUE;
}
else if( ABS(bl_ctrl->joint_motor_4.motor_measure->ecd) >= MOTOR_POS_LOWER_BOUND )
{
bl_ctrl->joint_motor_4.max_torque = LIMITED_TORQUE;
bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE;
}
else
{
bl_ctrl->joint_motor_4.max_torque = UNLIMITED_TORQUE;
bl_ctrl->joint_motor_4.min_torque = -1.0f * UNLIMITED_TORQUE;
}
LimitOutput( bl_ctrl->joint_motor_1.torque_out, bl_ctrl->joint_motor_1.min_torque, bl_ctrl->joint_motor_1.max_torque);
LimitOutput( bl_ctrl->joint_motor_2.torque_out, bl_ctrl->joint_motor_2.min_torque, bl_ctrl->joint_motor_2.max_torque);
LimitOutput( bl_ctrl->joint_motor_3.torque_out, bl_ctrl->joint_motor_3.min_torque, bl_ctrl->joint_motor_3.max_torque);
LimitOutput( bl_ctrl->joint_motor_4.torque_out, bl_ctrl->joint_motor_4.min_torque, bl_ctrl->joint_motor_4.max_torque);
}
void Motor_CMD_Send(chassis_control_t *CMD_Send)
{
if( CMD_Send->joint_motor_1.motor_mode == MOTOR_FORCE )
CAN_HT_CMD( 0x01, CMD_Send->joint_motor_1.torque_out );
else
CAN_HT_CMD( 0x01, 0.0 );
if( CMD_Send->joint_motor_2.motor_mode == MOTOR_FORCE )
CAN_HT_CMD( 0x02, CMD_Send->joint_motor_2.torque_out );
else
CAN_HT_CMD( 0x02, 0.0 );
if( CMD_Send->foot_motor_R.motor_mode != MOTOR_FORCE )
CMD_Send->foot_motor_R .torque_out = 0.0f;
if( CMD_Send->foot_motor_L.motor_mode != MOTOR_FORCE )
CMD_Send->foot_motor_L .torque_out = 0.0f;
CAN_BM_CONTROL_CMD( CMD_Send->foot_motor_L.torque_out, CMD_Send->foot_motor_R.torque_out );
vTaskDelay(1);
if( CMD_Send->joint_motor_3.motor_mode == MOTOR_FORCE )
CAN_HT_CMD( 0x03, CMD_Send->joint_motor_3.torque_out );
else
CAN_HT_CMD( 0x03, 0.0 );
if( CMD_Send->joint_motor_4.motor_mode == MOTOR_FORCE )
CAN_HT_CMD( 0x04, CMD_Send->joint_motor_4.torque_out );
else
CAN_HT_CMD( 0x04, 0.0 );
}
void Joint_Motor_to_Init_Pos()
{
int Init_Time = 0;
while( Init_Time < 300 )
{
CAN_HT_CMD( 0x01, 1.0 );
CAN_HT_CMD( 0x02, -1.0 );
CAN_HT_CMD( 0x03, 1.0 );
vTaskDelay(1);
CAN_HT_CMD( 0x04, -1.0 );
vTaskDelay(1);
Init_Time++;
}
}
void HT_Motor_zero_set(void)
{
uint8_t tx_buff[8];
for( int i = 0; i < 7; i++ )
tx_buff[i] = 0xFF;
tx_buff[7] = 0xfc;
// ENABLE_CHASSIS
CAN_CMD_HT_Enable( 0x01, tx_buff );
vTaskDelay(50);
CAN_CMD_HT_Enable( 0x02, tx_buff );
vTaskDelay(50);
CAN_CMD_HT_Enable( 0x03, tx_buff );
vTaskDelay(50);
CAN_CMD_HT_Enable( 0x04, tx_buff );
vTaskDelay(50);
// <20>?到限<E588B0>?
Joint_Motor_to_Init_Pos();
// Set zero init point
tx_buff[7] = 0xfe;
CAN_CMD_HT_Enable( 0x01, tx_buff );
vTaskDelay(50);
CAN_CMD_HT_Enable( 0x02, tx_buff );
vTaskDelay(50);
CAN_CMD_HT_Enable( 0x03, tx_buff );
vTaskDelay(50);
CAN_CMD_HT_Enable( 0x04, tx_buff );
vTaskDelay(50);
}
void Motor_Zero_CMD_Send(void)
{
CAN_HT_CMD( 0x01, 0.0 );
vTaskDelay(1);
CAN_HT_CMD( 0x02, 0.0 );
vTaskDelay(1);
CAN_HT_CMD( 0x03, 0.0 );
vTaskDelay(1);
CAN_HT_CMD( 0x04, 0.0 );
vTaskDelay(1);
// CAN_BM_ENABLE_CMD();
// vTaskDelay(1);
// CAN_BM_MODE_SET_CMD();
// vTaskDelay(2);
// CAN_BM_CURRENT_MODE_CMD();
// vTaskDelay(1);
}
Reference in New Issue View Git Blame Copy Permalink