liang brain / Mbed 2 deprecated Ex_econdercar_ROS

fdas

Dependencies:   QEI mbed

main.cpp@0:13ba01a70b4a, 2018-05-06 (annotated)

Committer:
brainliang
Date:
Sun May 06 06:01:45 2018 +0000
Revision:
0:13ba01a70b4a
sdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brainliang 0:13ba01a70b4a 1 #include "mbed.h"
brainliang 0:13ba01a70b4a 2 #include "Moter3_L298N.h"
brainliang 0:13ba01a70b4a 3 #include "QEI.h"
brainliang 0:13ba01a70b4a 4 #define LOW 0
brainliang 0:13ba01a70b4a 5 #define maxv 344.0 //电机最高转速
brainliang 0:13ba01a70b4a 6
brainliang 0:13ba01a70b4a 7 float setspeed[2]={150,150}, error_current[2]={0,0}, error_last[2]={0,0}, error_sum[2]={0,0}, kp[2]={0.9,0.9}, ki[2]={0,0}, kd[2]={0,0};
brainliang 0:13ba01a70b4a 8 float time_encoder_pid=0.2; //编码器读取周期,单位s,也是pid控制计算周期
brainliang 0:13ba01a70b4a 9
brainliang 0:13ba01a70b4a 10 Motor_3 motor[] = {Motor_3(PB_12,PB_13,PA_11),Motor_3(PB_14,PB_15,PA_8)};
brainliang 0:13ba01a70b4a 11 //Motor_3 motor2(PB_14,PB_15,PA_8);
brainliang 0:13ba01a70b4a 12
brainliang 0:13ba01a70b4a 13 //调试串口
brainliang 0:13ba01a70b4a 14 Serial pc(PB_10, PB_11);
brainliang 0:13ba01a70b4a 15 Serial bt(PA_2, PA_3);
brainliang 0:13ba01a70b4a 16
brainliang 0:13ba01a70b4a 17 Ticker toggle_time_ticker;
brainliang 0:13ba01a70b4a 18 //编码器1-4
brainliang 0:13ba01a70b4a 19 QEI wheel[] = {QEI(PB_5, PB_6, NC, 11, QEI::X4_ENCODING),QEI(PB_7, PB_8, NC, 11, QEI::X4_ENCODING)}; //买来的电机自带编码器,说明书上有电机每转一圈编码器发生11个信号
brainliang 0:13ba01a70b4a 20 //QEI wheel2(PB_7, PB_8, NC, 11, QEI::X4_ENCODING);
brainliang 0:13ba01a70b4a 21
brainliang 0:13ba01a70b4a 22 float a[2]= {150, 150}; //初始速度
brainliang 0:13ba01a70b4a 23 void time_ticker();
brainliang 0:13ba01a70b4a 24 void serialread();
brainliang 0:13ba01a70b4a 25 void pidcontroller();
brainliang 0:13ba01a70b4a 26
brainliang 0:13ba01a70b4a 27 void pidcontroller(){
brainliang 0:13ba01a70b4a 28 float target[2];
brainliang 0:13ba01a70b4a 29 for (int i=0; i<=1;i++){
brainliang 0:13ba01a70b4a 30 error_sum[i] += error_current[i];
brainliang 0:13ba01a70b4a 31 target[i]=(a[i] + kp[i]*error_current[i] + ki[i]*(error_current[i] - error_last[i]) +kd[i]*(error_sum[i]));
brainliang 0:13ba01a70b4a 32 motor[i].mv(target[i]/maxv*100); //mv函数的参数取值范围是-100~100
brainliang 0:13ba01a70b4a 33 // motor1.mv(a[0] + kp[0]*error_current[0] + ki*(error_current[0] - error_last[0]) +kd*(error_sum[0]));
brainliang 0:13ba01a70b4a 34 // motor2.mv(a[1] + kp[1]*error_current[1] + ki*(error_current[1] - error_last[1]) +kd*(error_sum[1]));
brainliang 0:13ba01a70b4a 35 error_last[i]=error_current[i];
brainliang 0:13ba01a70b4a 36 i++;
brainliang 0:13ba01a70b4a 37 }
brainliang 0:13ba01a70b4a 38
brainliang 0:13ba01a70b4a 39 pc.printf("target x:%f y:%f\r\n",target[0],target[1]);
brainliang 0:13ba01a70b4a 40 }
brainliang 0:13ba01a70b4a 41
brainliang 0:13ba01a70b4a 42
brainliang 0:13ba01a70b4a 43 void time_ticker(){
brainliang 0:13ba01a70b4a 44 for (int i=0; i<=1; i++){
brainliang 0:13ba01a70b4a 45 //获取一个轮子的编码器读到的数和旋转方向
brainliang 0:13ba01a70b4a 46 error_current[i] = setspeed[i] - wheel[i].getPulses();
brainliang 0:13ba01a70b4a 47 //wheel[i].getState(); //获取旋转方向,返回值为1--顺时针, -1--逆时针
brainliang 0:13ba01a70b4a 48 wheel[i].reset();
brainliang 0:13ba01a70b4a 49 }
brainliang 0:13ba01a70b4a 50
brainliang 0:13ba01a70b4a 51 pc.printf("error x:%f y:%f\r\n",error_current[0],error_current[1]);
brainliang 0:13ba01a70b4a 52 pidcontroller();
brainliang 0:13ba01a70b4a 53
brainliang 0:13ba01a70b4a 54
brainliang 0:13ba01a70b4a 55 }
brainliang 0:13ba01a70b4a 56
brainliang 0:13ba01a70b4a 57 void pcserialread(){
brainliang 0:13ba01a70b4a 58 switch (pc.getc())
brainliang 0:13ba01a70b4a 59 {
brainliang 0:13ba01a70b4a 60 case '1':
brainliang 0:13ba01a70b4a 61 setspeed[0]=100;
brainliang 0:13ba01a70b4a 62 setspeed[1]=100;
brainliang 0:13ba01a70b4a 63 break;
brainliang 0:13ba01a70b4a 64 case '2':
brainliang 0:13ba01a70b4a 65 setspeed[0]=300;
brainliang 0:13ba01a70b4a 66 setspeed[1]=300;
brainliang 0:13ba01a70b4a 67 break;
brainliang 0:13ba01a70b4a 68 default:
brainliang 0:13ba01a70b4a 69 break;
brainliang 0:13ba01a70b4a 70 }
brainliang 0:13ba01a70b4a 71 }
brainliang 0:13ba01a70b4a 72
brainliang 0:13ba01a70b4a 73 typedef enum
brainliang 0:13ba01a70b4a 74 {
brainliang 0:13ba01a70b4a 75 CheckS,CheckW,Checka,Checkb,Checkc,Checkd,Checke,Checkf,CheckSum
brainliang 0:13ba01a70b4a 76 }STATE;
brainliang 0:13ba01a70b4a 77
brainliang 0:13ba01a70b4a 78 void btserialread(){
brainliang 0:13ba01a70b4a 79 char u1Rec;
brainliang 0:13ba01a70b4a 80 static STATE State = CheckS;//初始状态待检查
brainliang 0:13ba01a70b4a 81 static char PositionTemp[7]={0,0,0,0,0,0,0};//装载数据值a(左右转) b(前后退) c(机械手高度) d(机械手旋转角度) Sum(求和校验)
brainliang 0:13ba01a70b4a 82 u1Rec = bt.getc();
brainliang 0:13ba01a70b4a 83 switch(State)
brainliang 0:13ba01a70b4a 84 {
brainliang 0:13ba01a70b4a 85 case CheckS:
brainliang 0:13ba01a70b4a 86 if(u1Rec==(char)'S')
brainliang 0:13ba01a70b4a 87 State=CheckW;//准备跳转至数据读取
brainliang 0:13ba01a70b4a 88 else
brainliang 0:13ba01a70b4a 89 State=CheckS;
brainliang 0:13ba01a70b4a 90 break;
brainliang 0:13ba01a70b4a 91 case CheckW:
brainliang 0:13ba01a70b4a 92 if(u1Rec==(char)'W')
brainliang 0:13ba01a70b4a 93 State=Checka;
brainliang 0:13ba01a70b4a 94 else if(u1Rec==(char)'S')
brainliang 0:13ba01a70b4a 95 State=CheckW;
brainliang 0:13ba01a70b4a 96 else
brainliang 0:13ba01a70b4a 97 State=CheckS;
brainliang 0:13ba01a70b4a 98 break;
brainliang 0:13ba01a70b4a 99 case Checka:
brainliang 0:13ba01a70b4a 100 PositionTemp[0]=u1Rec;
brainliang 0:13ba01a70b4a 101 State=Checkb;
brainliang 0:13ba01a70b4a 102 break;
brainliang 0:13ba01a70b4a 103 case Checkb:
brainliang 0:13ba01a70b4a 104 PositionTemp[1]=u1Rec;
brainliang 0:13ba01a70b4a 105 State=Checkc;
brainliang 0:13ba01a70b4a 106 break;
brainliang 0:13ba01a70b4a 107 case Checkc:
brainliang 0:13ba01a70b4a 108 PositionTemp[2]=u1Rec;
brainliang 0:13ba01a70b4a 109 State=Checkd;
brainliang 0:13ba01a70b4a 110 break;
brainliang 0:13ba01a70b4a 111 case Checkd:
brainliang 0:13ba01a70b4a 112 PositionTemp[3]=u1Rec;
brainliang 0:13ba01a70b4a 113 State=Checke;
brainliang 0:13ba01a70b4a 114 break;
brainliang 0:13ba01a70b4a 115 case Checke:
brainliang 0:13ba01a70b4a 116 PositionTemp[4]=u1Rec;
brainliang 0:13ba01a70b4a 117 State=Checkf;
brainliang 0:13ba01a70b4a 118 break;
brainliang 0:13ba01a70b4a 119 case Checkf:
brainliang 0:13ba01a70b4a 120 PositionTemp[5]=u1Rec;
brainliang 0:13ba01a70b4a 121 State=CheckSum;
brainliang 0:13ba01a70b4a 122 break;
brainliang 0:13ba01a70b4a 123 case CheckSum:
brainliang 0:13ba01a70b4a 124 PositionTemp[6]=u1Rec;
brainliang 0:13ba01a70b4a 125 if(PositionTemp[6]==(char)(PositionTemp[0]+PositionTemp[1]+PositionTemp[2]+PositionTemp[3]+PositionTemp[4]+PositionTemp[5]))
brainliang 0:13ba01a70b4a 126 {
brainliang 0:13ba01a70b4a 127 kp[0] = PositionTemp[0];
brainliang 0:13ba01a70b4a 128 ki[0] = PositionTemp[1];
brainliang 0:13ba01a70b4a 129 kd[0] = PositionTemp[2];
brainliang 0:13ba01a70b4a 130 kp[1] = PositionTemp[3];
brainliang 0:13ba01a70b4a 131 ki[1] = PositionTemp[4];
brainliang 0:13ba01a70b4a 132 kd[1] = PositionTemp[5];
brainliang 0:13ba01a70b4a 133 }
brainliang 0:13ba01a70b4a 134 State=CheckS;
brainliang 0:13ba01a70b4a 135 break;
brainliang 0:13ba01a70b4a 136 default:
brainliang 0:13ba01a70b4a 137 State=CheckS;
brainliang 0:13ba01a70b4a 138 break;
brainliang 0:13ba01a70b4a 139 }
brainliang 0:13ba01a70b4a 140 }
brainliang 0:13ba01a70b4a 141
brainliang 0:13ba01a70b4a 142
brainliang 0:13ba01a70b4a 143
brainliang 0:13ba01a70b4a 144 int main()
brainliang 0:13ba01a70b4a 145 {
brainliang 0:13ba01a70b4a 146 toggle_time_ticker.attach(&time_ticker, time_encoder_pid); //使能ticker中断,0.2s产生一次.中断函数用于获取每个轮子的计数值
brainliang 0:13ba01a70b4a 147
brainliang 0:13ba01a70b4a 148 pc.attach(&pcserialread);
brainliang 0:13ba01a70b4a 149 bt.attach(&btserialread);
brainliang 0:13ba01a70b4a 150 //for (int speed=10;speed<1000;speed+=10){
brainliang 0:13ba01a70b4a 151 // pc.printf("set to %d\r\n",speed);
brainliang 0:13ba01a70b4a 152 // for (int i=0; i<=1; i++){
brainliang 0:13ba01a70b4a 153 // motor[i].mv(speed/maxv*100);
brainliang 0:13ba01a70b4a 154 // }
brainliang 0:13ba01a70b4a 155 // wait(2);
brainliang 0:13ba01a70b4a 156 // }
brainliang 0:13ba01a70b4a 157 for (int i=0; i<=1; i++){
brainliang 0:13ba01a70b4a 158 motor[i].mv(a[i]/maxv*100);
brainliang 0:13ba01a70b4a 159 }
brainliang 0:13ba01a70b4a 160
brainliang 0:13ba01a70b4a 161 while(1);
brainliang 0:13ba01a70b4a 162 }