Revision 1:bf0a0d529479, committed 2014-06-28

Comitter:

Hatter

Date:

Sat Jun 28 09:33:24 2014 +0000

Parent:

0:9a36dbb02bc8

Child:

2:40cfcd06ac8b

Commit message:

Oooops!!

Changed in this revision

--- 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;