Revision 34:083073e54dbd, committed 2016-04-20

Comitter:

alaurens

Date:

Wed Apr 20 18:25:43 2016 +0000

Parent:

33:baf24c59affc

Commit message:

df

Changed in this revision

--- a/LOCOMOTION.cpp	Wed Apr 20 16:11:08 2016 +0000
+++ b/LOCOMOTION.cpp	Wed Apr 20 18:25:43 2016 +0000
@@ -57,16 +57,16 @@
     else
         s=((SPEED_X_MAX-SPEED_X_MIN)*abs(current-target)/FRAME_W)+SPEED_X_MIN;
     if(current>target+error) {
-        if(angle==0) {
+        if(angle==0+TILTZZ) {
+            _m1dir=0;
+            _m2dir=0;
+        } else {
             _m1dir=1;
             _m2dir=1;
-        } else {
-            _m1dir=0;
-            _m2dir=0;
         }
         setMotors(s);
     } else if(current<target-error) {
-        if(angle==0) {
+        if(angle==0+TILTZZ) {
             _m1dir=0;
             _m2dir=0;
         } else {
@@ -124,7 +124,6 @@
     else
         s=((SPEED_TURN_MAX-SPEED_TURN_MIN)*abs(wrap(current+diff)-180)/180)+SPEED_TURN_MIN;
     switch(mode) {
-        default:
         case ANGLE_TURN:
             if(wrap(current+diff)>180+error) {
                 _m1dir=0;
@@ -167,14 +166,14 @@
     switch(*xCurrState) {
 
         case X_INCREASE:
-            *angleGoal=0;
+            *angleGoal=0+TILTZZ;
             _m1dir=0;
             _m2dir=0;
             *xTarget=X_FAR_GOAL;
             break;
 
         case X_DECREASE:
-            *angleGoal=0;
+            *angleGoal=0-TILTZZ;
             _m1dir=1;
             _m2dir=1;
             *xTarget=X_NEAR_GOAL;

--- a/LOCOMOTION.h	Wed Apr 20 16:11:08 2016 +0000
+++ b/LOCOMOTION.h	Wed Apr 20 18:25:43 2016 +0000
@@ -9,9 +9,9 @@
 #define SPEED_TURN_MAX  0.60//0.45
 #define Y_BIAS_MIN      0.75
 #define Y_BIAS_MAX      1.00
-#define SPEED_X_MIN     0.10
-#define SPEED_X_MAX     0.35
-#define GAIN_GRAVITY    0.75
+#define SPEED_X_MIN     0.15
+#define SPEED_X_MAX     0.25
+#define GAIN_GRAVITY    0.5
 #define M_PI            3.1415926535897932384
  
 #define X_STOP 0
@@ -28,7 +28,7 @@
 #define Y_INCREMENT 7
 #define FRAME_HE 180
 #define ROB_SIZE 34
-#define TILTZZ 5   
+#define TILTZZ -5
  
 enum {
     ANGLE_TURN  = 0,

--- a/main.cpp	Wed Apr 20 16:11:08 2016 +0000
+++ b/main.cpp	Wed Apr 20 18:25:43 2016 +0000
@@ -86,12 +86,12 @@
     wait(0.5);
     suction.pulsewidth_us(1150);
     wait(0.5);
-    suction.pulsewidth_us(1200);
-    wait(0.5);
     suction.pulsewidth_us(1250);
     wait(0.5);
+    suction.pulsewidth_us(1300);
+    wait(0.5);
     while(1) {
-        suction.pulsewidth_us(1300);
+        suction.pulsewidth_us(1350);
         /*pressure=(int)read[0];//(int)pres.getTemperature();
         if(pressure==88)
             led1=1;
@@ -107,11 +107,11 @@
         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.setAngleTol(&angle_error,yGood,xGood);
+       // motion.setYgoal(xGood,angleGood,yGood,&yTarget);
         if(motion.setAngle(angleTarget,xya.a,angle_error,ANGLE_TURN)) {
             motion.setXPos(xTarget,xya.x,2,angleTarget);
-            motion.setYBias(yTarget,xya.y,2,angleTarget);
+         //   motion.setYBias(yTarget,xya.y,2,angleTarget);
 
         }
         //loc.get_xy(&xya);