Diff: main.cpp

Revision:

27:fb1298d35bd1

Parent:

26:0ea6a550a99d

Child:

28:65daccc10f31

--- a/main.cpp	Wed Apr 06 04:04:00 2016 +0000
+++ b/main.cpp	Wed Apr 13 03:14:43 2016 +0000
@@ -3,7 +3,7 @@
 #include "SAFETY.h"
 
 #define WATCHDOG_TIME   10
-#define PC_MODE         1
+//#define PC_MODE         1
 
 #if defined (PC_MODE)
 Serial pc(USBTX, USBRX);
@@ -20,16 +20,17 @@
 
 SAFETY safe;
 PwmOut suction(p26);
-I2C i2c1(p28, p27);
-I2C i2c2(p9, p10);
+I2C i2c2(p28, p27);
+I2C i2c1(p9, p10);
 LOCALIZE loc(i2c1, i2c2, p29, p8, p7, p6, p5, led1, led2, led3, led4);
 LOCALIZE_xya xya;
-LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4);
+LOCOMOTION motion(p11, p23, p24, p21, p22, p16, p15, led1, led2, led3, led4);
+//LOCOMOTION motion(p11, p21, p22, p23, p24, p15, p16, led1, led2, led3, led4);
 
 void BrownOut();
 
 int xTarget=20;
-int angle_error=2;
+int angle_error=5;
 bool xGood=false;
 bool yGood=false;
 bool angleGood=false;
@@ -49,6 +50,9 @@
     suction.period_ms(2);
     //Initialize to 0
     suction.pulsewidth_us(1000);
+    wait(2);
+    /*suction.pulsewidth_us(1000);
+    wait(2);*/
     //Watchdog Reset Indicator
     if ((LPC_WDT->WDMOD>>2)&1)
         led4=1;
@@ -71,8 +75,17 @@
     led3=0;
     led4=0;
     while(1) {
-        suction.pulsewidth_us(2000);
-        if (yTarget>120) {
+        suction.pulsewidth_us(1600);
+        loc.get_angle(&xya);
+        motion.setAngle(90,xya.a,angle_error,ANGLE_TURN);
+        //motion.moveF(xya.a);
+        /*if(abs(xya.a-180)<90)
+            motion.moveB();
+        else
+            motion.moveB();*/
+        //loc.get_xy(&xya);
+        
+        /*if (yTarget>120) {
             motion.mStop();
             safe.kick();
             continue;
@@ -88,7 +101,7 @@
             motion.setXPos(xTarget,xya.x,2,angleTarget);
             motion.setYBias(yTarget,xya.y,2,angleTarget);
 
-        }
+        }*/
 #if defined(PC_MODE)
         pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
 #endif