mbedを用いた制御学生の制御
/
Kouryuu_reciverv1
アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。
Diff: main.cpp
- Revision:
- 14:54f198b9947b
- Parent:
- 13:1423effc49b2
- Child:
- 15:02d4a5eed16c
diff -r 1423effc49b2 -r 54f198b9947b main.cpp --- a/main.cpp Tue Mar 17 23:43:14 2015 +0000 +++ b/main.cpp Wed Mar 18 00:19:43 2015 +0000 @@ -125,10 +125,11 @@ sousin.baud(9600); sensor.setOpMode(HMC6352_CONTINUOUS,1,20); int i,ec,motor[3],val; - double asen,sen,aval; + double asen,asen2,sen,aval; int8_t sw1,hun; i = 0; asen = sensor.sample()/10.0; + asen2 = asen; double sx,sy,motp[3],theta; while(1) { //pc.printf("START\n\r"); @@ -150,11 +151,11 @@ t.reset(); t.stop(); } - if ((hun == 1)&& (t.read_ms()>(i*20))){ - asen = asen+1; + if ((hun == 1)&& (t.read_ms()>(i*10))){ + asen = asen+2; i++; - }else if((hun == 2)&& (t.read_ms()>(i*20))){ - asen = asen-1; + }else if((hun == 2)&& (t.read_ms()>(i*10))){ + asen = asen-2; i++; } if (asen > 360){ @@ -167,9 +168,9 @@ val %= 360; val -= 180; printf(" :%d\n\r",val); - if (val > 10){ + if (val > 5){ hun = 2; - }else if (val <-10){ + }else if (val <-5){ hun = 1; }else{ hun = 0; @@ -190,6 +191,7 @@ if (ec == 0){ em.reset(); theta = get_theta(sx,sy); + theta -=asen2; //pc.printf("%f\n\r",theta); if (sw1 == -1){ omni_cont(hun,aval,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);