mbedを用いた制御学生の制御
/
Kouryuu_reciverv1
アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。
Diff: main.cpp
- Revision:
- 13:1423effc49b2
- Parent:
- 12:12dca00d1bca
- Child:
- 14:54f198b9947b
diff -r 12dca00d1bca -r 1423effc49b2 main.cpp --- a/main.cpp Tue Mar 17 00:30:30 2015 +0000 +++ b/main.cpp Tue Mar 17 23:43:14 2015 +0000 @@ -82,22 +82,22 @@ check = 1; while(1){} } -void omni_cont(int hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3) +void omni_cont(int hun,double aval,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3) { double md1,md2,md3,t1; if (hun == 2) { - t1 = 0.6; + t1 = (fabs(aval)/180)*0.3; }else if(hun == 1) { - t1 = -0.6; + t1 = -(fabs(aval)/180)*0.3; }else{ t1 = 0; } - md1 = -sin(theta)*0.4+t1; - md2 = sin(theta-pi*2/3)*0.4+t1; - md3 = sin(theta-pi*4/3)*0.4+t1; + md1 = sin(theta)*0.7+t1; + md2 = sin(theta-pi*2/3)*0.7+t1; + md3 = sin(theta-pi*4/3)*0.7+t1; if (md1 < 0){ *m1 = 2; *mp1 = -md1; @@ -124,15 +124,16 @@ int main() { sousin.baud(9600); sensor.setOpMode(HMC6352_CONTINUOUS,1,20); - int i,ec,motor[3],sen; - double asen; + int i,ec,motor[3],val; + double asen,sen,aval; int8_t sw1,hun; i = 0; asen = sensor.sample()/10.0; double sx,sy,motp[3],theta; while(1) { //pc.printf("START\n\r"); - + sen = sensor.sample()/10.0; + //pc.printf("%f\n\r",sen); hun = hunter; check2 = hun; ec = get_serial(&sx,&sy,&sw1); @@ -149,10 +150,10 @@ t.reset(); t.stop(); } - if ((hun == 1)&& (t.read_ms()>(i*200))){ + if ((hun == 1)&& (t.read_ms()>(i*20))){ asen = asen+1; i++; - }else if((hun == 2)&& (t.read_ms()>(i*200))){ + }else if((hun == 2)&& (t.read_ms()>(i*20))){ asen = asen-1; i++; } @@ -162,13 +163,26 @@ if (asen < 0){ asen = 360; } - if (sen > asen+20){ + val = sen+540-asen; + val %= 360; + val -= 180; + printf(" :%d\n\r",val); + if (val > 10){ + hun = 2; + }else if (val <-10){ hun = 1; - }else if (sen <asen-20){ + }else{ + hun = 0; + } + aval = val; + /*if(sen> asen-180){ hun = 2; + }else if (sen< asen-180){ + hun = 1; }else { hun = 0; } + */ //pc.printf("asen-->%f",asen); ec = 0; //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1); @@ -178,7 +192,7 @@ theta = get_theta(sx,sy); //pc.printf("%f\n\r",theta); if (sw1 == -1){ - omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]); + omni_cont(hun,aval,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]); mo1 = motor[0]; mo2 = motor[1]; mo3 = motor[2]; @@ -187,20 +201,20 @@ mop3 = motp[2]; }else - if (hun == 1){ + if (hun == 2){ mo1 = 1; mo2 = 1; mo3 = 1; - mop1 = 0.5; - mop2 = 0.5; - mop3 = 0.5; - }else if(hun == 2){ + mop1 = (fabs(aval)/180); + mop2 = (fabs(aval)/180); + mop3 = (fabs(aval)/180); + }else if(hun == 1){ mo1 = 2; mo2 = 2; mo3 = 2; - mop1 = 0.5; - mop2 = 0.5; - mop3 = 0.5; + mop1 = (fabs(aval)/180); + mop2 = (fabs(aval)/180); + mop3 = (fabs(aval)/180); }else { mo1 = 0; mo2 = 0;