Dependencies: mbed
Diff: main.cpp
- Revision:
- 1:bf0a0d529479
- Parent:
- 0:9a36dbb02bc8
- Child:
- 2:40cfcd06ac8b
diff -r 9a36dbb02bc8 -r bf0a0d529479 main.cpp --- a/main.cpp Sat Jun 28 06:58:19 2014 +0000 +++ b/main.cpp Sat Jun 28 09:33:24 2014 +0000 @@ -1,7 +1,7 @@ /* * 4WD_OMNI by Tomoki Hokida * Every direction Type - * + * Fujisawaリスペクトの4輪オムニ用ラフコード */ #include "mbed.h" @@ -11,10 +11,15 @@ Serial pc(USBTX,USBRX); DigitalOut check(LED1); -PwmOut wh[4] = {p21,p22,p23,p24}; +DigitalOut motor[8] = {p13,p14,p15,p19,p17,p18,p19,p20}; + +BusInOut sw(p5, p6, p7, p8); + +PwmOut pwm[4] = {p21,p22,p23,p24}; void moveOmni(int ox,int oy) -{ +{ + double trans[4] = {0}; trans[0] = ox*(cos(0.75*PI)) + oy*(sin(0.75*PI)); @@ -24,17 +29,23 @@ for(int i=0;i<4;i++){ - if(trans[i]>100){ - trans[i]=100; - }else if(trans[i]<-100){ - trans[i]=-100; - } - - wh[i] = (trans[i]/100.0); + if(trans[i]>0){ + motor[i*2] = 1; + motor[i*2+1] = 0; + }else{ + motor[i*2+1] = 1; + motor[i*2] = 0; + } + trans[i] = fabs(trans[i]); + if(trans[i]>100){ + trans[i]=100;} + pwm[i] = (trans[i]/100.0); } + pc.printf("motor:%f, motor:%f, motor:%f, motor:%f\n",trans[0],trans[1],trans[2],trans[3]); -} + +} int main() { @@ -43,13 +54,19 @@ for(;;){ ix=0; iy=0; - + + sw.input(); + sw.mode(PullUp); + //ex value - switch(pc.getc()){ - case '1': ix = iy = -100; break; - case '2': ix = iy = 0; break; - case '3': ix = iy = 100; break; - } + switch(sw){ + case 0xF: ix = iy = 0; break; + case 0xE: ix = iy = 100; break; + case 0xD: ix = iy = 50; break; + case 0xB: ix = iy = -100; break; + case 0x7: ix = iy = -50; break; + } + moveOmni(ix,iy); check = !check;