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

3
.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,3 @@
{
"cmake.sourceDirectory": "E:/liumin/1car/e3_176_ref/middleware/mbedtls/3rdparty"
}

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("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.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("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调用一次

2
boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c
View File

@@ -1394,7 +1394,7 @@ void canInterfaceInit(void)
// un_motor_output2.bit_data.set_torque = 30000;
// un_motor_output2.bit_data.set_rotation_speed = 30000;
// un_motor_output2.bit_data.mode = 0;
// can_fault_info.bit_data.navigator_state = 1;
can_fault_info.bit_data.navigator_state = 1;
memset(&un_inf_can_kgf_output1, 0, sizeof(UnInfCanKGFOutput));
memset(&un_inf_can_kgf_output2, 0, sizeof(UnInfCanKGFOutput));

21
boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c
View File

@@ -235,6 +235,13 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad
{
un_manual_computer_input.arr[i] = buf[i];
}
udp_temp = ((un_manual_computer_input.bit_data.set_speed << 8) | (un_manual_computer_input.bit_data.set_speed >> 8));//<2F>趨<EFBFBD>ٶ<EFBFBD> 20240928 <20>޸ĸߵ<C4B8>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
un_manual_computer_input.bit_data.set_speed = udp_temp;
udp_temp = ((un_manual_computer_input.bit_data.set_curvature << 8) | (un_manual_computer_input.bit_data.set_curvature >> 8));//<2F>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD>
un_manual_computer_input.bit_data.set_curvature = udp_temp;
publishMessage(&un_manual_computer_input, 1);
// p->len = len;
@@ -574,12 +581,26 @@ void udp_Callback_3(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad
static unsigned int received_size = 0;
uint8_t *buf = (uint8_t *)(p->payload);
uint8_t ip_addr[4] = {0,0,0,0};
//------------------------------------------------------------------------------------------------
//20250822 <20><><EFBFBD>ӱ<EFBFBD><D3B1><EFBFBD>ˢд<CBA2><D0B4>λ<EFBFBD><CEBB>IP
ip_addr[0] = addr->addr & 0xff; /* IADDR4 */
ip_addr[1] = (addr->addr >> 8) & 0xff; /* IADDR3 */
ip_addr[2] = (addr->addr >> 16) & 0xff; /* IADDR2 */
ip_addr[3] = (addr->addr >> 24) & 0xff; /* IADDR1 */
// <20><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>µ<EFBFBD>֡ͷ
if ((0x80 == buf[1]) && (0xFF == buf[0]))
{
// <20><><EFBFBD>ý<EFBFBD><C3BD>չ<EFBFBD><D5B9><EFBFBD>
received_size = 0;
printf("<EFBFBD><EFBFBD><EFBFBD>⵽<EFBFBD>µ<EFBFBD>֡ͷ,<2C><><EFBFBD>ý<EFBFBD><C3BD>չ<EFBFBD><D5B9><EFBFBD>\n");
printf("IP<EFBFBD><EFBFBD>ַ: %d.%d.%d.%d\n", ip_addr[0], ip_addr[1], ip_addr[2], ip_addr[3]);
}
// <20><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>㹻<EFBFBD>Ŀռ<C4BF><D5BC><EFBFBD><EFBFBD>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>