Diff: main.cpp

Revision:

15:7729da55873a

Parent:

14:4839989ae907

Child:

16:d6f15a13c3aa

--- a/main.cpp	Thu Mar 24 03:54:29 2016 +0000
+++ b/main.cpp	Thu Mar 24 07:20:51 2016 +0000
@@ -2,9 +2,6 @@
 #include "LOCOMOTION.h"
 #include "WATCHDOG.h"
 
-
-#define SPEED_TURN_MIN  0.15
-#define SPEED_TURN_MAX  0.35
 #define SPEED_FB_MIN    0.15
 #define SPEED_FB_MAX    0.50
 
@@ -15,41 +12,29 @@
 
 I2C i2c1(p28, p27);
 I2C i2c2(p9, p10);
-LOCALIZE loc(i2c1, i2c2, p26);
+LOCALIZE loc(i2c1, i2c2, p26, p20, p19, p18, p17);
 LOCALIZE_xya xya;
-
-DigitalOut dir1(p15);
-DigitalOut dir2(p16);
-
-PwmOut motor1F(p21);
-PwmOut motor1B(p22);
-PwmOut motor2F(p23);
-PwmOut motor2B(p24);
+LOCOMOTION motion(p21, p22, p23, p24, p15, p16);
 
 Ticker t;
 bool flag=false;
 
 void send();
-void setAngle(int angle);
+//void setAngle(int angle);
 int wrap(int a);
 
 int main()
 {
     wdt.kick(5);
     pc.baud(9600);
-    dir1=0;
-    dir2=0;
-    motor1F=0;
-    motor1B=0;
-    motor2F=0;
-    motor2B=0;
     //pc.printf("Initialized Localization: %d\n",loc.init());
     t.attach(&send,1);
     while(1) {
+        motion.setAngle(0,0,5,ANGLE_TURN);
         //loc.get_angle(&xya);
         loc.get_xy(&xya);
         //pc.printf("X: %3d\tY: %3d\tP: %3d\n",xya.x,xya.y,xya.a);
-        setAngle(0);
+        //setAngle(0);
         wdt.kick();
     }
 }
@@ -58,7 +43,7 @@
 {
     pc.printf("%c%c%c%c\n",(char)xya.x,(char)xya.y,xya.a/10,xya.a%10);
 }
-
+/*
 void setAngle(int angle)
 {
     float s = 0;
@@ -84,7 +69,7 @@
         motor2F=0;
         motor2B=0;
     }
-}
+}*/
 
 int wrap(int a)
 {