Diff: main.cpp

Revision:

9:65e998f810d1

Parent:

8:eaa3e91dc534

Child:

10:57f60ac40cf6

--- a/main.cpp	Wed Oct 29 10:42:42 2014 +0000
+++ b/main.cpp	Wed Oct 29 10:46:10 2014 +0000
@@ -27,6 +27,9 @@
 
 //define global variables
 int y1;
+float new_speed;
+float max_speed;
+float enca;
 
 int main()
 {
@@ -90,9 +93,6 @@
 
 void slam_II()
 {
-    float new_speed;
-    float max_speed;
-    float enca;
     switch (y1)
     {
         case 1:
@@ -116,7 +116,7 @@
             break;
     }
     enca=encoderA.getPosition();
-    while(fabs(enca)-fabs(MAXENCO)>0.5)
+    while(fabs(enca-MAXENCO)>0.5)
     {
         new_speed=pid(MAXENCO,encoderA.getPosition());
         clamp(&new_speed,-max_speed,max_speed);
@@ -131,7 +131,7 @@
         m1_speed.write(fabs(new_speed));
         enca=encoderA.getPosition();
     }
-    while(fabs(enca)-fabs(MINENCO)>0.5)
+    while(fabs(enca-MINENCO)>0.5)
     {
         new_speed=pid(MINENCO,encoderA.getPosition());
         clamp(&new_speed,-0.5,0.5);