Diff: Motion.cpp

Revision:

23:accd07ca2da7

Parent:

22:91526c8d15ba

Child:

24:11c5fb5280eb

--- a/Motion.cpp	Tue May 15 11:19:15 2018 +0000
+++ b/Motion.cpp	Tue May 15 13:35:50 2018 +0000
@@ -75,7 +75,7 @@
                 countsLOld += 500;
                 countsROld += 500; 
                   
-            }else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) {
+            }/*else if ( lastMove == true && ( distanceL < 80.0f || distanceR < 80.0f ) && distanceC > 120.0f ) {
                 countsLOld = counterLeft.read();
                 countsROld = counterRight.read();
                 
@@ -87,6 +87,50 @@
                 }
                 stop();
                 break;
+            }*/
+            
+        }    
+        
+        t.stop();
+        t.reset();
+        lastMove = false;
+}
+
+/**
+ * Eine Feldstrecke druchführen
+ */
+void Motion::moveHalf() {
+        
+        countsLOld = counterLeft.read();
+        countsROld = counterRight.read();
+        countsL = counterLeft.read();
+        countsR = counterRight.read();
+        
+        avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
+            
+        t.start();
+        
+        while ((countsL - countsLOld)  < 824 || (countsR - countsROld) > -824) {
+            
+            countsL = counterLeft.read();
+            countsR = counterRight.read();
+            distanceC = irSensorC.readC();
+            distanceL = irSensorL.readL();
+            distanceR = irSensorR.readR();
+            
+            if (enableMotorDriver == 0) {enableMotorDriver = 1;}
+                
+            avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
+        
+            accel(MOVE_SPEED);
+            control();
+            
+            if (distanceC < 101.0f && lastMove == false) {
+                stop();
+                break;    
+            }else if (distanceC < 40.0 && lastMove == true) {
+                stop();
+                break;
             }
             
         }    
@@ -132,9 +176,6 @@
             accel(SCAN_SPEED);
             control();
             
-            //controller.setDesiredSpeedLeft(actSpeed);
-            //controller.setDesiredSpeedRight(-actSpeed);
-            
             if ((distanceC) < 40.0f) {
                 countsLOld = countsL;
                 countsROld = countsR;
@@ -146,24 +187,16 @@
                         break;
                     }
                 }
+                
                 stop();
-                break;    
+                break;   
+                 
             }else if ( ((countsL - countsLOld)  >= MOVE_DIST || (countsR - countsROld) <= -MOVE_DIST) && (distanceC < 100.0f) && (distanceC > 40.0f)) {
                 countsLOld += 500;
                 countsROld += 500;   
-            }/*else if ( ( distanceL > 80.0f || distanceR > 80.0f ) ) {
-                countsLOld = counterRight.read();
-                countsROld = counterRight.read();
-                while ((countsL - countsLOld)  <  MOVE_DIST*0.5f + 404.0f || (countsR - countsROld) > -0.5f*MOVE_DIST - 404.0f) {
-                    countsL = counterLeft.read();
-                    countsR = counterRight.read();
-                    accel(SCAN_SPEED);
-                    control();
-                }
-                stop();
-                break;
-            }*/
-        }  
+            }
+        } 
+         
         t.stop();
         t.reset();
         lastMove = false;
@@ -386,6 +419,8 @@
         switch(path[task]) {
             
             case 1:
+            
+                //Acceleration
                 if ( reverse == true && path[task-1] == path[task] && path[task+1] != path[task] && task != junction) {
                     
                     acceleration = true;
@@ -403,7 +438,8 @@
                     deceleration = false;
                     avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
                 }
-            
+                
+                //Deceleration
                 if (reverse == true && ( path[task-1] != path[task] || task == junction ) && avgSpeed > 2.4f*MOVE_SPEED) {
                     
                     deceleration = true;
@@ -434,8 +470,47 @@
                 rotateR();
                 break;
             case 4:
-                stop();
-                break;    
+            
+                if (reverse == false && path[task] == 4 && path[task+1] == 1) {
+                    
+                    acceleration = true;
+                    longMove = true;
+                    deceleration = false;
+                    
+                }else{
+                    acceleration = false; 
+                    deceleration = false;
+                    avgSpeed = ( abs(controller.getSpeedLeft()) + abs(controller.getSpeedRight()) ) * 0.5f;
+                }
+                
+                if (reverse == false && path[task-1] == 1 && path[task] == 4) {
+                    
+                    deceleration = true;
+                    acceleration = false;
+                    lastMove = true;
+                    longMove = false;
+                    
+                }else{
+                    deceleration = false;
+                }
+                
+                if (reverse == false && path[task+1] == 0) {
+                    
+                    lastMove = true;
+                       
+                }
+                
+            
+                moveHalf();
+                break; 
+            case 5:
+                turnL();
+                break;
+            case 6:
+                turnR();
+                break;
+            case 7:
+                break;   
         }
 }