Diff: main.cpp

Revision:

0:65eb28b8a9ae

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 16 08:54:31 2015 +0000
@@ -0,0 +1,90 @@
+#include "mbed.h"
+#define pi 3.1415
+AnalogIn stx(p20);
+AnalogIn sty(p19);
+DigitalIn sw1 (p11);
+Serial pc(USBTX,USBRX);
+BusOut m1(p5,p6);
+BusOut m2(p7,p8);
+BusOut m3(p9,p10);
+PwmOut mp1(p21);
+PwmOut mp2(p22);
+PwmOut mp3(p23);
+double get_theta(double stickx,double sticky)
+{
+    double x,y,theta;
+    if (stickx>0.5){
+        x = stickx - 0.5;
+    }else if(stickx <= 0.5){
+        x = -(0.5 - stickx);
+    }else{
+        x = 0;
+    }
+    if (sticky>0.5){
+        y = sticky - 0.5;
+    }else if(sticky <= 0.5){
+        y = -(0.5 - sticky);
+    }else{
+        y = 0;
+    }
+    if(!(x==0 && y ==0)){
+        theta =atan(y/x);
+    }else {
+            
+       theta = 0;
+    }
+    if(x>0 && y > 0){
+        theta = 3.141592+theta;
+    }else
+    if(x>0 && y < 0){
+       theta = pi+theta;
+    }else
+    if(x<0 && y < 0){
+        theta = theta;
+    }else{
+     theta = 2*pi+theta;
+    }
+    return theta;
+}
+void omni_cont(double theta)
+{
+    double md1,md2,md3;
+    md1 = sin(theta-pi/3);
+    md2 = sin(theta-pi);
+    md3 = sin(theta-pi*5/3);
+    if (md1 < 0){
+        m1 = 2;
+        mp1 = -md1;
+    }else{
+        m1 = 1;
+        mp1 = md1;
+    } 
+    if (md2 < 0){
+        m2 = 2;
+        mp2 = -md2;
+    }else{
+        m2 = 1;
+        mp2 = md2;
+    } 
+    if (md3 < 0){
+        m3 = 2;
+        mp3 = -md3;
+    }else{
+        m3 = 1;
+        mp3 = md3;
+    }
+} 
+int main() {
+    sw1.mode(PullUp);
+    double theta;   
+    while(1) {
+        theta = get_theta(stx,sty);
+        if(sw1 == 0){
+            omni_cont(theta);
+        }else{
+            m1 = 0;
+            m2 = 0;
+            m3 = 0;
+        }
+    }
+}