main.cpp

00001 /*
00002  * 4WD_OMNI  by Tomoki Hokida
00003  * Every direction Type
00004  *
00005  */
00006 
00007 #include "mbed.h"
00008 #include <math.h>
00009 
00010 #define PI 3.141592
00011 
00012 Serial pc(USBTX,USBRX);
00013 DigitalOut check(LED1);
00014 PwmOut wh[4] = {p21,p22,p23,p24};
00015 
00016 void moveOmni(int ox,int oy)
00017 {
00018     double trans[4] = {0};
00019 
00020     trans[0] = ox*(cos(0.75*PI)) + oy*(sin(0.75*PI));
00021     trans[1] = ox*(cos(-0.75*PI)) + oy*(sin(-0.75*PI));
00022     trans[2] = ox*(cos(-0.25*PI)) + oy*(sin(-0.25*PI));
00023     trans[3] = ox*(cos(0.25*PI)) + oy*(sin(0.25*PI));
00024 
00025     for(int i=0;i<4;i++){
00026 
00027         if(trans[i]>100){
00028             trans[i]=100; 
00029         }else if(trans[i]<-100){
00030             trans[i]=-100;
00031         }
00032 
00033         wh[i] = (trans[i]/100.0);        
00034     }
00035 
00036     pc.printf("motor:%f, motor:%f, motor:%f, motor:%f\n",trans[0],trans[1],trans[2],trans[3]);
00037 }
00038 
00039 int main()
00040 {
00041     int ix,iy;
00042 
00043     for(;;){
00044         ix=0;
00045         iy=0;
00046 
00047         //ex value 
00048         switch(pc.getc()){
00049             case '1': ix = iy = -100; break;
00050             case '2': ix = iy = 0; break;
00051             case '3': ix = iy = 100; break;        
00052         }    
00053         moveOmni(ix,iy);  
00054         
00055         check = !check; 
00056     }
00057 
00058 }