本文主要是介绍中国大学生工程实践与创新能力竞赛(工程训练大赛)——智慧物流搬运小车 ③ 让车轮先转起来,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
让轮子动起来
如果是初学者,建议看这位博主的基于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)
这篇关于中国大学生工程实践与创新能力竞赛(工程训练大赛)——智慧物流搬运小车 ③ 让车轮先转起来的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!