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@1:ad8acc6cfba7, 2015-10-30 (annotated)

Committer:
kikoaac
Date:
Fri Oct 30 12:48:08 2015 +0000
Revision:
1:ad8acc6cfba7
Parent:
0:8719ed91e6fd 
???

;

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 1:ad8acc6cfba7 14 #define Ku 0.48000//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 1:ad8acc6cfba7 30 DigitalOut Elec(D15);
kikoaac 0:8719ed91e6fd 31 /*
kikoaac 0:8719ed91e6fd 32 PID p(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 33 QEI i(QEI1_A,QEI1_B,NC,50,&Time);
kikoaac 0:8719ed91e6fd 34 Motor d(PD_2,PA_13,PA_14,PA_15,PC_9);
kikoaac 0:8719ed91e6fd 35 fusion_Motor mecanam3(i,p,d);
kikoaac 0:8719ed91e6fd 36 */
kikoaac 0:8719ed91e6fd 37
kikoaac 1:ad8acc6cfba7 38 float duty_max;
kikoaac 0:8719ed91e6fd 39 Serial device(D8,D2);
kikoaac 0:8719ed91e6fd 40 /*
kikoaac 0:8719ed91e6fd 41 PID *pid;
kikoaac 0:8719ed91e6fd 42 QEI *wheel;
kikoaac 0:8719ed91e6fd 43 Motor *motor;
kikoaac 0:8719ed91e6fd 44 */
kikoaac 0:8719ed91e6fd 45 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 46 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 1:ad8acc6cfba7 47 Motor motor[4] = {Motor(A1,A2,A3,A4,A0),Motor(PD_2,PA_13,PA_14,PA_15,PC_9),Motor(D7,D6,D5,D4,D3),Motor(D13,D12,D11,D10,D9)};
kikoaac 0:8719ed91e6fd 48 fusion_Motor mecanam[4] = {
kikoaac 0:8719ed91e6fd 49 fusion_Motor(wheel[0],pid[0],motor[0]),//front right
kikoaac 0:8719ed91e6fd 50 fusion_Motor(wheel[1],pid[1],motor[1]),//Back right
kikoaac 0:8719ed91e6fd 51 fusion_Motor(wheel[2],pid[2],motor[2]),//front left
kikoaac 0:8719ed91e6fd 52 fusion_Motor(wheel[3],pid[3],motor[3])
kikoaac 0:8719ed91e6fd 53 };
kikoaac 0:8719ed91e6fd 54 PwmOut led(A0);
kikoaac 0:8719ed91e6fd 55 //back left
kikoaac 0:8719ed91e6fd 56 //Motor d(D5,D4,D3,D1,D0);
kikoaac 0:8719ed91e6fd 57 /*
kikoaac 0:8719ed91e6fd 58 void Delete()
kikoaac 0:8719ed91e6fd 59 {
kikoaac 0:8719ed91e6fd 60 delete pid;
kikoaac 0:8719ed91e6fd 61 delete wheel;
kikoaac 0:8719ed91e6fd 62 delete motor;
kikoaac 0:8719ed91e6fd 63 }
kikoaac 0:8719ed91e6fd 64 fusion_Motor *mecanam1;
kikoaac 0:8719ed91e6fd 65 fusion_Motor *mecanam2;
kikoaac 0:8719ed91e6fd 66 fusion_Motor *mecanam3;
kikoaac 0:8719ed91e6fd 67 fusion_Motor *mecanam4;
kikoaac 0:8719ed91e6fd 68 void setup()
kikoaac 0:8719ed91e6fd 69 {
kikoaac 0:8719ed91e6fd 70 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 71 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 72 motor = new Motor(A1,A2,A3,A4,A0);
kikoaac 0:8719ed91e6fd 73 mecanam1 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 74 Delete();
kikoaac 0:8719ed91e6fd 75 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 76 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 77 motor = new Motor(D4,D3,D2,D1,D0);
kikoaac 0:8719ed91e6fd 78 mecanam2 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 79 Delete();
kikoaac 0:8719ed91e6fd 80 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 81 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 82 motor = new Motor(PD_2,PA_13,PA_14,PA_15,PC_9);
kikoaac 0:8719ed91e6fd 83 mecanam3 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 84 Delete();
kikoaac 0:8719ed91e6fd 85 pid = new PID(P,I,D,&Time);
kikoaac 0:8719ed91e6fd 86 wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
kikoaac 0:8719ed91e6fd 87 motor = new Motor(PC_7,PB_6,PA_7,PA_6,PA_8);
kikoaac 0:8719ed91e6fd 88 mecanam4 = new fusion_Motor(*wheel,*pid,*motor);
kikoaac 0:8719ed91e6fd 89 Delete();
kikoaac 0:8719ed91e6fd 90 }
kikoaac 0:8719ed91e6fd 91 */
kikoaac 0:8719ed91e6fd 92 /*
kikoaac 0:8719ed91e6fd 93 |
kikoaac 0:8719ed91e6fd 94 L\ /R
kikoaac 0:8719ed91e6fd 95 - -
kikoaac 0:8719ed91e6fd 96 R/ \L
kikoaac 0:8719ed91e6fd 97 |
kikoaac 0:8719ed91e6fd 98 */
kikoaac 0:8719ed91e6fd 99 enum STATE {
kikoaac 0:8719ed91e6fd 100 R,L,F,B
kikoaac 0:8719ed91e6fd 101 } state;
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 1:ad8acc6cfba7 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 1:ad8acc6cfba7 138 return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4) : 1.4142135623730950488016887242097*cos(radian-PI/4);
kikoaac 1:ad8acc6cfba7 139 }
kikoaac 1:ad8acc6cfba7 140 bool stop_flag()
kikoaac 1:ad8acc6cfba7 141 {
kikoaac 1:ad8acc6cfba7 142 bool flag=1;
kikoaac 1:ad8acc6cfba7 143 for(int i=0;i<4;i++)
kikoaac 1:ad8acc6cfba7 144 {
kikoaac 1:ad8acc6cfba7 145 flag &= (DUTY[i] <duty_max*0.8f && DUTY[i]>-duty_max*0.8f);
kikoaac 1:ad8acc6cfba7 146 }
kikoaac 1:ad8acc6cfba7 147 return flag;
kikoaac 0:8719ed91e6fd 148 }
kikoaac 0:8719ed91e6fd 149 void vector(float angle)
kikoaac 0:8719ed91e6fd 150 {
kikoaac 0:8719ed91e6fd 151 angle-=90-360;
kikoaac 0:8719ed91e6fd 152 if(angle<0)angle+=360;
kikoaac 0:8719ed91e6fd 153 angle-=360;
kikoaac 0:8719ed91e6fd 154 if(angle>0)angle-=360;
kikoaac 0:8719ed91e6fd 155 angle*=-1;
kikoaac 0:8719ed91e6fd 156 motruns(R,mecCot(angle,R));
kikoaac 0:8719ed91e6fd 157 motruns(L,mecCot(angle,L));
kikoaac 0:8719ed91e6fd 158 printf("%5.5f %5.5f %5.5f ",angle , mecCot(angle,L),mecCot(angle,R));
kikoaac 0:8719ed91e6fd 159 }
kikoaac 0:8719ed91e6fd 160
kikoaac 0:8719ed91e6fd 161
kikoaac 0:8719ed91e6fd 162 void pid_setpoint()
kikoaac 0:8719ed91e6fd 163 {
kikoaac 0:8719ed91e6fd 164 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 165 pid[i].dPoint=wheel[i].getRPM();
kikoaac 0:8719ed91e6fd 166 }
kikoaac 0:8719ed91e6fd 167 void pid_start()
kikoaac 0:8719ed91e6fd 168 {
kikoaac 0:8719ed91e6fd 169 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 170 pid[i].start();
kikoaac 0:8719ed91e6fd 171 }
kikoaac 0:8719ed91e6fd 172 void pid_reset()
kikoaac 0:8719ed91e6fd 173 {
kikoaac 0:8719ed91e6fd 174 for(int i=0; i<4; i++)
kikoaac 1:ad8acc6cfba7 175 {
kikoaac 1:ad8acc6cfba7 176 DUTY_fill[i]= 0;
kikoaac 1:ad8acc6cfba7 177 DUTY_inte[i]= 0;
kikoaac 1:ad8acc6cfba7 178 DUTY[i]= 0;
kikoaac 1:ad8acc6cfba7 179 DUTY_prev[i]= 0;
kikoaac 1:ad8acc6cfba7 180 }
kikoaac 0:8719ed91e6fd 181 }
kikoaac 0:8719ed91e6fd 182 void getData()
kikoaac 0:8719ed91e6fd 183 {
kikoaac 0:8719ed91e6fd 184
kikoaac 0:8719ed91e6fd 185 data = device.getc();
kikoaac 0:8719ed91e6fd 186 speed = data>>Mstate;
kikoaac 0:8719ed91e6fd 187 sot = (data>>SorT)&0x01;
kikoaac 0:8719ed91e6fd 188 Data2 = data&0x1f;
kikoaac 0:8719ed91e6fd 189 //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 190 //printf("0x%x ,0x%x ,0x%x ,0x%x\r\n" ,data,speed,sot,Data2);
kikoaac 0:8719ed91e6fd 191 //printf("getdata!!\r\n");
kikoaac 0:8719ed91e6fd 192 }
kikoaac 1:ad8acc6cfba7 193 int flag=0;
kikoaac 0:8719ed91e6fd 194 bool MotorState()
kikoaac 0:8719ed91e6fd 195 {
kikoaac 0:8719ed91e6fd 196 for(int a=0; a<4; a++) {
kikoaac 0:8719ed91e6fd 197 if(speed==0) {
kikoaac 1:ad8acc6cfba7 198 mecanam[a].run(Stop,1);
kikoaac 1:ad8acc6cfba7 199 pid_reset();
kikoaac 1:ad8acc6cfba7 200 //mecanam[a].pid_reset();
kikoaac 0:8719ed91e6fd 201 } else if(speed==1) {
kikoaac 1:ad8acc6cfba7 202 if(stop_flag())
kikoaac 1:ad8acc6cfba7 203 {
kikoaac 1:ad8acc6cfba7 204 mecanam[a].run(Stop,1.0);
kikoaac 1:ad8acc6cfba7 205 pid_reset();
kikoaac 1:ad8acc6cfba7 206 }
kikoaac 1:ad8acc6cfba7 207 else
kikoaac 1:ad8acc6cfba7 208 {
kikoaac 1:ad8acc6cfba7 209 motruns(R,0);
kikoaac 1:ad8acc6cfba7 210 motruns(L,0);
kikoaac 1:ad8acc6cfba7 211 }
kikoaac 0:8719ed91e6fd 212 } else if(speed==2) {
kikoaac 1:ad8acc6cfba7 213 duty_max=0.2f;
kikoaac 1:ad8acc6cfba7 214 mecanam[a].duty_max(duty_max);
kikoaac 1:ad8acc6cfba7 215 mecanam[a].duty_min(-duty_max);
kikoaac 0:8719ed91e6fd 216 } else if(speed==3) {
kikoaac 1:ad8acc6cfba7 217 duty_max=1.0f;
kikoaac 1:ad8acc6cfba7 218 mecanam[a].duty_max(duty_max);
kikoaac 1:ad8acc6cfba7 219 mecanam[a].duty_min(-duty_max);
kikoaac 0:8719ed91e6fd 220 }
kikoaac 0:8719ed91e6fd 221 }
kikoaac 0:8719ed91e6fd 222 return speed==0||speed==1;
kikoaac 0:8719ed91e6fd 223 }
kikoaac 0:8719ed91e6fd 224 float Y=1;
kikoaac 0:8719ed91e6fd 225 void f()
kikoaac 0:8719ed91e6fd 226 {
kikoaac 0:8719ed91e6fd 227 static int i;
kikoaac 0:8719ed91e6fd 228 float x[]= {-100,102,503,-4040};
kikoaac 0:8719ed91e6fd 229 Y=x[i];
kikoaac 0:8719ed91e6fd 230 //for(int x=0;x<4;x++)mecanam[x]=x[i];
kikoaac 0:8719ed91e6fd 231 //printf("Target %f\r\n",x[i]);
kikoaac 0:8719ed91e6fd 232 i++;
kikoaac 0:8719ed91e6fd 233 if(i==3)i=0;
kikoaac 0:8719ed91e6fd 234 }
kikoaac 0:8719ed91e6fd 235 int main()
kikoaac 0:8719ed91e6fd 236 {
kikoaac 1:ad8acc6cfba7 237 Elec=0;
kikoaac 0:8719ed91e6fd 238 for(int i=0; i<4; i++) {
kikoaac 0:8719ed91e6fd 239 mecanam[i](fusion_Motor::Normal,1);
kikoaac 0:8719ed91e6fd 240 mecanam[i].run(Free,1.0);
kikoaac 1:ad8acc6cfba7 241 mecanam[i].setbias(0.0);
kikoaac 1:ad8acc6cfba7 242 mecanam[i].setbias_motor(0.0);
kikoaac 0:8719ed91e6fd 243 }
kikoaac 1:ad8acc6cfba7 244 for(int a=0; a<4; a++) {
kikoaac 1:ad8acc6cfba7 245 mecanam[a].duty_max(1.0);
kikoaac 1:ad8acc6cfba7 246 mecanam[a].duty_min(-1.0);
kikoaac 1:ad8acc6cfba7 247 }
kikoaac 0:8719ed91e6fd 248 //M=0x1f;
kikoaac 0:8719ed91e6fd 249 //wait(3);
kikoaac 0:8719ed91e6fd 250 //setup();
kikoaac 0:8719ed91e6fd 251 device.baud(230400);
kikoaac 0:8719ed91e6fd 252 printf("start main\r\n");
kikoaac 0:8719ed91e6fd 253 //device.attach(&getData);
kikoaac 0:8719ed91e6fd 254 bool pidf=0;
kikoaac 0:8719ed91e6fd 255 bool prev_sot=0;
kikoaac 0:8719ed91e6fd 256
kikoaac 0:8719ed91e6fd 257 wait(1);
kikoaac 1:ad8acc6cfba7 258 // Ticker tic;
kikoaac 1:ad8acc6cfba7 259 // tic.attach(&f,3);
kikoaac 0:8719ed91e6fd 260 //mecanam3=200;
kikoaac 0:8719ed91e6fd 261 //for(float i=-1;i<1;i+=0.05)printf("%f\r\n",mecanam3.rerpmper(i));
kikoaac 0:8719ed91e6fd 262 //for(int i = 0 ;i<4;i++)mecanam[i]=100;
kikoaac 0:8719ed91e6fd 263 //wait(5);
kikoaac 0:8719ed91e6fd 264 DigitalIn PIN(USER_BUTTON);
kikoaac 1:ad8acc6cfba7 265 if(PIN==0) {
kikoaac 1:ad8acc6cfba7 266
kikoaac 1:ad8acc6cfba7 267 bool flag=0;
kikoaac 1:ad8acc6cfba7 268 float duty=1;
kikoaac 1:ad8acc6cfba7 269 while(1) {
kikoaac 1:ad8acc6cfba7 270 for(int i=0;i<4;i++)
kikoaac 1:ad8acc6cfba7 271 mecanam[i].run(Front,1);
kikoaac 1:ad8acc6cfba7 272 wait(4);
kikoaac 1:ad8acc6cfba7 273 for(int i=0;i<4;i++)
kikoaac 1:ad8acc6cfba7 274 mecanam[i].run(Front,0);
kikoaac 1:ad8acc6cfba7 275 wait(4);
kikoaac 1:ad8acc6cfba7 276 /*if(PIN==0&&flag==1) {
kikoaac 1:ad8acc6cfba7 277 flag=0;
kikoaac 1:ad8acc6cfba7 278 duty-=0.1;
kikoaac 1:ad8acc6cfba7 279 } else if(PIN==1&&flag==0) {
kikoaac 1:ad8acc6cfba7 280 flag=1;
kikoaac 1:ad8acc6cfba7 281 for(int i=0; i<4; i++)
kikoaac 1:ad8acc6cfba7 282 mecanam[i].run(Front,duty);
kikoaac 1:ad8acc6cfba7 283 }*/
kikoaac 1:ad8acc6cfba7 284 }
kikoaac 1:ad8acc6cfba7 285 }
kikoaac 1:ad8acc6cfba7 286 flag=PIN;
kikoaac 1:ad8acc6cfba7 287 int prevang=0;
kikoaac 1:ad8acc6cfba7 288 Timer waittimer;
kikoaac 0:8719ed91e6fd 289 while(1) {
kikoaac 1:ad8acc6cfba7 290 //Elec=!PIN;
kikoaac 1:ad8acc6cfba7 291 /*
kikoaac 1:ad8acc6cfba7 292 for(int x = 0; 1; x++) {
kikoaac 1:ad8acc6cfba7 293 vector(x);
kikoaac 1:ad8acc6cfba7 294 for(int i=0; i<4; i++)
kikoaac 1:ad8acc6cfba7 295 printf("%f ",mecanam[i].getduty());
kikoaac 1:ad8acc6cfba7 296 printf("\r\n");
kikoaac 1:ad8acc6cfba7 297 }
kikoaac 1:ad8acc6cfba7 298 */
kikoaac 1:ad8acc6cfba7 299 //printf("start main\r\n");
kikoaac 0:8719ed91e6fd 300 for(int x = 0; 1; x++) {
kikoaac 0:8719ed91e6fd 301 if(PIN==0)GAIN_P+=0.001;
kikoaac 1:ad8acc6cfba7 302 //printf("start main\r\n");
kikoaac 1:ad8acc6cfba7 303 /*
kikoaac 1:ad8acc6cfba7 304 vector(Y);
kikoaac 1:ad8acc6cfba7 305 for(int i=0; i<4; i++)
kikoaac 1:ad8acc6cfba7 306 printf("%f ",DUTY[i]);
kikoaac 1:ad8acc6cfba7 307 printf("%F \r\n",GAIN_P);*/
kikoaac 0:8719ed91e6fd 308 wait(TIME);
kikoaac 0:8719ed91e6fd 309 //mecanam3=mecCot(i,R);
kikoaac 0:8719ed91e6fd 310 //wait(0.5);
kikoaac 0:8719ed91e6fd 311 //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 1:ad8acc6cfba7 312
kikoaac 0:8719ed91e6fd 313 getData();
kikoaac 0:8719ed91e6fd 314 //wait(0.01);
kikoaac 0:8719ed91e6fd 315 if(MotorState())break;
kikoaac 0:8719ed91e6fd 316 //if(sot!=prev_sot);//pid_reset();
kikoaac 0:8719ed91e6fd 317
kikoaac 0:8719ed91e6fd 318 if(sot==0) {
kikoaac 0:8719ed91e6fd 319 prev_sot=sot;
kikoaac 0:8719ed91e6fd 320 float angle = Data2*11.25;
kikoaac 1:ad8acc6cfba7 321 prevang=Data2;
kikoaac 0:8719ed91e6fd 322 vector(angle);
kikoaac 0:8719ed91e6fd 323 } else if(sot==1) {
kikoaac 0:8719ed91e6fd 324 if(Data2==0) {
kikoaac 0:8719ed91e6fd 325 for(int i=0; i<2; i++) {
kikoaac 0:8719ed91e6fd 326 mecanam[i]=1;
kikoaac 0:8719ed91e6fd 327 mecanam[i+2]=-1;
kikoaac 0:8719ed91e6fd 328 }
kikoaac 1:ad8acc6cfba7 329 } else if(Data2==1) {
kikoaac 0:8719ed91e6fd 330 for(int i=0; i<2; i++) {
kikoaac 0:8719ed91e6fd 331 mecanam[i]=-1;
kikoaac 0:8719ed91e6fd 332 mecanam[i+2]=1;
kikoaac 0:8719ed91e6fd 333 }
kikoaac 1:ad8acc6cfba7 334 } else if(Data2==2){
kikoaac 1:ad8acc6cfba7 335 Elec=1;
kikoaac 1:ad8acc6cfba7 336 } else if(Data2==3){
kikoaac 1:ad8acc6cfba7 337 Elec=0;
kikoaac 0:8719ed91e6fd 338 } else {
kikoaac 0:8719ed91e6fd 339 for(int i=0; i<4; i++)
kikoaac 0:8719ed91e6fd 340 mecanam[i].run(Stop,1);
kikoaac 0:8719ed91e6fd 341 }
kikoaac 0:8719ed91e6fd 342 }
kikoaac 0:8719ed91e6fd 343 }
kikoaac 0:8719ed91e6fd 344 }
kikoaac 0:8719ed91e6fd 345 }
kikoaac 0:8719ed91e6fd 346
kikoaac 0:8719ed91e6fd 347
kikoaac 1:ad8acc6cfba7 348