diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c index 2bb7d04..2c52e02 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.c @@ -19,6 +19,9 @@ DiffData diff_data; PID_t speed_pid; PID_t yaw_rate_pid; +PID_t Acc_front_speed_pid; +PID_t Dec_front_speed_pid; + // 设置电机输出 void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power) { @@ -36,7 +39,7 @@ void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint // 设置左右电机期望转速 un_motor_output1.bit_data.MotCon_1Signal3 = (uint16_t)(-abs_right_front_speed);//20250502方向原因，需要把1号电机控制器的左右电机反相 - un_motor_output2.bit_data.MotCon_1Signal4 = (uint16_t)abs_right_rear_speed; + un_motor_output2.bit_data.MotCon_1Signal4 = (uint16_t)(-abs_right_rear_speed); un_motor_output1.bit_data.MotCon_1Signal4 = (uint16_t)(-abs_left_front_speed); un_motor_output2.bit_data.MotCon_1Signal3 = (uint16_t)abs_left_rear_speed; @@ -312,8 +315,8 @@ 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],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); +// 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); // // 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); @@ -484,11 +487,67 @@ static void diffProcess(void *signal_id) // } // } - + float out_torque[4] = {0,0,0,0}; // 使用差速车辆动力学模型计算左右电机的期望速度 - computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]); + computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &out_torque[0]); + + if( fabs(diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 + { + diff_data.left_speed_diff = diff_data.left_front_motor_speed - diff_data.left_rear_motor_speed; + diff_data.left_diff_touue = calculatePidOutput(&Acc_front_speed_pid, 0.0f, diff_data.left_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.left_speed_diff = 0; + Acc_front_speed_pid.integral = 0; + diff_data.left_diff_touue = 0; + } + if( fabs(diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed) >= diff_data.diff_dead_zone )//如果超过系数 + { + diff_data.right_speed_diff = diff_data.right_front_motor_speed - diff_data.right_rear_motor_speed; + diff_data.right_diff_touue = calculatePidOutput(&Dec_front_speed_pid, 0.0f, diff_data.right_speed_diff, 0.0f, dt); //左侧转速差PID + } + else + { + diff_data.right_speed_diff = 0; + Dec_front_speed_pid.integral = 0; + diff_data.right_diff_touue = 0; + } + + + if(out_torque[0] > 0)//根据大小来限定值为分配扭矩。最小就是0扭矩。 + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, -out_torque[0], out_torque[0]); + } + else + { + diff_data.left_diff_touue = constrain(diff_data.left_diff_touue, out_torque[0], -out_torque[0]); + } + + + if(out_torque[1] > 0) + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, -out_torque[1], out_torque[1]); + } + else + { + diff_data.right_diff_touue = constrain(diff_data.right_diff_touue, out_torque[1], -out_torque[1]); + } + + + diff_data.out_torq[0] = (out_torque[0] + diff_data.left_diff_touue);//因为每一个电机都是相同的扭矩，所以扭矩和为2倍。 + diff_data.out_torq[2] = (out_torque[0] - diff_data.left_diff_touue); + + diff_data.out_torq[1] = (out_torque[1] + diff_data.right_diff_touue); + diff_data.out_torq[3] = (out_torque[1] - diff_data.right_diff_touue); + + out_torque[0] = constrain(out_torque[0], -diff_data.max_Torq, diff_data.max_Torq); //限定最大扭矩 + out_torque[1] = constrain(out_torque[1], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[2] = constrain(out_torque[2], -diff_data.max_Torq, diff_data.max_Torq); + out_torque[3] = constrain(out_torque[3], -diff_data.max_Torq, diff_data.max_Torq); + @@ -596,8 +655,8 @@ static void diffInput(void *signal_id) 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.2, 2, 10, 1, 5);//20250320 修改死区为0.2解决停不住的问题 - diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); + 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); } else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) ) { @@ -608,13 +667,13 @@ static void diffInput(void *signal_id) 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); +// diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1); } else if (signal_id == &un_motor_input1)// 处理第一个电机速度信号（左电机） { diff_data.right_front_motor_speed = -(float)( (int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) ) / 6.0; // 20250502 1号控制器增加反相 - diff_data.right_rear_motor_speed = (float)( (int16_t)(un_motor_input2.bit_data.MotCon_1Signal4) ) /6.0; + diff_data.right_rear_motor_speed = -(float)( (int16_t)(un_motor_input2.bit_data.MotCon_1Signal4) ) /6.0; // motor_speed_temp = (motor_speed_temp + (int16_t)un_motor_input2.bit_data.MotCon_1Signal4)/2/6; if(fabs(diff_data.right_rear_motor_speed) > fabs(diff_data.right_front_motor_speed))//取速度较小的轮速 @@ -642,7 +701,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_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; if(fabs(diff_data.left_front_motor_speed) > fabs(diff_data.left_rear_motor_speed))//取速度较小的轮速 @@ -759,11 +818,41 @@ void diffParametersInit(void *signal_id) getParam("crv_ol") ); } + + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 设置曲率 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + + if(0 == (float)getParam("diff_sp"))//20250711 防止参数为0，影响计算。 + { + diff_data.diff_dead_zone = 2; + } + else + { + diff_data.diff_dead_zone = (float)getParam("diff_sp");//参数读取设定最大扭矩 + } + + + 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("left_motor_speed = %f\n",diff_data.left_motor_speed); @@ -778,7 +867,17 @@ 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); + + + deffspeed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed)); + deffcurvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature)); + // 单位转换 + deffspeed = deffspeed * 0.01f; + deffcurvature = deffcurvature * 0.0001f; + + printf("manual speed = %f, manual curvature = %f\n", deffspeed, deffcurvature); + timerStart(&diff_app_timer,1000,1);//1s调用一次 } @@ -823,6 +922,28 @@ void diffAppInit(void) getParam("crv_ol") ); + // 初始化减速 PID 控制器 + initializePid(&Dec_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置 PID 控制器的参数 + setPidParameters(&Dec_front_speed_pid, + getParam("mot_kp"), + getParam("mot_ki"), + getParam("mot_kd"), + getParam("mot_il"), + getParam("mot_ol") + ); + + // 初始化加速 PID 控制器 + initializePid(&Acc_front_speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f); + // 设置 PID 控制器的参数 + setPidParameters(&Acc_front_speed_pid, + Dec_front_speed_pid.kp, + Dec_front_speed_pid.ki, + Dec_front_speed_pid.kd, + Dec_front_speed_pid.integral_limit, + Dec_front_speed_pid.output_limit + ); + subscribe(&diff_app_timer, diffParametersInit); timerStart(&diff_app_timer,1000,1);//1s调用一次 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h index 568996f..dd6451c 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_differential_drive.h @@ -55,7 +55,15 @@ typedef struct DiffData float out_right_motor_speed; // 输出右电机速度 float out_torq[4]; //4个电机扭矩 float max_Torq; // 最大扭矩限制 - float min_Torq; // 最小扭矩限制 + float min_Torq; // 最小扭矩限制 + + float left_speed_diff; // 左侧转速差 + float right_speed_diff; // 右侧转速差 + + float left_diff_touue; // 左侧扭矩差 + float right_diff_touue; // 右侧扭矩差 + float diff_dead_zone; // 差速速度死区 + } DiffData; diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.c index b7950d6..aa3b97c 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.c @@ -265,9 +265,9 @@ void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, for (int i = 0; i < 256; ++i) { if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) { writeParameter(paramRequest->bit_data.param_name[i], paramRequest->bit_data.data[i]); - printf("paramRequest->bit_data.param_name[i]:%s \n",paramRequest->bit_data.param_name[i]); + //printf("paramRequest->bit_data.param_name[i]:%s \n",paramRequest->bit_data.param_name[i]); memcpy(&value, paramRequest->bit_data.data[i], sizeof(float)); - printf("paramRequest->bit_data.data[i]:%f \n", value); + //printf("paramRequest->bit_data.data[i]:%f \n", value); } } diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h index 695cec8..9f9bf6d 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_param_manage.h @@ -54,6 +54,13 @@ extern "C" X(minTorq) \ X(minYpos) \ X(maxYpos) \ + X(mot_kp) \ + X(mot_ki) \ + X(mot_kd) \ + X(mot_il) \ + X(mot_ol) \ + X(diff_sp) \ + X(turn_sp) \ X(test) // 定义一个包含所有参数名称的结构体 diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c index c3f57e3..97c7864 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.c @@ -265,7 +265,7 @@ static void turnableProcess(void *signal_id) switch(turnable_data.turnable_state)//先发送切换模式以及电机失能，后面直接使能 最后发送数据 { case 0: - timerStart(&turnable_data.turnable_timer, 1000, 1); // 启动定时器，1s + timerStart(&turnable_data.turnable_timer, 3000, 1); // 启动定时器，1s turnable_data.turnable_state = 1; break; @@ -376,10 +376,10 @@ 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( "turnable left %f\n",turnable_data.out_left_motor_ampere); + printf( "turnable right %f\n",turnable_data.out_right_motor_ampere); + printf( "turnable pitch %f\n",turnable_data.out_pitch_motor_ampere); + printf( "turnable x_axis %d\n",un_remote_control_input.bit_data.x_axis); // printf( "speed %f\n",turnable_data.speed); // printf( "turnable state %d\n",turnable_data.turnable_state); @@ -416,7 +416,7 @@ static void turnableInput(void *signal_id) float x_axis_temp = (float)(un_remote_control_input.bit_data.x_axis) - REMOTE_ZERO; if( ( x_axis_temp > 50 ) || ( x_axis_temp < -50 ) ) { - turnable_data.out_left_motor_ampere = 0.02*(x_axis_temp);//计算电流 + turnable_data.out_left_motor_ampere = 0.01*(x_axis_temp);//计算电流 turnable_data.out_right_motor_ampere = turnable_data.out_left_motor_ampere; } else @@ -428,12 +428,12 @@ static void turnableInput(void *signal_id) x_axis_temp = (float)(un_remote_control_input.bit_data.y_axis) - REMOTE_ZERO; if(x_axis_temp > 50) //根据Y轴数据来定义 { - turnable_data.out_pitch_motor_ampere = 0.01*fabs(x_axis_temp); + turnable_data.out_pitch_motor_ampere = 0.02*fabs(x_axis_temp); turnable_data.desired_pitch_position = turnable_data.max_pitch_postion; } else if(x_axis_temp < -50) { - turnable_data.out_pitch_motor_ampere = 0.01*fabs(x_axis_temp); + turnable_data.out_pitch_motor_ampere = 0.02*fabs(x_axis_temp); turnable_data.desired_pitch_position = turnable_data.min_pitch_postion; } else diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.h b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.h index 814f16b..724a4ed 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/app/app_turntable.h @@ -58,7 +58,7 @@ extern "C" { #define ZERO_VAULE 0x0080 // 32768,Ҫλǰ -#define REMOTE_ZERO 1022 +#define REMOTE_ZERO 980 #define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF)) @@ -106,7 +106,7 @@ typedef struct TurnableData float min_pitch_postion; // λϢ float max_pitch_postion; // λϢ - float max_ampere; // + float max_ampere; // } TurnableData; diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h b/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h index 7bbbc8d..6e474b4 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface.h @@ -191,7 +191,7 @@ typedef struct _StrManualComputerInput typedef union _UnManualComputerInput { StrManualComputerInput bit_data; // 使用定义的结构体变量名 - unsigned int arr[sizeof(StrManualComputerInput) / sizeof(unsigned int)]; // 通过结构体类型确定大小 + uint8_t arr[sizeof(StrManualComputerInput)]; // 通过结构体类型确定大小 } UnManualComputerInput; diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c index 6bce3e4..44fdebb 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_can.c @@ -1054,7 +1054,7 @@ void canTimerProcess(void *signal_id) static void processSdoOutput1(void *signal_id) { (void)signal_id; // DZΪʹã -// CAN_Send_Msg(&can_handle_5, un_sdo_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1.tx_can_data.arr[0], 8, 15);// + CAN_Send_Msg(&can_handle_4, un_sdo_output1.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output1.tx_can_data.arr[0], 8, 15);// } static void processSdoOutput2(void *signal_id) @@ -1066,19 +1066,19 @@ static void processSdoOutput2(void *signal_id) static void processSdoOutput3(void *signal_id) { (void)signal_id; // DZΪʹã -// CAN_Send_Msg(&can_handle_4, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 17); + CAN_Send_Msg(&can_handle_4, un_sdo_output3.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output3.tx_can_data.arr[0], 8, 17); } static void processSdoOutput4(void *signal_id) { (void)signal_id; // DZΪʹã -// CAN_Send_Msg(&can_handle_4, un_sdo_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4.tx_can_data.arr[0], 8, 18); + CAN_Send_Msg(&can_handle_4, un_sdo_output4.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output4.tx_can_data.arr[0], 8, 18); } static void processSdoOutput5(void *signal_id) { (void)signal_id; // DZΪʹã -// CAN_Send_Msg(&can_handle_4, un_sdo_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5.tx_can_data.arr[0], 8, 19); + CAN_Send_Msg(&can_handle_4, un_sdo_output5.tx_can_id.raw, FLEXCAN_EXTEND_FRAME, FLEXCAN_FrameTypeData, (uint8_t *)&un_sdo_output5.tx_can_data.arr[0], 8, 19); } //static void processSdoOutput4(void *signal_id) //{ diff --git a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c index e0c444e..00ffbf3 100644 --- a/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c +++ b/boards/e3_176_ref/app_demo/eth-xip/sf/interface_ethernet.c @@ -245,7 +245,8 @@ void udp_Callback_1(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_ad publishMessage(&un_manual_computer_input, 1); // p->len = len; -// printf("Manualrecive len:%d\n",len); +// printf("Manualrecive len:%d, speed: %d , cur: %d\n",len,un_manual_computer_input.bit_data.set_speed,un_manual_computer_input.bit_data.set_curvature); + // udp_sendto(upcb, p, addr, port); } else if( (0xFF == buf[0] ) && (0xCC == buf[1] ) )// Զ