main.cpp

00001 /*
00002  * 4WD_OMNI  by Tomoki Hokida
00003  * Every direction Type
00004  * Fujisawaリスペクトの4輪オムニ用ラフコード
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 DigitalOut motor[8] = {p13,p14,p15,p16,p17,p18,p19,p20};
00015 
00016 BusInOut sw(p5, p6, p7, p8);
00017 
00018 PwmOut pwm[4] = {p21,p22,p23,p24};
00019 
00020 void moveOmni(int ox,int oy)
00021 {    
00022     double trans[4] = {0};
00023 
00024     trans[0] = ox*(cos(0.75*PI)) + oy*(sin(0.75*PI));
00025     trans[1] = ox*(cos(-0.75*PI)) + oy*(sin(-0.75*PI));
00026     trans[2] = ox*(cos(-0.25*PI)) + oy*(sin(-0.25*PI));
00027     trans[3] = ox*(cos(0.25*PI)) + oy*(sin(0.25*PI));
00028 
00029     for(int i=0;i<4;i++){
00030             if(trans[i]>0){
00031                 motor[i*2] = 1;
00032                 motor[i*2+1] = 0;                               
00033                 }else{
00034                 motor[i*2+1] = 1;
00035                 motor[i*2] = 0;        
00036             }          
00037             trans[i] = fabs(trans[i]);
00038             if(trans[i]>100){
00039             trans[i]=100;}
00040             pwm[i] = (trans[i]/100.0);        
00041     }
00042     pc.printf("motor:%f, motor:%f, motor:%f, motor:%f\n",trans[0],trans[1],trans[2],trans[3]);  
00043 }  
00044 
00045 int main()
00046 {
00047     int ix,iy;
00048 
00049     for(;;){
00050         ix=0;
00051         iy=0;
00052         
00053         sw.input();
00054         sw.mode(PullUp);
00055         
00056         //ex value 
00057         switch(sw){
00058             case 0xF: ix = iy = 0; break;
00059             case 0xE: ix = iy = 100; break;
00060             case 0xD: ix = iy = 50; break;
00061             case 0xB: ix = iy = -100; break;        
00062             case 0x7: ix = iy = -50; break;
00063             }            
00064         moveOmni(ix,iy);          
00065         check = !check; 
00066     }
00067 
00068 }