Diff: main.cpp

Revision:

0:2ac89e0419ac

Child:

1:252fd967389e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 02 07:27:41 2015 +0000
@@ -0,0 +1,173 @@
+#include "mbed.h"
+#define pi 3.141592653589793
+Serial sousin(p9,p10);
+Serial hunter(p28,p27);
+Serial pc(USBTX,USBRX);
+Timer em;
+DigitalOut  check (LED1);
+BusOut mo1 (p13,p14);
+BusOut mo2 (p15,p16);
+BusOut mo3 (p17,p18);
+PwmOut mop1 (p21);
+PwmOut mop2 (p22);
+PwmOut mop3 (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 = pi+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;
+}
+int get_serial(double *stx,double *sty,int8_t *sw)
+{   
+    int error,x,y;
+    if (sousin.getc() == 255)
+    {
+        x = sousin.getc();
+        y = sousin.getc();
+        *sw = sousin.getc();
+        error = sousin.getc();
+        *stx = x/100.0;
+        *sty = y/100.0; 
+        if (error == x^y^(*sw)){
+            em.reset();
+            //pc.printf("sw-->%d\n\r",*sw);
+            return 0;
+        }else{
+            return 1;
+        }
+    }else{
+        return 1;
+    }
+}
+void allstop()
+{
+    mo1 = 0;
+    mo2 = 0;
+    mo3 = 0;
+    check = 1;
+    while(1){}
+}
+void omni_cont(int8_t hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
+{   
+    
+    double md1,md2,md3,t1;
+    if (hun == 2)
+    {
+        t1 = 0.1;
+    }else if(hun == 1)
+    {
+        t1 = -0.1;
+    }else{
+        t1 = 0;
+    }
+    md1 = sin(theta)*0.9+t1;
+    md2 = sin(theta-pi*2/3)*0.9+t1;
+    md3 = sin(theta-pi*4/3)*0.9+t1;
+    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;
+    }
+} 
+int8_t get_hunter()
+{
+    int8_t s;
+    s = 0;
+    if (hunter.getc() == 48){
+        s = hunter.getc();
+    }
+    if (s == 255)
+    {
+        allstop();
+    }
+    pc.printf("s-->%d\n\r",s);
+    return s;
+}
+int main() {
+    hunter.baud(9600);
+    sousin.baud(9600);
+    int i,ec,motor[3];
+    int8_t sw1,hun;
+    double sx,sy,motp[3],theta;
+    while(1) {
+        //pc.printf("START\n\r");
+        i = 0;
+        hun = get_hunter();
+        ec = get_serial(&sx,&sy,&sw1);
+        ec =0;
+        //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1);
+        //pc.printf("%d\n\r",hun);
+        if (ec == 0){
+            em.reset();
+            theta = get_theta(sx,sy);
+            if (sw1 == -1){
+                omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
+            }else{
+                motor[0] = 0;
+                motor[1] = 0;
+                motor[2] = 0;
+                motp[0] = 0;
+                motp[1] = 0;
+                motp[2] = 0;
+            }
+            
+            mo1 = motor[0];
+            mo2 = motor[1];
+            mo3 = motor[2];
+            mop1 = motp[0];
+            mop2 = motp[1];
+            mop3 = motp[2];
+        }else{
+            em.start();
+            //if(em.read_ms()>200){
+            //    allstop();
+            //}
+        }                     
+    }
+}