Diff: main.cpp

Revision:

6:0602a9e8118b

Parent:

5:c89308dc1827

Child:

7:d6dca30f7959

--- a/main.cpp	Sun Mar 06 18:31:59 2016 +0000
+++ b/main.cpp	Wed Mar 16 22:05:23 2016 +0000
@@ -1,27 +1,179 @@
 #include "LOCALIZE.h"
+#include "LOCOMOTION.h"
+#include "WATCHDOG.h"
 
-//Serial pc(p13, p14);
-Serial pc(USBTX, USBRX);
+#define SPEED_TURN_MIN  0.15
+#define SPEED_TURN_MAX  0.35
+#define SPEED_FB_MIN    0.15
+#define SPEED_FB_MAX    0.50
+
+Serial pc(p13, p14);
+//Serial pc(USBTX, USBRX);
+
+Watchdog wdt;
 
 I2C i2c1(p28, p27);
 I2C i2c2(p9, p10);
 LOCALIZE loc(i2c1, i2c2, p26);
 LOCALIZE_xya xya;
 
+DigitalIn sw1(p20);
+DigitalIn sw2(p19);
+DigitalIn sw3(p18);
+DigitalIn sw4(p17);
+DigitalOut dir1(p15);
+DigitalOut dir2(p16);
+
+PwmOut motor1F(p21);
+PwmOut motor1B(p22);
+PwmOut motor2F(p23);
+PwmOut motor2B(p24);
+
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
+Ticker t;
+bool flag=false;
+
+void send();
+void turn(int angle);
+
 int main()
 {
+    wdt.kick(5);
     pc.baud(9600);
-    pc.printf("Initialized Localization: %d\n",loc.init());
+    dir1=0;
+    dir2=0;
+    motor1F=0;
+    motor1B=0;
+    motor2F=0;
+    motor2B=0;
+    //pc.printf("Initialized Localization: %d\n",loc.init());
+    t.attach(&send,1);
+    sw1.mode(PullUp);
+    sw2.mode(PullUp);
+    sw3.mode(PullUp);
+    sw4.mode(PullUp);
+    while(abs(xya.a-180)>5) {
+        loc.get_angle(&xya);
+        turn(180);
+    }
     while(1) {
-        //loc.get_angle(&xya);
-        loc.get_xy(&xya);
-        pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
+        led1=!sw1;
+        led2=!sw2;
+        led3=!sw3;
+        led4=!sw4;
+        loc.get_angle(&xya);
+        //loc.get_xy(&xya);
+        loc.get_raw_xy();
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
-        //pc.printf("%c%c%c%c\n",xya.x,xya.y,xya.a/10,xya.a%10);
+        /*if(loc._rx_n>20) {
+            dir1=1;
+            dir2=1;
+        } else if(loc._rx_n<10) {
+            dir1=0;
+            dir2=0;
+        }*/
+
+        if((!sw1==1 && !sw2==1) && flag) {
+            motor1F=0.3;
+            motor1B=0.3;
+            motor2F=0.3;
+            motor2B=0.3;
+            dir1=0;
+            dir2=0;
+            wait(0.5);
+            while((xya.a>5 && xya.a<355)) {
+                loc.get_angle(&xya);
+                turn(0);
+            }
+            flag=false;
+        } else if((!sw1==1 && !sw2==1) && !flag) {
+            motor1F=0.3;
+            motor1B=0.3;
+            motor2F=0.3;
+            motor2B=0.3;
+            dir1=0;
+            dir2=0;
+            wait(0.5);
+            while(abs(xya.a-180)>=5) {
+                loc.get_angle(&xya);
+                turn(180);
+            }
+            flag=true;
+        } else {
+            float s=0;
+            if(loc._rx_n<=10)
+                s=0.1;
+            else if(loc._rx_n<=30)
+                s=0.4*abs(loc._rx_n-30)/30+0.1;
+            else
+                s=0.5;
+            motor1F=s;
+            motor1B=s;
+            motor2F=s;
+            motor2B=s;
+            dir1=1;
+            dir2=1;
+        }
+
+        wdt.kick();
     }
 }
+
+void send()
+{
+    pc.printf("%c%c%c%c\n",(char)loc._rx_n,(char)loc._ry_n,xya.a/10,xya.a%10);
+}
+
+void turn(int angle)
+{
+    float s = 0;
+    if(angle==180) {
+        if(abs(xya.a-angle)<=5)
+            s=SPEED_TURN_MIN;
+        else
+            s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN;
+        motor1F=s;
+        motor1B=s;
+        motor2F=s;
+        motor2B=s;
+        if(xya.a>angle+5) {
+            dir1=1;
+            dir2=0;
+        } else if(xya.a<angle-5) {
+            dir1=0;
+            dir2=1;
+        } else {
+            motor1F=0;
+            motor1B=0;
+            motor2F=0;
+            motor2B=0;
+        }
+    } else if(angle==0) {
+        if(xya.a>angle+5 && xya.a<180) {
+            s=(SPEED_TURN_MAX*abs(xya.a-angle)/180)+SPEED_TURN_MIN;
+            motor1F=s;
+            motor1B=s;
+            motor2F=s;
+            motor2B=s;
+            dir1=1;
+            dir2=0;
+        } else if(xya.a>180 && xya.a<355) {
+            s=(SPEED_TURN_MAX*abs(xya.a-359)/180)+SPEED_TURN_MIN;
+            motor1F=s;
+            motor1B=s;
+            motor2F=s;
+            motor2B=s;
+            dir1=0;
+            dir2=1;
+        } else {
+            motor1F=0;
+            motor1B=0;
+            motor2F=0;
+            motor2B=0;
+        }
+    }
+}
\ No newline at end of file