Diff: main.cpp

Revision:

26:5ae5ef623b72

Parent:

25:f3bf8351bbd4

diff -r f3bf8351bbd4 -r 5ae5ef623b72 main.cpp
--- a/main.cpp	Tue Apr 05 02:30:40 2016 +0000
+++ b/main.cpp	Wed Apr 06 03:54:29 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;
@@ -70,11 +70,16 @@
     led4=0;
     suction.pulsewidth_us(1000);
     while(1) {
+        if (yTarget>120){
+        motion.mStop();
+        safe.kick();
+        continue;
+        }
         //suction.pulsewidth_us(1900);
         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)) {
@@ -96,7 +101,7 @@
 
 
 #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();