中国大学生工程实践与创新能力竞赛（工程训练大赛）——智慧物流搬运小车 ③ 让车轮先转起来

本文主要是介绍中国大学生工程实践与创新能力竞赛（工程训练大赛）——智慧物流搬运小车 ③ 让车轮先转起来，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

让轮子动起来

如果是初学者，建议看这位博主的基于arduino设计小车的一篇推文，非常基础，看完并动手操作起来后就算入门。我当初刚刚入门的时候，这位博主的文章看了很多遍。

传送门：(35条消息) Arduino智能小车——拼装篇_漫长IT路-CSDN博客_arduino智能小车https://blog.csdn.net/qq_16775293/article/details/77427060

先把这几篇全部看完再来这里接着看吧，学习是循序渐进的。

至此，想必大家已经明白了怎么让轮子动起来，接下来就是让轮子按照设定转速来动

下面是我的motor.c文件，这个.c用来输出pwm信号控制车轮旋转

#define PWM_1_TIM				TIM_1
#define PWM_1_CH1				TIM_1_CH1_A08
#define PWM_1_CH2				TIM_1_CH2_A09
#define PWM_1_CH3				TIM_1_CH3_A10
#define PWM_1_CH4				TIM_1_CH4_A11
#define PWM_2_TIM				TIM_8
#define PWM_2_CH1				TIM_8_CH1_C06
#define PWM_2_CH2				TIM_8_CH2_C07
#define PWM_2_CH3				TIM_8_CH3_C08
#define PWM_2_CH4				TIM_8_CH4_C09#define MOTOR_PWM_MAX 50000//接错线电机反转的话，0改成1
//1234分别是左前、右前、左后、右后
#define IF_MOTOT_1_DIRECTION_WRONG 0
#define IF_MOTOT_2_DIRECTION_WRONG 0
#define IF_MOTOT_3_DIRECTION_WRONG 0
#define IF_MOTOT_4_DIRECTION_WRONG 0#define ever ;;char if_motor_1_back = 0;	 //电机是否反转
char if_motor_2_back = 0;
char if_motor_3_back = 0;
char if_motor_4_back = 0;int motor_pwm_1 = 0;  //四个轮的PWM
int motor_pwm_2 = 0;
int motor_pwm_3 = 0;
int motor_pwm_4 = 0;//我写代码习惯一个函数控制一个轮，然后四个轮被一个大函数封装起来
//我设计的驱动控制一路电机需要两路PWM，一个赋值一个赋0就可以动起来void motor_1_run(int pwm, char if_back)
{#if IF_MOTOT_1_DIRECTION_WRONGif(if_back){pwm_duty_updata(PWM_1_TIM, PWM_1_CH1, -pwm);pwm_duty_updata(PWM_1_TIM, PWM_1_CH2, 0);//前}else {pwm_duty_updata(PWM_1_TIM, PWM_1_CH1, 0);pwm_duty_updata(PWM_1_TIM, PWM_1_CH2, pwm);}#else if(if_back == 0){pwm_duty_updata(PWM_1_TIM, PWM_1_CH1, 0);pwm_duty_updata(PWM_1_TIM, PWM_1_CH2, pwm);//前}else {pwm_duty_updata(PWM_1_TIM, PWM_1_CH1, -pwm);pwm_duty_updata(PWM_1_TIM, PWM_1_CH2, 0);}#endif
}void motor_2_run(int pwm, char if_back)
{#if IF_MOTOT_2_DIRECTION_WRONGif(if_back){pwm_duty_updata(PWM_1_TIM, PWM_1_CH3, 0);//前pwm_duty_updata(PWM_1_TIM, PWM_1_CH4, pwm);}else {pwm_duty_updata(PWM_1_TIM, PWM_1_CH3, pwm);//前pwm_duty_updata(PWM_1_TIM, PWM_1_CH4, 0);}#else if(!if_back){pwm_duty_updata(PWM_1_TIM, PWM_1_CH3, pwm);//前pwm_duty_updata(PWM_1_TIM, PWM_1_CH4, 0);}else {pwm_duty_updata(PWM_1_TIM, PWM_1_CH3, 0);//前pwm_duty_updata(PWM_1_TIM, PWM_1_CH4, -pwm);}#endif
}void motor_3_run(int pwm, char if_back)
{#if IF_MOTOT_3_DIRECTION_WRONGif(if_back){pwm_duty_updata(PWM_2_TIM, PWM_2_CH3, pwm);pwm_duty_updata(PWM_2_TIM, PWM_2_CH4, 0);//前}else {pwm_duty_updata(PWM_2_TIM, PWM_2_CH3, 0);pwm_duty_updata(PWM_2_TIM, PWM_2_CH4, pwm);//前}#else if(!if_back){pwm_duty_updata(PWM_2_TIM, PWM_2_CH3, 0);pwm_duty_updata(PWM_2_TIM, PWM_2_CH4, pwm);//前}else {pwm_duty_updata(PWM_2_TIM, PWM_2_CH3, -pwm);pwm_duty_updata(PWM_2_TIM, PWM_2_CH4, 0);//前}#endif
}void motor_4_run(int pwm, char if_back)
{#if IF_MOTOT_4_DIRECTION_WRONGif(if_back){pwm_duty_updata(PWM_2_TIM, PWM_2_CH1, 0);//前pwm_duty_updata(PWM_2_TIM, PWM_2_CH2, pwm);}else {pwm_duty_updata(PWM_2_TIM, PWM_2_CH1, pwm);//前pwm_duty_updata(PWM_2_TIM, PWM_2_CH2, 0);}#else if(!if_back){pwm_duty_updata(PWM_2_TIM, PWM_2_CH1, pwm);//前pwm_duty_updata(PWM_2_TIM, PWM_2_CH2, 0);}else {pwm_duty_updata(PWM_2_TIM, PWM_2_CH1, 0);//前pwm_duty_updata(PWM_2_TIM, PWM_2_CH2, -pwm);}#endif
}void motor_run(void)
{motor_1_run(motor_pwm_1, if_motor_1_back);motor_2_run(motor_pwm_2, if_motor_2_back);motor_3_run(motor_pwm_3, if_motor_3_back);motor_4_run(motor_pwm_4, if_motor_4_back);
}

