drive down

Dependencies:   BMP280 BNO055_fusion PowerControl mbed

Fork of TEAM_G_FLOW_RIDA by Edwin Cho

Files at this revision

API Documentation at this revision

Comitter:
alaurens
Date:
Wed Apr 20 18:25:43 2016 +0000
Parent:
33:baf24c59affc
Commit message:
df

Changed in this revision

LOCOMOTION.cpp Show annotated file Show diff for this revision Revisions of this file
LOCOMOTION.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r baf24c59affc -r 083073e54dbd LOCOMOTION.cpp
--- 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;
diff -r baf24c59affc -r 083073e54dbd LOCOMOTION.h
--- 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,
diff -r baf24c59affc -r 083073e54dbd main.cpp
--- 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);