Dependencies: mbed
main.cpp
- Committer:
- Hatter
- Date:
- 2014-07-01
- Revision:
- 4:076faa2578a4
- Parent:
- 2:40cfcd06ac8b
File content as of revision 4:076faa2578a4:
/* * 4WD_OMNI by Tomoki Hokida * Every direction Type * Fujisawaリスペクトの4輪オムニ用ラフコード */ #include "mbed.h" #include <math.h> #define PI 3.141592 Serial pc(USBTX,USBRX); DigitalOut check(LED1); DigitalOut motor[8] = {p13,p14,p15,p16,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)); trans[1] = ox*(cos(-0.75*PI)) + oy*(sin(-0.75*PI)); trans[2] = ox*(cos(-0.25*PI)) + oy*(sin(-0.25*PI)); trans[3] = ox*(cos(0.25*PI)) + oy*(sin(0.25*PI)); for(int i=0;i<4;i++){ 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() { int ix,iy; for(;;){ ix=0; iy=0; sw.input(); sw.mode(PullUp); //ex value 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; } }