GR-PEACH_producer_meeting / Mbed 2 deprecated Nucleo_Motor

MotorDrive制御

Dependencies:   mbed Motor PID QEI2 fusion_Motor

Fork of Nucleo_Motor by Kiko Ishimoto

main.cpp@0:8719ed91e6fd, 2015-09-23 (annotated)

Committer:
kikoaac
Date:
Wed Sep 23 06:06:49 2015 +0000
Revision:
0:8719ed91e6fd
Child:
1:ad8acc6cfba7
No mount QEI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kikoaac 0:8719ed91e6fd 1 /*
kikoaac 0:8719ed91e6fd 2 motor 555
kikoaac 0:8719ed91e6fd 3 Lipo 4sel
kikoaac 0:8719ed91e6fd 4 Linear accelerator
kikoaac 0:8719ed91e6fd 5 Not mount QEI
kikoaac 0:8719ed91e6fd 6 */
kikoaac 0:8719ed91e6fd 7 #include "mbed.h"
kikoaac 0:8719ed91e6fd 8 #include "Motor.h"
kikoaac 0:8719ed91e6fd 9 #include "PID.h"
kikoaac 0:8719ed91e6fd 10 #include "QEI.h"
kikoaac 0:8719ed91e6fd 11 #include "fusion_Motor.h"
kikoaac 0:8719ed91e6fd 12 #include "Pin.h"
kikoaac 0:8719ed91e6fd 13 #define Lx 0.0001
kikoaac 0:8719ed91e6fd 14 #define Ku 0.32200//0.922000
kikoaac 0:8719ed91e6fd 15 #define Pu 0.0125
kikoaac 0:8719ed91e6fd 16 #define Kp Ku*0.6
kikoaac 0:8719ed91e6fd 17 #define Ti Pu*0.5
kikoaac 0:8719ed91e6fd 18 #define Td Pu*0.125
kikoaac 0:8719ed91e6fd 19
kikoaac 0:8719ed91e6fd 20
kikoaac 0:8719ed91e6fd 21 char data=0,speed=0,sot=0,Data2=0;
kikoaac 0:8719ed91e6fd 22 #define P Kp
kikoaac 0:8719ed91e6fd 23 #define I Kp/Ti
kikoaac 0:8719ed91e6fd 24 #define D Kp*Td
kikoaac 0:8719ed91e6fd 25 #define Mstate 6
kikoaac 0:8719ed91e6fd 26 #define SorT 5
kikoaac 0:8719ed91e6fd 27 #define Databit 0
kikoaac 0:8719ed91e6fd 28 #define TIME 0.0125
kikoaac 0:8719ed91e6fd 29 Timer Time;
kikoaac 0:8719ed91e6fd 30 /*
kikoaac 0:8719ed91e6fd 31 PID p(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 32 QEI i(QEI1_A,QEI1_B,NC,50,&Time);
kikoaac 0:8719ed91e6fd 33 Motor d(PD_2,PA_13,PA_14,PA_15,PC_9);
kikoaac 0:8719ed91e6fd 34 fusion_Motor mecanam3(i,p,d);
kikoaac 0:8719ed91e6fd 35 */
kikoaac 0:8719ed91e6fd 36
kikoaac 0:8719ed91e6fd 37
kikoaac 0:8719ed91e6fd 38 Serial device(D8,D2);
kikoaac 0:8719ed91e6fd 39 /*
kikoaac 0:8719ed91e6fd 40 PID *pid;
kikoaac 0:8719ed91e6fd 41 QEI *wheel;
kikoaac 0:8719ed91e6fd 42 Motor *motor;
kikoaac 0:8719ed91e6fd 43 */
kikoaac 0:8719ed91e6fd 44 PID pid[4] = {PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time)};
kikoaac 0:8719ed91e6fd 45 QEI wheel[4] = {QEI(QEI1_A,QEI1_B,NC,100,&Time),QEI(QEI2_A,QEI2_B,NC,100,&Time),QEI(QEI3_A,QEI3_B,NC,100,&Time),QEI(QEI4_A,QEI4_B,NC,100,&Time)};
kikoaac 0:8719ed91e6fd 46 Motor motor[4] = {Motor(A1,A2,A3,A4,A0),Motor(PD_2,PA_13,PA_14,PA_15,PC_9),Motor(D5,D4,D3,PB_8,PB_9),Motor(D12,D11,D10,D9,D7)};
kikoaac 0:8719ed91e6fd 47 fusion_Motor mecanam[4] = {
kikoaac 0:8719ed91e6fd 48 fusion_Motor(wheel[0],pid[0],motor[0]),//front right
kikoaac 0:8719ed91e6fd 49 fusion_Motor(wheel[1],pid[1],motor[1]),//Back right
kikoaac 0:8719ed91e6fd 50 fusion_Motor(wheel[2],pid[2],motor[2]),//front left
kikoaac 0:8719ed91e6fd 51 fusion_Motor(wheel[3],pid[3],motor[3])
kikoaac 0:8719ed91e6fd 52 };
kikoaac 0:8719ed91e6fd 53 PwmOut led(A0);
kikoaac 0:8719ed91e6fd 54 //back left
kikoaac 0:8719ed91e6fd 55 //Motor d(D5,D4,D3,D1,D0);
kikoaac 0:8719ed91e6fd 56 /*
kikoaac 0:8719ed91e6fd 57 void Delete()
kikoaac 0:8719ed91e6fd 58 {
kikoaac 0:8719ed91e6fd 59 delete pid;
kikoaac 0:8719ed91e6fd 60 delete wheel;
kikoaac 0:8719ed91e6fd 61 delete motor;
kikoaac 0:8719ed91e6fd 62 }
kikoaac 0:8719ed91e6fd 63 fusion_Motor *mecanam1;
kikoaac 0:8719ed91e6fd 64 fusion_Motor *mecanam2;
kikoaac 0:8719ed91e6fd 65 fusion_Motor *mecanam3;
kikoaac 0:8719ed91e6fd 66 fusion_Motor *mecanam4;
kikoaac 0:8719ed91e6fd 67 void setup()
kikoaac 0:8719ed91e6fd 68 {
kikoaac 0:8719ed91e6fd 69 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 70 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 71 motor = new Motor(A1,A2,A3,A4,A0);
kikoaac 0:8719ed91e6fd 72 mecanam1 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 73 Delete();
kikoaac 0:8719ed91e6fd 74 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 75 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 76 motor = new Motor(D4,D3,D2,D1,D0);
kikoaac 0:8719ed91e6fd 77 mecanam2 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 78 Delete();
kikoaac 0:8719ed91e6fd 79 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 80 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 81 motor = new Motor(PD_2,PA_13,PA_14,PA_15,PC_9);
kikoaac 0:8719ed91e6fd 82 mecanam3 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 83 Delete();
kikoaac 0:8719ed91e6fd 84 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 85 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 86 motor = new Motor(PC_7,PB_6,PA_7,PA_6,PA_8);
kikoaac 0:8719ed91e6fd 87 mecanam4 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 88 Delete();
kikoaac 0:8719ed91e6fd 89 }
kikoaac 0:8719ed91e6fd 90 */
kikoaac 0:8719ed91e6fd 91 /*
kikoaac 0:8719ed91e6fd 92 |
kikoaac 0:8719ed91e6fd 93 L\ /R
kikoaac 0:8719ed91e6fd 94 - -
kikoaac 0:8719ed91e6fd 95 R/ \L
kikoaac 0:8719ed91e6fd 96 |
kikoaac 0:8719ed91e6fd 97 */
kikoaac 0:8719ed91e6fd 98 enum STATE {
kikoaac 0:8719ed91e6fd 99 R,L,F,B
kikoaac 0:8719ed91e6fd 100 } state;
kikoaac 0:8719ed91e6fd 101 PwmOut LED(LED1);
kikoaac 0:8719ed91e6fd 102 #define PI 3.141592653589793
kikoaac 0:8719ed91e6fd 103 float DUTY_fill[4]= {0};
kikoaac 0:8719ed91e6fd 104 float DUTY_inte[4]= {0};
kikoaac 0:8719ed91e6fd 105 float DUTY[4]= {0};
kikoaac 0:8719ed91e6fd 106 float DUTY_prev[4]= {0};
kikoaac 0:8719ed91e6fd 107 float GAIN_P=P,GAIN_I=I,GAIN_D=D;
kikoaac 0:8719ed91e6fd 108 void ACT(float tardata,float *data,fusion_Motor *fusion,float *fill,float *inte,float *prev_dat)
kikoaac 0:8719ed91e6fd 109 {
kikoaac 0:8719ed91e6fd 110 float inter = TIME;
kikoaac 0:8719ed91e6fd 111 float dat = tardata-*data;
kikoaac 0:8719ed91e6fd 112 *inte += (dat + *prev_dat)/2*inter;
kikoaac 0:8719ed91e6fd 113 *data = GAIN_P*dat+GAIN_D**fill+GAIN_I**inte;
kikoaac 0:8719ed91e6fd 114 *fill = (dat - *prev_dat)*inter;
kikoaac 0:8719ed91e6fd 115 *prev_dat =dat;
kikoaac 0:8719ed91e6fd 116
kikoaac 0:8719ed91e6fd 117 /*
kikoaac 0:8719ed91e6fd 118 if(tardata>*data)
kikoaac 0:8719ed91e6fd 119 *data+=0.01f;
kikoaac 0:8719ed91e6fd 120 if(tardata<*data)
kikoaac 0:8719ed91e6fd 121 *data-=0.01f;*/
kikoaac 0:8719ed91e6fd 122 *fusion=*data;
kikoaac 0:8719ed91e6fd 123 }
kikoaac 0:8719ed91e6fd 124 void motruns(STATE RorL,float data)
kikoaac 0:8719ed91e6fd 125 {
kikoaac 0:8719ed91e6fd 126 if(RorL == R) {
kikoaac 0:8719ed91e6fd 127
kikoaac 0:8719ed91e6fd 128 ACT(data,&DUTY[R],&mecanam[R],&DUTY_fill[R],&DUTY_inte[R],&DUTY_prev[R]);
kikoaac 0:8719ed91e6fd 129 ACT(data,&DUTY[R+2],&mecanam[R+2],&DUTY_fill[R+2],&DUTY_inte[R+2],&DUTY_prev[R+2]);
kikoaac 0:8719ed91e6fd 130 } else if(RorL == L) {
kikoaac 0:8719ed91e6fd 131 ACT(data,&DUTY[L],&mecanam[L],&DUTY_fill[L],&DUTY_inte[L],&DUTY_prev[L]);
kikoaac 0:8719ed91e6fd 132 ACT(data,&DUTY[L+2],&mecanam[L+2],&DUTY_fill[L+2],&DUTY_inte[L+2],&DUTY_prev[L+2]);
kikoaac 0:8719ed91e6fd 133 }
kikoaac 0:8719ed91e6fd 134 }
kikoaac 0:8719ed91e6fd 135 double mecCot(float angle,STATE RorL)
kikoaac 0:8719ed91e6fd 136 {
kikoaac 0:8719ed91e6fd 137 double radian = (double)angle * PI / 180.0;
kikoaac 0:8719ed91e6fd 138 return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4-PI/2) : 1.4142135623730950488016887242097*cos(radian-PI/4-PI/2);
kikoaac 0:8719ed91e6fd 139 }
kikoaac 0:8719ed91e6fd 140 void vector(float angle)
kikoaac 0:8719ed91e6fd 141 {
kikoaac 0:8719ed91e6fd 142 angle-=90-360;
kikoaac 0:8719ed91e6fd 143 if(angle<0)angle+=360;
kikoaac 0:8719ed91e6fd 144 angle-=360;
kikoaac 0:8719ed91e6fd 145 if(angle>0)angle-=360;
kikoaac 0:8719ed91e6fd 146 angle*=-1;
kikoaac 0:8719ed91e6fd 147 motruns(R,mecCot(angle,R));
kikoaac 0:8719ed91e6fd 148 motruns(L,mecCot(angle,L));
kikoaac 0:8719ed91e6fd 149 printf("%5.5f %5.5f %5.5f ",angle , mecCot(angle,L),mecCot(angle,R));
kikoaac 0:8719ed91e6fd 150 }
kikoaac 0:8719ed91e6fd 151
kikoaac 0:8719ed91e6fd 152
kikoaac 0:8719ed91e6fd 153 void pid_setpoint()
kikoaac 0:8719ed91e6fd 154 {
kikoaac 0:8719ed91e6fd 155 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 156 pid[i].dPoint=wheel[i].getRPM();
kikoaac 0:8719ed91e6fd 157 }
kikoaac 0:8719ed91e6fd 158 void pid_start()
kikoaac 0:8719ed91e6fd 159 {
kikoaac 0:8719ed91e6fd 160 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 161 pid[i].start();
kikoaac 0:8719ed91e6fd 162 }
kikoaac 0:8719ed91e6fd 163 void pid_reset()
kikoaac 0:8719ed91e6fd 164 {
kikoaac 0:8719ed91e6fd 165 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 166 pid[i].pid_reset();
kikoaac 0:8719ed91e6fd 167 }
kikoaac 0:8719ed91e6fd 168 void getData()
kikoaac 0:8719ed91e6fd 169 {
kikoaac 0:8719ed91e6fd 170
kikoaac 0:8719ed91e6fd 171 data = device.getc();
kikoaac 0:8719ed91e6fd 172 speed = data>>Mstate;
kikoaac 0:8719ed91e6fd 173 sot = (data>>SorT)&0x01;
kikoaac 0:8719ed91e6fd 174 Data2 = data&0x1f;
kikoaac 0:8719ed91e6fd 175 //printf("%d%d%d%d%d%d%d%d ",(data&0x80)>>7,(data&0x40)>>6,(data&0x20)>>5,(data&0x10)>>4,(data&0x08)>>3,(data&0x04)>>2,(data&0x02)>>1,(data&0x01));
kikoaac 0:8719ed91e6fd 176 //printf("0x%x ,0x%x ,0x%x ,0x%x\r\n" ,data,speed,sot,Data2);
kikoaac 0:8719ed91e6fd 177 //printf("getdata!!\r\n");
kikoaac 0:8719ed91e6fd 178 }
kikoaac 0:8719ed91e6fd 179 bool MotorState()
kikoaac 0:8719ed91e6fd 180 {
kikoaac 0:8719ed91e6fd 181 for(int a=0; a<4; a++) {
kikoaac 0:8719ed91e6fd 182 if(speed==0) {
kikoaac 0:8719ed91e6fd 183 mecanam[a].run(Free,1);
kikoaac 0:8719ed91e6fd 184 } else if(speed==1) {
kikoaac 0:8719ed91e6fd 185 mecanam[a].run(Stop,1);
kikoaac 0:8719ed91e6fd 186
kikoaac 0:8719ed91e6fd 187 } else if(speed==2) {
kikoaac 0:8719ed91e6fd 188 mecanam[a].duty_max(0.3);
kikoaac 0:8719ed91e6fd 189 mecanam[a].duty_min(-0.3);
kikoaac 0:8719ed91e6fd 190 } else if(speed==3) {
kikoaac 0:8719ed91e6fd 191 mecanam[a].duty_max(1.0);
kikoaac 0:8719ed91e6fd 192 mecanam[a].duty_min(-1.0);
kikoaac 0:8719ed91e6fd 193 }
kikoaac 0:8719ed91e6fd 194 }
kikoaac 0:8719ed91e6fd 195 return speed==0||speed==1;
kikoaac 0:8719ed91e6fd 196 }
kikoaac 0:8719ed91e6fd 197 float Y=1;
kikoaac 0:8719ed91e6fd 198 void f()
kikoaac 0:8719ed91e6fd 199 {
kikoaac 0:8719ed91e6fd 200 static int i;
kikoaac 0:8719ed91e6fd 201 float x[]= {-100,102,503,-4040};
kikoaac 0:8719ed91e6fd 202 Y=x[i];
kikoaac 0:8719ed91e6fd 203 //for(int x=0;x<4;x++)mecanam[x]=x[i];
kikoaac 0:8719ed91e6fd 204 //printf("Target %f\r\n",x[i]);
kikoaac 0:8719ed91e6fd 205 i++;
kikoaac 0:8719ed91e6fd 206 if(i==3)i=0;
kikoaac 0:8719ed91e6fd 207 }
kikoaac 0:8719ed91e6fd 208 int main()
kikoaac 0:8719ed91e6fd 209 {
kikoaac 0:8719ed91e6fd 210 for(int i=0; i<4; i++) {
kikoaac 0:8719ed91e6fd 211 mecanam[i](fusion_Motor::Normal,1);
kikoaac 0:8719ed91e6fd 212 mecanam[i].run(Free,1.0);
kikoaac 0:8719ed91e6fd 213 }
kikoaac 0:8719ed91e6fd 214 for(int a=0;a<4;a++){
kikoaac 0:8719ed91e6fd 215 mecanam[a].duty_max(1.0);
kikoaac 0:8719ed91e6fd 216 mecanam[a].duty_min(-1.0);}
kikoaac 0:8719ed91e6fd 217 led=0.5;
kikoaac 0:8719ed91e6fd 218 //M=0x1f;
kikoaac 0:8719ed91e6fd 219 //wait(3);
kikoaac 0:8719ed91e6fd 220 //setup();
kikoaac 0:8719ed91e6fd 221 device.baud(230400);
kikoaac 0:8719ed91e6fd 222 printf("start main\r\n");
kikoaac 0:8719ed91e6fd 223 //device.attach(&getData);
kikoaac 0:8719ed91e6fd 224 bool pidf=0;
kikoaac 0:8719ed91e6fd 225 bool prev_sot=0;
kikoaac 0:8719ed91e6fd 226
kikoaac 0:8719ed91e6fd 227 wait(1);
kikoaac 0:8719ed91e6fd 228 Ticker tic;
kikoaac 0:8719ed91e6fd 229 tic.attach(&f,3);
kikoaac 0:8719ed91e6fd 230 //mecanam3=200;
kikoaac 0:8719ed91e6fd 231 //for(float i=-1;i<1;i+=0.05)printf("%f\r\n",mecanam3.rerpmper(i));
kikoaac 0:8719ed91e6fd 232 //for(int i = 0 ;i<4;i++)mecanam[i]=100;
kikoaac 0:8719ed91e6fd 233 //wait(5);
kikoaac 0:8719ed91e6fd 234 DigitalIn PIN(USER_BUTTON);
kikoaac 0:8719ed91e6fd 235 while(1) {
kikoaac 0:8719ed91e6fd 236 //printf("start main\r\n");
kikoaac 0:8719ed91e6fd 237
kikoaac 0:8719ed91e6fd 238 for(int x = 0; 1; x++) {
kikoaac 0:8719ed91e6fd 239 if(PIN==0)GAIN_P+=0.001;
kikoaac 0:8719ed91e6fd 240 //printf("start main\r\n");
kikoaac 0:8719ed91e6fd 241 /*vector(Y);
kikoaac 0:8719ed91e6fd 242 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 243 printf("%f ",DUTY[i]);
kikoaac 0:8719ed91e6fd 244 printf("%F \r\n",GAIN_P);*/
kikoaac 0:8719ed91e6fd 245 wait(TIME);
kikoaac 0:8719ed91e6fd 246 //mecanam3=mecCot(i,R);
kikoaac 0:8719ed91e6fd 247 //wait(0.5);
kikoaac 0:8719ed91e6fd 248 //printf("%5d, %5.5f , %5.5f, %5.5f\r\n",i,mecanam3.getrpm(),mecanam3.rerpmper(mecCot(i,R)),mecanam3.rpmper(300*mecCot(i,R)));
kikoaac 0:8719ed91e6fd 249
kikoaac 0:8719ed91e6fd 250 getData();
kikoaac 0:8719ed91e6fd 251 //wait(0.01);
kikoaac 0:8719ed91e6fd 252 if(MotorState())break;
kikoaac 0:8719ed91e6fd 253 //if(sot!=prev_sot);//pid_reset();
kikoaac 0:8719ed91e6fd 254
kikoaac 0:8719ed91e6fd 255 if(sot==0) {
kikoaac 0:8719ed91e6fd 256 prev_sot=sot;
kikoaac 0:8719ed91e6fd 257 float angle = Data2*11.25;
kikoaac 0:8719ed91e6fd 258 vector(angle);
kikoaac 0:8719ed91e6fd 259 } else if(sot==1) {
kikoaac 0:8719ed91e6fd 260 if(Data2==0) {
kikoaac 0:8719ed91e6fd 261 for(int i=0; i<2; i++) {
kikoaac 0:8719ed91e6fd 262 mecanam[i]=1;
kikoaac 0:8719ed91e6fd 263 mecanam[i+2]=-1;
kikoaac 0:8719ed91e6fd 264 }
kikoaac 0:8719ed91e6fd 265 }
kikoaac 0:8719ed91e6fd 266 else if(Data2==1) {
kikoaac 0:8719ed91e6fd 267 for(int i=0; i<2; i++) {
kikoaac 0:8719ed91e6fd 268 mecanam[i]=-1;
kikoaac 0:8719ed91e6fd 269 mecanam[i+2]=1;
kikoaac 0:8719ed91e6fd 270 }
kikoaac 0:8719ed91e6fd 271 } else {
kikoaac 0:8719ed91e6fd 272 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 273 mecanam[i].run(Stop,1);
kikoaac 0:8719ed91e6fd 274 }
kikoaac 0:8719ed91e6fd 275 }
kikoaac 0:8719ed91e6fd 276 }
kikoaac 0:8719ed91e6fd 277 }
kikoaac 0:8719ed91e6fd 278 }
kikoaac 0:8719ed91e6fd 279
kikoaac 0:8719ed91e6fd 280