liumin/1CAR
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

修改导航仪错误解决自动模式刹车问题

Browse Source
This commit is contained in:
liumin
2025-11-03 06:34:55 +08:00
parent 3134504178
commit bb13f0c06d
5 changed files with 47 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c
View File

@@ -304,7 +304,7 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
}
else{}
printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
// printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
motor_speed[0] = left_speed_mps;
motor_speed[2] = left_speed_mps;
@@ -312,9 +312,9 @@ void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max
motor_speed[1] = right_speed_mps;
motor_speed[3] = right_speed_mps;
adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);
adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],20, 5);
adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],20, 5);
//
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
@@ -572,7 +572,7 @@ static void diffInput(void *signal_id)
diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
// 单位转换
diff_data.desired_speed = -diff_data.desired_speed * 0.01f;
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);
@@ -605,7 +605,7 @@ static void diffInput(void *signal_id)
diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature));
// 单位转换
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;// 20241016 增加转弯反相
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;//
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5);
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
@@ -627,7 +627,7 @@ static void diffInput(void *signal_id)
}
// diff_data.right_motor_speed = LOWPASS_FILTER(motor_speed_temp,right_speed_fiter[0]);
diff_data.right_motor_speed = motor_speed_temp;
diff_data.right_motor_speed = motor_speed_temp;//
// right_speed_fiter[0] = diff_data.right_motor_speed;
@@ -643,7 +643,7 @@ static void diffInput(void *signal_id)
else if (signal_id == &un_motor_input2)// 处理第二个电机速度信号（右电机）
{
diff_data.left_front_motor_speed = -(float)( (int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) ) /6.0; // 20250502 1号控制器增加反相
diff_data.left_rear_motor_speed = (float)( (int16_t)un_motor_input2.bit_data.MotCon_1Signal3 ) / 6.0;
diff_data.left_rear_motor_speed = (float)( (int16_t) (un_motor_input2.bit_data.MotCon_1Signal3) ) / 6.0;
if(fabs(diff_data.left_front_motor_speed) > fabs(diff_data.left_rear_motor_speed))//取速度较小的轮速
{
@@ -654,7 +654,7 @@ static void diffInput(void *signal_id)
motor_speed_temp = diff_data.left_front_motor_speed;
}
diff_data.left_motor_speed = motor_speed_temp;
diff_data.left_motor_speed = motor_speed_temp; //
// diff_data.left_motor_speed = LOWPASS_FILTER(motor_speed_temp,left_speed_fiter[0]);//低通滤波器
// left_speed_fiter[0] = diff_data.left_motor_speed;
@@ -762,15 +762,15 @@ void diffParametersInit(void *signal_id)
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
// printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
// printf("left_motor_speed = %f\n",diff_data.left_motor_speed);
// printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
printf("left_motor_speed = %f\n",diff_data.left_motor_speed);
printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);
printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);
float deffspeed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
@@ -778,7 +778,7 @@ void diffParametersInit(void *signal_id)
deffspeed = deffspeed * 0.01f;
deffcurvature = deffcurvature * 0.0001f;
// printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);
printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);
timerStart(&diff_app_timer,1000,1);//1s调用一次
}

12
boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c
View File

@@ -376,12 +376,12 @@ void turnableParametersInit(void *signal_id)
turnable_data.min_pitch_postion = getParam("minYpos"); //俯仰位置最小限制值
turnable_data.max_pitch_postion = getParam("maxYpos"); //俯仰位置最大限制值
printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere);
printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere);
printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere);
printf( "desired speed %f\n",turnable_data.desired_speed);
printf( "speed %f\n",turnable_data.speed);
printf( "turnable state %d\n",turnable_data.turnable_state);
// printf( "turnable left A %f\n",turnable_data.out_left_motor_ampere);
// printf( "turnable right A %f\n",turnable_data.out_right_motor_ampere);
// printf( "turnable pitch A %f\n",turnable_data.out_pitch_motor_ampere);
// printf( "desired speed %f\n",turnable_data.desired_speed);
// printf( "speed %f\n",turnable_data.speed);
// printf( "turnable state %d\n",turnable_data.turnable_state);
timerStart(&turnable_data.turnable_timer1,1000,1);//1s调用一次