bousiya03
/
4wd_omni
Fujisawaコードをリスペクトした4輪オムニのラフコードだよ
Fork of 4wd_omni by
main.cpp
- Committer:
- Hatter
- Date:
- 2014-06-28
- Revision:
- 0:9a36dbb02bc8
File content as of revision 0:9a36dbb02bc8:
/* * 4WD_OMNI by Tomoki Hokida * Every direction Type * */ #include "mbed.h" #include <math.h> #define PI 3.141592 Serial pc(USBTX,USBRX); DigitalOut check(LED1); PwmOut wh[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]>100){ trans[i]=100; }else if(trans[i]<-100){ trans[i]=-100; } wh[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; //ex value switch(pc.getc()){ case '1': ix = iy = -100; break; case '2': ix = iy = 0; break; case '3': ix = iy = 100; break; } moveOmni(ix,iy); check = !check; } }