2015 robotic contest arakawa A
MotorDrive制御
Dependencies: mbed Motor PID QEI2 fusion_Motor
Revision 0:8719ed91e6fd, committed 2015-09-23
- Comitter:
- kikoaac
- Date:
- Wed Sep 23 06:06:49 2015 +0000
- Commit message:
- No mount QEI
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Pin.h Wed Sep 23 06:06:49 2015 +0000 @@ -0,0 +1,8 @@ +#define QEI1_A PB_13 +#define QEI1_B PC_4 +#define QEI2_A PB_15 +#define QEI2_B PB_14 +#define QEI3_A PB_2 +#define QEI3_B PB_1 +#define QEI4_A PA_11 +#define QEI4_B PB_12
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Sep 23 06:06:49 2015 +0000
@@ -0,0 +1,280 @@
+/*
+motor 555
+Lipo 4sel
+Linear accelerator
+Not mount QEI
+*/
+#include "mbed.h"
+#include "Motor.h"
+#include "PID.h"
+#include "QEI.h"
+#include "fusion_Motor.h"
+#include "Pin.h"
+#define Lx 0.0001
+#define Ku 0.32200//0.922000
+#define Pu 0.0125
+#define Kp Ku*0.6
+#define Ti Pu*0.5
+#define Td Pu*0.125
+
+
+char data=0,speed=0,sot=0,Data2=0;
+#define P Kp
+#define I Kp/Ti
+#define D Kp*Td
+#define Mstate 6
+#define SorT 5
+#define Databit 0
+#define TIME 0.0125
+Timer Time;
+/*
+PID p(P,I,D,&Time);
+QEI i(QEI1_A,QEI1_B,NC,50,&Time);
+Motor d(PD_2,PA_13,PA_14,PA_15,PC_9);
+fusion_Motor mecanam3(i,p,d);
+*/
+
+
+Serial device(D8,D2);
+/*
+PID *pid;
+QEI *wheel;
+Motor *motor;
+*/
+PID pid[4] = {PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time),PID(P,I,D,&Time)};
+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)};
+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)};
+fusion_Motor mecanam[4] = {
+ fusion_Motor(wheel[0],pid[0],motor[0]),//front right
+ fusion_Motor(wheel[1],pid[1],motor[1]),//Back right
+ fusion_Motor(wheel[2],pid[2],motor[2]),//front left
+ fusion_Motor(wheel[3],pid[3],motor[3])
+};
+PwmOut led(A0);
+//back left
+//Motor d(D5,D4,D3,D1,D0);
+/*
+void Delete()
+{
+ delete pid;
+ delete wheel;
+ delete motor;
+}
+fusion_Motor *mecanam1;
+fusion_Motor *mecanam2;
+fusion_Motor *mecanam3;
+fusion_Motor *mecanam4;
+void setup()
+{
+ pid = new PID(P,I,D,&Time);
+ wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
+ motor = new Motor(A1,A2,A3,A4,A0);
+ mecanam1 = new fusion_Motor(*wheel,*pid,*motor);
+ Delete();
+ pid = new PID(P,I,D,&Time);
+ wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
+ motor = new Motor(D4,D3,D2,D1,D0);
+ mecanam2 = new fusion_Motor(*wheel,*pid,*motor);
+ Delete();
+ pid = new PID(P,I,D,&Time);
+ wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
+ motor = new Motor(PD_2,PA_13,PA_14,PA_15,PC_9);
+ mecanam3 = new fusion_Motor(*wheel,*pid,*motor);
+ Delete();
+ pid = new PID(P,I,D,&Time);
+ wheel = new QEI(QEI1_A,QEI1_B,NC,100,&Time);
+ motor = new Motor(PC_7,PB_6,PA_7,PA_6,PA_8);
+ mecanam4 = new fusion_Motor(*wheel,*pid,*motor);
+ Delete();
+}
+*/
+/*
+ |
+ L\ /R
+ - -
+ R/ \L
+ |
+*/
+enum STATE {
+ R,L,F,B
+} state;
+PwmOut LED(LED1);
+#define PI 3.141592653589793
+float DUTY_fill[4]= {0};
+float DUTY_inte[4]= {0};
+float DUTY[4]= {0};
+float DUTY_prev[4]= {0};
+float GAIN_P=P,GAIN_I=I,GAIN_D=D;
+void ACT(float tardata,float *data,fusion_Motor *fusion,float *fill,float *inte,float *prev_dat)
+{
+ float inter = TIME;
+ float dat = tardata-*data;
+ *inte += (dat + *prev_dat)/2*inter;
+ *data = GAIN_P*dat+GAIN_D**fill+GAIN_I**inte;
+ *fill = (dat - *prev_dat)*inter;
+ *prev_dat =dat;
+
+ /*
+ if(tardata>*data)
+ *data+=0.01f;
+ if(tardata<*data)
+ *data-=0.01f;*/
+ *fusion=*data;
+}
+void motruns(STATE RorL,float data)
+{
+ if(RorL == R) {
+
+ ACT(data,&DUTY[R],&mecanam[R],&DUTY_fill[R],&DUTY_inte[R],&DUTY_prev[R]);
+ ACT(data,&DUTY[R+2],&mecanam[R+2],&DUTY_fill[R+2],&DUTY_inte[R+2],&DUTY_prev[R+2]);
+ } else if(RorL == L) {
+ ACT(data,&DUTY[L],&mecanam[L],&DUTY_fill[L],&DUTY_inte[L],&DUTY_prev[L]);
+ ACT(data,&DUTY[L+2],&mecanam[L+2],&DUTY_fill[L+2],&DUTY_inte[L+2],&DUTY_prev[L+2]);
+ }
+}
+double mecCot(float angle,STATE RorL)
+{
+ double radian = (double)angle * PI / 180.0;
+ return RorL==R ? 1.4142135623730950488016887242097*sin(radian-PI/4-PI/2) : 1.4142135623730950488016887242097*cos(radian-PI/4-PI/2);
+}
+void vector(float angle)
+{
+ angle-=90-360;
+ if(angle<0)angle+=360;
+ angle-=360;
+ if(angle>0)angle-=360;
+ angle*=-1;
+ motruns(R,mecCot(angle,R));
+ motruns(L,mecCot(angle,L));
+ printf("%5.5f %5.5f %5.5f ",angle , mecCot(angle,L),mecCot(angle,R));
+}
+
+
+void pid_setpoint()
+{
+ for(int i=0; i<4; i++)
+ pid[i].dPoint=wheel[i].getRPM();
+}
+void pid_start()
+{
+ for(int i=0; i<4; i++)
+ pid[i].start();
+}
+void pid_reset()
+{
+ for(int i=0; i<4; i++)
+ pid[i].pid_reset();
+}
+void getData()
+{
+
+ data = device.getc();
+ speed = data>>Mstate;
+ sot = (data>>SorT)&0x01;
+ Data2 = data&0x1f;
+ //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));
+ //printf("0x%x ,0x%x ,0x%x ,0x%x\r\n" ,data,speed,sot,Data2);
+ //printf("getdata!!\r\n");
+}
+bool MotorState()
+{
+ for(int a=0; a<4; a++) {
+ if(speed==0) {
+ mecanam[a].run(Free,1);
+ } else if(speed==1) {
+ mecanam[a].run(Stop,1);
+
+ } else if(speed==2) {
+ mecanam[a].duty_max(0.3);
+ mecanam[a].duty_min(-0.3);
+ } else if(speed==3) {
+ mecanam[a].duty_max(1.0);
+ mecanam[a].duty_min(-1.0);
+ }
+ }
+ return speed==0||speed==1;
+}
+float Y=1;
+void f()
+{
+ static int i;
+ float x[]= {-100,102,503,-4040};
+ Y=x[i];
+ //for(int x=0;x<4;x++)mecanam[x]=x[i];
+ //printf("Target %f\r\n",x[i]);
+ i++;
+ if(i==3)i=0;
+}
+int main()
+{
+ for(int i=0; i<4; i++) {
+ mecanam[i](fusion_Motor::Normal,1);
+ mecanam[i].run(Free,1.0);
+ }
+for(int a=0;a<4;a++){
+ mecanam[a].duty_max(1.0);
+ mecanam[a].duty_min(-1.0);}
+ led=0.5;
+ //M=0x1f;
+ //wait(3);
+ //setup();
+ device.baud(230400);
+ printf("start main\r\n");
+ //device.attach(&getData);
+ bool pidf=0;
+ bool prev_sot=0;
+
+ wait(1);
+ Ticker tic;
+ tic.attach(&f,3);
+ //mecanam3=200;
+ //for(float i=-1;i<1;i+=0.05)printf("%f\r\n",mecanam3.rerpmper(i));
+ //for(int i = 0 ;i<4;i++)mecanam[i]=100;
+ //wait(5);
+ DigitalIn PIN(USER_BUTTON);
+ while(1) {
+ //printf("start main\r\n");
+
+ for(int x = 0; 1; x++) {
+ if(PIN==0)GAIN_P+=0.001;
+ //printf("start main\r\n");
+ /*vector(Y);
+ for(int i=0; i<4; i++)
+ printf("%f ",DUTY[i]);
+ printf("%F \r\n",GAIN_P);*/
+ wait(TIME);
+ //mecanam3=mecCot(i,R);
+ //wait(0.5);
+ //printf("%5d, %5.5f , %5.5f, %5.5f\r\n",i,mecanam3.getrpm(),mecanam3.rerpmper(mecCot(i,R)),mecanam3.rpmper(300*mecCot(i,R)));
+
+ getData();
+ //wait(0.01);
+ if(MotorState())break;
+ //if(sot!=prev_sot);//pid_reset();
+
+ if(sot==0) {
+ prev_sot=sot;
+ float angle = Data2*11.25;
+ vector(angle);
+ } else if(sot==1) {
+ if(Data2==0) {
+ for(int i=0; i<2; i++) {
+ mecanam[i]=1;
+ mecanam[i+2]=-1;
+ }
+ }
+ else if(Data2==1) {
+ for(int i=0; i<2; i++) {
+ mecanam[i]=-1;
+ mecanam[i+2]=1;
+ }
+ } else {
+ for(int i=0; i<4; i++)
+ mecanam[i].run(Stop,1);
+ }
+ }
+ }
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Sep 23 06:06:49 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/8ed44a420e5c \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor_lib/Motor.lib Wed Sep 23 06:06:49 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/kikoaac/code/Motor/#dea2df71cb97
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor_lib/PID.lib Wed Sep 23 06:06:49 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/kikoaac/code/PID/#3519920d064d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor_lib/QEI.lib Wed Sep 23 06:06:49 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/kikoaac/code/QEI2/#49fa8827718d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor_lib/fusion_Motor.lib Wed Sep 23 06:06:49 2015 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/2015-robotic-contest-arakawa-A/code/fusion_Motor/#0d2b4508badd