Revision 36:dc69442c4c47, committed 2016-04-25

Comitter:

12104404

Date:

Mon Apr 25 17:47:51 2016 +0000

Parent:

35:68917796c30a

Child:

37:e8a6ea255c09

Commit message:

DERPS

Changed in this revision

--- a/LOCOMOTION.cpp	Sun Apr 24 22:37:07 2016 +0000
+++ b/LOCOMOTION.cpp	Mon Apr 25 17:47:51 2016 +0000
@@ -50,7 +50,7 @@
     _en=0;
 }
 
-bool LOCOMOTION::setXPos(int target, int current, int error, int angle, int curr_angle)
+bool LOCOMOTION::setXPos(int target, int current, int error, int angle)
 {
     /*if(abs(current-target)<=error)
         s=SPEED_X_MIN;
@@ -62,11 +62,11 @@
         s=SPEED_X_MAX;
     if(current>target+error) {
         if(angle==0) {
+            _m1dir=1;
+            _m2dir=1;
+        } else {
             _m1dir=0;
             _m2dir=0;
-        } else {
-            _m1dir=1;
-            _m2dir=1;
         }
         setMotors(s);
     } else if(current<target-error) {
@@ -82,7 +82,6 @@
         setMotors(0);
         return true;
     }
-    //setMotors12(compG(s,~_m1dir,curr_angle),compG(s,~_m2dir,curr_angle));
     return false;
 }
 
@@ -147,23 +146,25 @@
             break;
         case ANGLE_BIAS:
             if(wrap(current+diff)>180+error) {
-                //_m1dir=0;
-                //_m2dir=1;
-                setMotors12(s+a,s*0.6);
+                if(_m1dir==0)
+                    setMotors12(s+(a*0.75),s);
+                else
+                    setMotors12(s,s+(a*0.75));
                 //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
                 //_m2dir=0;
             } else if(wrap(current+diff)<180-error) {
                 //_m1dir=1;
                 //_m2dir=0;
-                setMotors12(s,s+a*0.1);
+                if(_m1dir==0)
+                    setMotors12(s,s+(a*0.75));
+                else
+                    setMotors12(s+(a*0.75),s);
                 //setMotors12(compG(s,_m1dir,current),compG(s,_m2dir,current));
                 //_m2dir=1;
             } else {
                 //setMotors(0);
                 return true;
-            }
-            
-            
+            }         
             break;
     }
     return false;

--- a/LOCOMOTION.h	Sun Apr 24 22:37:07 2016 +0000
+++ b/LOCOMOTION.h	Mon Apr 25 17:47:51 2016 +0000
@@ -9,8 +9,8 @@
 #define SPEED_TURN_MAX  0.35//0.45
 #define Y_BIAS_MIN      0.95
 #define Y_BIAS_MAX      1.00
-#define SPEED_X_MIN     0.15//0.65//0.15
-#define SPEED_X_MAX     0.25//0.75//0.25
+#define SPEED_X_MIN     0.70//0.15
+#define SPEED_X_MAX     0.90//0.25
 #define GAIN_GRAVITY    0.5
 #define M_PI            3.1415926535897932384
  
@@ -52,7 +52,7 @@
     DigitalOut _led4;
     void eStop(void);
     void mStop(void);
-    bool setXPos(int target, int current, int error, int angle, int curr_angle);
+    bool setXPos(int target, int current, int error, int angle);
     bool setYBias(int target, int current, int error, int angle);
     bool setAngle(int target, int current, int error, int mode);
     

--- a/main.cpp	Sun Apr 24 22:37:07 2016 +0000
+++ b/main.cpp	Mon Apr 25 17:47:51 2016 +0000
@@ -32,7 +32,7 @@
 void BrownOut();
 
 int xTarget=120;
-int angle_error=0;
+int angle_error=1;
 bool xGood=false;
 bool yGood=false;
 bool angleGood=false;
@@ -95,11 +95,11 @@
     wait(0.5);*/
     while(1) {
         suction.pulsewidth_us(1300);
-         /*pressure=(int)read[0];//(int)pres.getTemperature();
+        /*pressure=(int)read[0];//(int)pres.getTemperature();
         if(pressure==88)
-            led1=1;
+           led1=1;
         else
-            led1=0;*/
+           led1=0;*/
         //uncomment this part if you want the robot to just drive down the window with no separtor
         if (xState==0) {
             motion.mStop();
@@ -107,17 +107,22 @@
             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,&angle_error,yTarget);
+        //motion.check_xya(&xGood,&yGood,&angleGood,xTarget,angleTarget,yTarget,xya,3,3,angle_error);
+
+        //motion.setXstate(&xState,&xTarget,xGood,angleGood,&angleTarget,&angle_error,yTarget);
         //motion.setAngleTol(&angle_error,yGood,xGood);
-       // motion.setYgoal(xGood,angleGood,yGood,&yTarget);
-        motion.setXPos(xTarget,xya.x,5,angleTarget,xya.a);
-        motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_BIAS);
+        // motion.setYgoal(xGood,angleGood,yGood,&yTarget);
+        //motion.setXPos(xTarget,xya.x,5,angleTarget,xya.a);
+        xGood=motion.setXPos(xTarget,xya.x,2,0);
+        if(!xGood)
+            motion.setAngle(0,xya.a,angle_error,ANGLE_BIAS);
+        else {
+            xTarget=(xTarget==120)?30:120;
+        }
             //motion.setYBias(0,xya.y,2,angleTarget);
-        //loc.get_xy(&xya);
+            //loc.get_xy(&xya);
 #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);
+            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();
@@ -127,7 +132,7 @@
 void send()
 {
     //Print over serial port to WiFi MCU
-    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xGood,xya.a/10,xya.a%10);
+    pc.printf("%c%c%c%c\n",(char)xya.x,(char)xTarget,xya.a/10,xya.a%10);
 }
 
 void BrownOut()