Changeset 716

Timestamp:

07/20/09 05:01:43 (4 years ago)

Author:

atorf

Message:

MotorControl20:

• Improved detection of external STOP direct commands, cleaner control task exit
• Blocking functions return true/false now to indicate whether they have been stopped externally
• Orange emergency stop button now holds on and exits the program completely (restart / new matlab handle is required)

Location:

branches/atorf/NXC/MotorControl2

Files:

• ControllerCore.nxc (modified) (10 diffs)
• MotorControl20.nxc (modified) (9 diffs)

branches/atorf/NXC/MotorControl2/ControllerCore.nxc

@@ -56 +56 @@
 
 
-safecall void _ADD_MOTORNAME(WaitUntilMotorStopped)(const byte &port) {
+safecall bool _ADD_MOTORNAME(WaitUntilMotorStopped)(const byte &port) {
 /*
+RETURNS true if stopped by direct command
+
 This function waits until a braked motor has come to a full stop. The "trick" is
 that it's not only beeing waited until the motor has stopped for a very short
@@ -90 +92 @@
         }//end if
         lastPos = curPos;
+        
+        // check if direct command stopped us (indicated by TachoLimit...)
+        if ((MotorTachoLimit(port) == 0) && (MotorPower(port) == 0)) {
+            //direct command must have stopped us!
+            #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
+                TextOut(0, DebugLinePos[port], "ABORTED(wait4Stop)        ");
+                Wait(300);
+            #endif
+            return true;
+        }//end if
+
+        
 
         if (CurrentTick() >= timeoutTick) {
@@ -96 +110 @@
     }//end while
 
+    return false;
 
 }//end void
@@ -111 +126 @@
 
 // note how RunMotor2 doesn't need to be inline and takes 1 more argument...
+// RETURN is wether a direct command stopped us!!!!
 #ifdef RUNMOTOR2_SYNCMODE
-void        RunMotor2(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart, const byte &port2) {
+bool        RunMotor2(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart, const byte &port2) {
 #else
-inline void _ADD_MOTORNAME(RunMotor) (const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart) {
+inline bool _ADD_MOTORNAME(RunMotor) (const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart) {
 #endif
 
@@ -371 +387 @@
                 Wait(300);
             #endif
-            return;
+            return true;
         }//end if
         
@@ -702 +718 @@
                     Wait(300);
                 #endif
-                return;
+                return true;
             }//end if
             
@@ -753 +769 @@
                         break;
                     }//end if
+                    // even for syncmode, only check first motor...
+                    if (MotorTachoLimit(port) == 0) {
+                        //direct command must have stopped us!
+                        #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
+                            TextOut(0, DebugLinePos[port], "ABORTED(endgame)        ");
+                            Wait(300);
+                        #endif
+                        return true;
+                    }//end if
                     #ifdef ENABLEDEBUGGING_LCD
                         endgameLoopCount++;
@@ -768 +793 @@
                         break;
                     }//end if
+                    // even for syncmode, only check first motor...
+                    if (MotorTachoLimit(port) == 0) {
+                        //direct command must have stopped us!
+                        #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
+                            TextOut(0, DebugLinePos[port], "ABORTED(endgame)        ");
+                            Wait(300);
+                        #endif
+                        return true;
+                    }//end if
+
                     #ifdef ENABLEDEBUGGING_LCD
                         endgameLoopCount++;
@@ -833 +868 @@
     #endif
 
-    _ADD_MOTORNAME(WaitUntilMotorStopped)(port);
+    bool stoppedByDirectCmd = false;
+    stoppedByDirectCmd = _ADD_MOTORNAME(WaitUntilMotorStopped)(port);
 
 
@@ -900 +936 @@
         TextOut(0, DebugLinePos[port], "FINISHED        ");
     #endif
+    
+     // return wether a direct command stopped us...
+    return stoppedByDirectCmd;
+    
 
 }//end void

branches/atorf/NXC/MotorControl2/MotorControl20.nxc

@@ -148 +148 @@
     taskArunning = true;
     
-    RunMotorA(OUT_A, motorParamsA.power, motorParamsA.tacholimit, motorParamsA.speedreg, motorParamsA.holdbrake, motorParamsA.smoothstart);
+    bool stoppedByDirectCmd;
+    stoppedByDirectCmd = RunMotorA(OUT_A, motorParamsA.power, motorParamsA.tacholimit, motorParamsA.speedreg, motorParamsA.holdbrake, motorParamsA.smoothstart);
+    
+    // if we exited from external NXTMotor.Stop command, we might've overwritten
+    // the power value before exiting the main controller loop, so restore defined
+    // end state here again:
+    if (stoppedByDirectCmd) {
+        if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) {
+            MotorBrake(OUT_A);
+        } else {
+            MotorOff(OUT_A);
+        }//end if
+    }//end if
     
     taskArunning = false;
@@ -167 +179 @@
     #endif
 
-    RunMotorB(OUT_B, motorParamsB.power, motorParamsB.tacholimit, motorParamsB.speedreg, motorParamsB.holdbrake, motorParamsB.smoothstart);
-
+    bool stoppedByDirectCmd;
+    stoppedByDirectCmd = RunMotorB(OUT_B, motorParamsB.power, motorParamsB.tacholimit, motorParamsB.speedreg, motorParamsB.holdbrake, motorParamsB.smoothstart);
+
+    // if we exited from external NXTMotor.Stop command, we might've overwritten
+    // the power value before exiting the main controller loop, so restore defined
+    // end state here again:
+    if (stoppedByDirectCmd) {
+        if (MotorRegulation(OUT_B) == OUT_REGMODE_SPEED) {
+            MotorBrake(OUT_B);
+        } else {
+            MotorOff(OUT_B);
+        }//end if
+    }//end if
 
     #ifdef ENABLEDEBUGGING_OLDLCDTIMING
@@ -199 +222 @@
     taskCrunning = true;
 
-    RunMotorC(OUT_C, motorParamsC.power, motorParamsC.tacholimit, motorParamsC.speedreg, motorParamsC.holdbrake, motorParamsC.smoothstart);
-
+    bool stoppedByDirectCmd;
+    stoppedByDirectCmd = RunMotorC(OUT_C, motorParamsC.power, motorParamsC.tacholimit, motorParamsC.speedreg, motorParamsC.holdbrake, motorParamsC.smoothstart);
+    
+    // if we exited from external NXTMotor.Stop command, we might've overwritten
+    // the power value before exiting the main controller loop, so restore defined
+    // end state here again:
+    if (stoppedByDirectCmd) {
+        if (MotorRegulation(OUT_C) == OUT_REGMODE_SPEED) {
+            MotorBrake(OUT_C);
+        } else {
+            MotorOff(OUT_C);
+        }//end if
+    }//end if
+    
     taskCrunning = false;
 
@@ -213 +248 @@
         taskSyncRunning = true;
     #endif
+    
+    bool stoppedByDirectCmd;
 
     if (SyncPorts == 3) { // OUT_AB
@@ -219 +256 @@
             taskArunning = true;
             taskBrunning = true;
-            RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_B);
+            stoppedByDirectCmd = RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_B);
+            if (stoppedByDirectCmd) {
+                if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) {
+                    MotorBrake2(OUT_A, OUT_B);
+                } else {
+                    MotorOff2(OUT_A, OUT_B);
+                }//end if
+            }//end if
             taskArunning = false;
             taskBrunning = false;
@@ -229 +273 @@
             taskArunning = true;
             taskCrunning = true;
-            RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C);
+            stoppedByDirectCmd = RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C);
+            if (stoppedByDirectCmd) {
+                if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) {
+                    MotorBrake2(OUT_A, OUT_C);
+                } else {
+                    MotorOff2(OUT_A, OUT_C);
+                }//end if
+            }//end if
             taskArunning = false;
             taskCrunning = false;
@@ -239 +290 @@
             taskBrunning = true;
             taskCrunning = true;
-            RunMotor2(OUT_B, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C);
+            stoppedByDirectCmd = RunMotor2(OUT_B, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C);
+            if (stoppedByDirectCmd) {
+                if (MotorRegulation(OUT_B) == OUT_REGMODE_SPEED) {
+                    MotorBrake2(OUT_B, OUT_C);
+                } else {
+                    MotorOff2(OUT_B, OUT_C);
+                }//end if
+            }//end if
             taskBrunning = false;
             taskCrunning = false;
@@ -767 +825 @@
             MotorOff(OUT_B);
             MotorOff(OUT_C);
-            PlayTone(440, 100);
+            PlayTone(440, 500);
             Wait(500);
+            
+            
+            TextOut(5,LCD_LINE7, "EMERGENCY STOP    ", false);
+            TextOut(2,LCD_LINE8, "Restart program!  ", false);
+            Wait(2000);
+            StopAllTasks();
+            
+            /*
             // keep
             while(ButtonPressed(BTNCENTER, false)) {
@@ -777 +843 @@
                 Wait(500);
             }//end while
+            
+            // purge incoming queue...
+            in = "...";
+            while(StrLen(in) > 0) {
+                ReceiveRemoteString(INBOX, true, in);
+            }//end while
+            in = "";
+            
+            // manually reset task-semaphores
+            // not very clean, but this button is for emergencies anyway...
+            taskArunning = false;
+            taskBrunning = false;
+            taskCrunning = false;
+            
+            // reset counters for a nice clean start
+            ResetErrorCorrectionAndBlockCount(OUT_A);
+            ResetErrorCorrectionAndBlockCount(OUT_B);
+            ResetErrorCorrectionAndBlockCount(OUT_C);
+            */
         }//end if