电机闭环

这里我参考的是这一篇文章
传送门：(35条消息) 用Arduino实现霍尔编码减速电机PI调速（增量式）_cbirdfly's Blog-CSDN博客https://blog.csdn.net/C1664510416/article/details/107227754?spm=1001.2014.3001.5506

需要配置一路定时器中断，10ms调用一次终端服务函数就行，然后在整个中断服务函数里一直在运行电机闭环，始终让车轮转速和期望转速一致。然后根据车不同的运行状况来修改期望速度即可。

下面是我的中断服务函数

void tim6_handler(void)
{static char if_angle_correct = 0;static int laser_test_count = 0;if(if_angle_correct==2)//分频，角度环50hz，定时器100hz，二分频{get_angle_z();angle_correct();if_angle_correct = 0;}set_z_speed();//设定目标角速度full_laser_locat(if_4,if_1,if_2);//使用激光传感器定位，参数为左中右//速度合成sum_speed();//下面两行进行电机闭环update_pwm();motor_run();if(if_laser_arrive())laser_count++;elselaser_count = 0;//下面的一大堆东西都是用来记时的，后面再讲
//我觉得我这个思路还行，在buzzer_bbbi();if_angle_correct++;if_send_info = 1;adjust_time++;change_time++;use_direction_time++;wait_for_arm_count++;time_sum++;}

中断服务函数写的有点冗余，其实重点就是10ms内不断调用几个函数来修改期望速度罢了

下面是我的speedcontrol.c函数

//这四个变量定义在motor.c里
extern int motor_pwm_1 ;
extern int motor_pwm_2 ;
extern int motor_pwm_3 ;
extern int motor_pwm_4 ;struct wheel_speed{int basic_speed;	//基础的速度，大部分时间这个速度int trace_line_speed;		//寻线的时候，需要改变的速度，是个增量int angle_z_speed;		//角度闭环，Z轴旋转分量，增量
};int current_speed_1 = 0;
int current_speed_2 = 0;
int current_speed_3 = 0;
int current_speed_4 = 0;int final_speed_1 = 0;
int final_speed_2 = 0;
int final_speed_3 = 0;
int final_speed_4 = 0;//两个参数分别是现在的速度和期望速度
int update_pwm_1(float current_speed,float target_speed)      //增量PI调速代码，输入
{static int pwm;static int error_1,last_error_1,prev_error_1;  //静态变量存在程序全周期：//pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差error_1=target_speed-current_speed;    //计算本次偏差e(k)pwm += (kp*(error_1-last_error_1)+ki*error_1+kd*(error_1-2*last_error_1+prev_error_1));   //增量式PID控制器prev_error_1=last_error_1;  //保存上上次偏差last_error_1=error_1;     //保存上一次偏差//电机是否反转if(pwm<0){if_motor_1_back = 1;  }else if_motor_1_back = 0;//PWM限幅if(pwm>45000){pwm = 45000;  }if(pwm<-45000){pwm = -45000;}return pwm;
}int update_pwm_2(float current_speed,float target_speed)      //增量PI调速代码，输入
{static int pwm;static int error_1,last_error_1,prev_error_1;  //静态变量存在程序全周期：//pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差error_1=target_speed-current_speed;    //计算本次偏差e(k)pwm += (kp*(error_1-last_error_1)+ki*error_1+kd*(error_1-2*last_error_1+prev_error_1));   //增量式PID控制器prev_error_1=last_error_1;  //保存上上次偏差last_error_1=error_1;     //保存上一次偏差if(pwm<0){if_motor_2_back = 1;  }else if_motor_2_back = 0;if(pwm>45000){pwm = 45000;  }if(pwm<-45000){pwm = -45000;}return pwm;
}
int update_pwm_3(float current_speed,float target_speed)      //增量PI调速代码，输入
{static int pwm;static int error_1,last_error_1,prev_error_1;  //静态变量存在程序全周期：//pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差error_1=target_speed-current_speed;    //计算本次偏差e(k)pwm += (kp*(error_1-last_error_1)+ki*error_1+kd*(error_1-2*last_error_1+prev_error_1));   //增量式PID控制器prev_error_1=last_error_1;  //保存上上次偏差last_error_1=error_1;     //保存上一次偏差if(pwm<0){if_motor_3_back = 1;  }else if_motor_3_back = 0;if(pwm>45000){pwm = 45000;  }if(pwm<-45000){pwm = -45000;}return pwm;
}
int update_pwm_4(float current_speed,float target_speed)      //增量PI调速代码，输入
{static int pwm;static int error_1,last_error_1,prev_error_1;  //静态变量存在程序全周期：//pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差error_1=target_speed-current_speed;    //计算本次偏差e(k)pwm += (kp*(error_1-last_error_1)+ki*error_1+kd*(error_1-2*last_error_1+prev_error_1));   //增量式PID控制器prev_error_1=last_error_1;  //保存上上次偏差last_error_1=error_1;     //保存上一次偏差if(pwm<0){if_motor_4_back = 1;  }else if_motor_4_back = 0;if(pwm>45000){pwm = 45000;  }if(pwm<-45000){pwm = -45000;}return pwm;
}//这个函数通过小车的各种运行情况，来计算四个轮子的期望速度
//这个函数当时写的像一坨屎山，下面展开讲这个函数
int sum_speed(void);//函数功能就是计算四个轮PWM值，让车轮转速和预期转速一致
void update_pwm(void)
{motor_pwm_1 = update_pwm_1(current_speed_1, final_speed_1);motor_pwm_2 = update_pwm_2(current_speed_2, final_speed_2);motor_pwm_3 = update_pwm_3(current_speed_3, final_speed_3);motor_pwm_4 = update_pwm_4(current_speed_4, final_speed_4);
}

麦克纳姆轮的全向移动计算

我本人的数学能力还是很差的。。所以这里直接放传送门吧，建议大家先搞清楚麦克纳姆轮的全向移动机制，然后开始编写控制算法

传送门：(35条消息) 一文读懂麦克纳姆轮全向移动原理及剖析_u014453443的博客-CSDN博客https://blog.csdn.net/u014453443/article/details/107228531?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163525429516780357275505%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=163525429516780357275505&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_click~default-2-107228531.first_rank_v2_pc_rank_v29&utm_term=%E9%BA%A6%E5%85%8B%E7%BA%B3%E5%A7%86%E8%BD%AE&spm=1018.2226.3001.4187

原理搞清楚后编写软件，这里我用一个函数 int sum_speed(void);根据不同的运动情况，计算具体到每一个轮子的转速。

int sum_speed(void)
{static int speed_choose = 0;encoder_get_count();current_speed_1 = encoder_1_count;current_speed_2 = encoder_2_count;current_speed_3 = encoder_3_count;current_speed_4 = encoder_4_count;//根据调节选择速度挡位if(if_use_direction_and_time){//跳过}	else//需要合成速度的时候{//先选择速度大小if(if_adjust==1)//需要调整姿态{speed_choose = adjust_speed;}else if(if_approach==1)//数格走，需要降速speed_choose = approach_speed;else//正常情况speed_choose = move_speed_normal;//合成带方向的速度矢量if(if_adjust==1){wheel_1.basic_speed = adjust_direction[0]*speed_choose + adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose - adjust_direction[3]*speed_choose;wheel_2.basic_speed = adjust_direction[0]*speed_choose - adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose + adjust_direction[3]*speed_choose;wheel_3.basic_speed = adjust_direction[0]*speed_choose - adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose + adjust_direction[3]*speed_choose;wheel_4.basic_speed = adjust_direction[0]*speed_choose + adjust_direction[1]*speed_choose - adjust_direction[2]*speed_choose - adjust_direction[3]*speed_choose;}else if(use_laser==1){//跳过}else //最简单的前后左右移动情况{//根据运动方向，给基础速度赋值if(move_direction==1){wheel_1.basic_speed = speed_choose;wheel_2.basic_speed = speed_choose;wheel_3.basic_speed = speed_choose;wheel_4.basic_speed = speed_choose;}else if(move_direction==2){wheel_1.basic_speed = speed_choose;wheel_2.basic_speed = -speed_choose;wheel_3.basic_speed = -speed_choose;wheel_4.basic_speed = speed_choose;}else if(move_direction==3){wheel_1.basic_speed = -speed_choose;wheel_2.basic_speed = -speed_choose;wheel_3.basic_speed = -speed_choose;wheel_4.basic_speed = -speed_choose;}else if(move_direction==4){wheel_1.basic_speed = -speed_choose;wheel_2.basic_speed = speed_choose;wheel_3.basic_speed = speed_choose;wheel_4.basic_speed = -speed_choose;}else {wheel_1.basic_speed = 0;wheel_2.basic_speed = 0;wheel_3.basic_speed = 0;wheel_4.basic_speed = 0;}}}//速度合成结束/final_speed_1 = wheel_1.angle_z_speed + wheel_1.basic_speed + wheel_1.trace_line_speed;final_speed_2 = wheel_2.angle_z_speed + wheel_2.basic_speed + wheel_2.trace_line_speed;final_speed_3 = wheel_3.angle_z_speed + wheel_3.basic_speed + wheel_3.trace_line_speed;final_speed_4 = wheel_4.angle_z_speed + wheel_4.basic_speed + wheel_4.trace_line_speed;return 1;
}

current_speed_x就是每个轮子的编码器在10ms读到的脉冲，每10ms更新一次，当好是一次控制周期。
第一个大的if里的调节是if_use_direction_and_time，这个后续再写，是一个简单的功能，即：指定速度方向、指定时间，开环走一段距离，实测还是准确的。如果开环的话，四个轮的basic_speed就不在这个函数里赋值了，故直接跳过。
第二个大的if里是大多数时间，速度大小和方向需要被给定。

先根据运动情况选择一个合适速度大小，我这里主要写了三种情况，第一种是卡线的时候用的速度，是一个较低的值，即adjust_speed，有一个if_adjust标志位；第二种是缓停的时候用的速度，比如走四格距离，前三格速度可以快点，第四个要提前减速，速度为approach_speed，有一个if_approach的标志位；第三种情况就是正常速度了，即move_speed_normal。
然后指定速度的方向，这里一般就选择四个轮的正反转即可，在下面第二大段if...else...里，第一个是卡线时候的速度大小以及方向情况，后面会先，直接看最后的else就行，根据不同的运动方向，改变四个轮的旋转方向即可实现左右移动。

然后计算期望速度final_speed_x即可，把它丢到电机闭环函数里即可。

实现简单的循迹

因为这个比赛的赛道相对简单，网格状，所以犯不着上智能车时期的一大堆控制算法，就用最简单的if...else...，然后修改wheel_speed这个结构体里的trace_line_speed即可
直接枚举7种巡线情况即可，这里写的还是很像屎山的
话不多说，直接上代码：

void line_correct_1(void)//向前
{int a = 0;wheel_1.trace_line_speed = 0;wheel_2.trace_line_speed = 0;wheel_3.trace_line_speed = 0;wheel_4.trace_line_speed = 0;if((huidu_value[0][0]==0)&&(huidu_value[0][1]==0)&&(huidu_value[0][2]==0)&&(huidu_value[0][3]==1)&&(huidu_value[0][4]==0)&&(huidu_value[0][5]==0)&&(huidu_value[0][6]==0))a=0;else if((huidu_value[0][0]==0)&&(huidu_value[0][1]==0)&&(huidu_value[0][2]==0)&&(huidu_value[0][3]==0)&&(huidu_value[0][4]==1)&&(huidu_value[0][5]==0)&&(huidu_value[0][6]==0))a=-10;else if((huidu_value[0][0]==0)&&(huidu_value[0][1]==0)&&(huidu_value[0][2]==0)&&(huidu_value[0][3]==0)&&(huidu_value[0][4]==0)&&(huidu_value[0][5]==1)&&(huidu_value[0][6]==0))a=-20;else if((huidu_value[0][0]==0)&&(huidu_value[0][1]==0)&&(huidu_value[0][2]==0)&&(huidu_value[0][3]==0)&&(huidu_value[0][4]==0)&&(huidu_value[0][5]==0)&&(huidu_value[0][6]==1))a=-30;else if((huidu_value[0][0]==0)&&(huidu_value[0][1]==0)&&(huidu_value[0][2]==1)&&(huidu_value[0][3]==0)&&(huidu_value[0][4]==0)&&(huidu_value[0][5]==0)&&(huidu_value[0][6]==0))a=10;else if((huidu_value[0][0]==0)&&(huidu_value[0][1]==1)&&(huidu_value[0][2]==0)&&(huidu_value[0][3]==0)&&(huidu_value[0][4]==0)&&(huidu_value[0][5]==0)&&(huidu_value[0][6]==0))a=20;else if((huidu_value[0][0]==1)&&(huidu_value[0][1]==0)&&(huidu_value[0][2]==0)&&(huidu_value[0][3]==0)&&(huidu_value[0][4]==0)&&(huidu_value[0][5]==0)&&(huidu_value[0][6]==0))a=30;wheel_2.trace_line_speed = a;wheel_3.trace_line_speed = a;wheel_1.trace_line_speed = -a;wheel_4.trace_line_speed = -a;}//复制粘贴上面这一段，再写三个方向的函数即可//然后用一个函数封装起来void line_correct(char move_direction) //巡线，纠正左右偏差,参数为小车运动方向
{//分别是前后左右if(move_direction==1)line_correct_1();else if(move_direction==2)line_correct_2();else if(move_direction==3)line_correct_3();else if(move_direction==4)line_correct_4();
}

speed_control结构体变量里有三个变量：basic_speed、trace_line_speed、angle_z_speed
basic_speed就是车默认的速度值，根据你的需求来改
trace_line_speed是修改四个轮的转速，然后让车产生平移的分量来循迹
angle_z_speed是根据陀螺仪反馈的偏差来控制小车航向角

直线循迹根据灰度传感器修改trace_line_speed即可循迹运动，然后修改函数里if...else...里的值可以调整循迹的强度（想不到用什么准确的词，类比于PID控制器的P）

这篇关于中国大学生工程实践与创新能力竞赛（工程训练大赛）——智慧物流搬运小车 ③ 让车轮先转起来的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！