Diff: main.cpp

Revision:

26:0ea6a550a99d

Parent:

25:f3bf8351bbd4

Child:

27:fb1298d35bd1

--- a/main.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/main.cpp	Wed Apr 06 04:04:00 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);
@@ -35,7 +35,7 @@
 bool angleGood=false;
 int xState=X_INCREASE;
 int angleTarget=0;
-int yTarget=100;
+int yTarget=30;
 
 //bool flag=false;
 //int target=20;
@@ -60,6 +60,8 @@
     safe.init((unsigned)BrownOut);
     //Serial Baudrate
     pc.baud(9600);
+    //Initialize Locomotion
+    loc.init();
     //Attach Periodic Wireless Printing
 #if not defined(PC_MODE)
     t.attach(&send,1);
@@ -68,13 +70,18 @@
     led2=0;
     led3=0;
     led4=0;
-    suction.pulsewidth_us(1000);
     while(1) {
-        //suction.pulsewidth_us(1900);
+        suction.pulsewidth_us(2000);
+        if (yTarget>120) {
+            motion.mStop();
+            safe.kick();
+            continue;
+        }
+
         loc.get_xy(&xya);
         motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);
-        
-        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget);
+
+        motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error);
         motion.setAngleTol(&angle_error,yGood,xGood);
         motion.setYgoal(xGood,angleGood,yGood,&yTarget);
         if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
@@ -82,21 +89,8 @@
             motion.setYBias(yTarget,xya.y,2,angleTarget);
 
         }
-        /*if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
-            xGood = motion.setXPos(target,xya.x,2,angleTarget);
-            if(motion.setYBias(130,xya.y,2,angleTarget)) {
-                angle_error=2;
-            } else if(xGood) {
-                target=target==20?80:20;
-                angleTarget=angleTarget==0?180:0;
-                angle_error=2;
-            } else
-                angle_error=10;
-        }*/
-
-
 #if defined(PC_MODE)
-        pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.x,xya.y,xya.a,xGood,angleGood,xState);
+        pc.printf("X: %3d\tY: %3d\tP: %3d\txGood: %d\tAngleGood: %d\tState: %d \n",xya.y,yTarget,xya.a,xGood,angleGood,xState);
 #endif
         //Feed the dog
         safe.kick();