Diff: main.cpp

Revision:

0:9a36dbb02bc8

Child:

1:bf0a0d529479

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jun 28 06:58:19 2014 +0000
@@ -0,0 +1,58 @@
+/*
+ * 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; 
+    }
+
+}
\ No newline at end of file