PageRenderTime 116ms CodeModel.GetById 41ms app.highlight 7ms RepoModel.GetById 63ms app.codeStats 0ms

/motcont/MotorControl22.nxc

http://nxt-python.googlecode.com/
Unknown | 918 lines | 701 code | 217 blank | 0 comment | 0 complexity | e51729f0f261d95c27d434263c38cd9d MD5 | raw file
  1//#!C
  2/*
  3% The NXT Program MotorControl enables precise motor movement via direct
  4% commands. It listens to "NXT BT messages", interprets their content (own user-
  5% defined "protocol", if you will), and carries out highly precise motor actions
  6% in a different thread for each motor, allowing the upper level program (in
  7% this case, MATLAB) to carry on with execution...
  8%
  9% See also: http://www.mindstorms.rwth-aachen.de/trac/wiki/MotorControl
 10%
 11% CHANGELOG:
 12%
 13% * Version 2.2, 2010/09/14
 14%   First released with toolbox version 4.04
 15%   - Commented dead code in case compiler optimization doesn't find it
 16%   - No real code changes
 17%   - Version to be recompiled with newer NBC/NXC compiler versions,
 18%     as some multi-threading / optimization bugs were fixed.
 19%   - Not recommending FW version 1.26 anymore
 20%   - Updated version numbers
 21%
 22%  * Version 2.1, 2009/08/31
 23%    First released with toolbox version 4.01
 24%    - Considered to be stable and working
 25%    - Very occasional freezes with FW 1.26 during
 26%      massive production use, see also:
 27%      http://www.mindstorms.rwth-aachen.de/trac/ticket/51
 28%
 29%
 30% Signature
 31%   Author: Linus Atorf (see AUTHORS)
 32%   Date: 2010/09/14
 33%   Copyright: 2007-2010, RWTH Aachen University
 34%
 35%
 36% ***********************************************************************************************
 37% *  This file is part of the RWTH - Mindstorms NXT Toolbox.                                    *
 38% *                                                                                             *
 39% *  The RWTH - Mindstorms NXT Toolbox is free software: you can redistribute it and/or modify  *
 40% *  it under the terms of the GNU General Public License as published by the Free Software     *
 41% *  Foundation, either version 3 of the License, or (at your option) any later version.        *
 42% *                                                                                             *
 43% *  The RWTH - Mindstorms NXT Toolbox is distributed in the hope that it will be useful,       *
 44% *  but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS  *
 45% *  FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.             *
 46% *                                                                                             *
 47% *  You should have received a copy of the GNU General Public License along with the           *
 48% *  RWTH - Mindstorms NXT Toolbox. If not, see <http://www.gnu.org/licenses/>.                 *
 49% ***********************************************************************************************
 50*/
 51
 52
 53// *************** GLOBAL DEFINES
 54// important to be before includes,
 55// since they might be used inside the includes!
 56#define PROGRAM_VERSION "2.2"
 57
 58#define INBOX           1
 59#define OUTBOX          0
 60#define DEBUG_OUTBOX    2
 61
 62// Protocol constants
 63#define PROTO_CONTROLLED_MOTORCMD       1
 64#define PROTO_RESET_ERROR_CORRECTION    2
 65#define PROTO_ISMOTORREADY              3
 66#define PROTO_CLASSIC_MOTORCMD          4
 67#define PROTO_JUMBOPACKET               5
 68
 69
 70
 71
 72
 73// *************** INCLUDES
 74#include "Controller.nxc"
 75// Current file structure:
 76// + MotorControl.nxc (this file)
 77// |-+ Controller.nxc
 78//   |-- MotorFunctions.nxc
 79//   |-- SpeedFromPosLookup.nxc
 80//   |-- ControllerCore.nxc (for Motor A)
 81//   |-- ControllerCore.nxc (for Motor B)
 82//   |-- ControllerCore.nxc (for Motor C)
 83//   |-- ControllerCore.nxc (for sync mode)
 84
 85
 86
 87// *************** PROGRAM STARTS
 88
 89
 90
 91struct typRunMotorParams {
 92    int  power;
 93    long tacholimit;
 94    bool speedreg;
 95    bool holdbrake;
 96    bool smoothstart;
 97    int turnratio;
 98};
 99
100
101byte SyncPorts;
102
103
104typRunMotorParams motorParamsA;
105typRunMotorParams motorParamsB;
106typRunMotorParams motorParamsC;
107typRunMotorParams motorParamsSync;
108
109
110//TODO are the mutexes really still needed?
111mutex movingA;
112mutex movingB;
113mutex movingC;
114
115// these semaphores are actually interpreted as "motorArunning", as they are
116// also set and interpreted for synced movements... a bit like the mutexes above
117bool taskArunning;
118bool taskBrunning;
119bool taskCrunning;
120#ifdef ENABLEDEBUGGING_SHOWTASKS
121    bool taskSyncRunning;
122#endif
123
124
125
126
127#ifdef ENABLEDEBUGGING_OLDLCDTIMING
128	long parsedTime;
129	long taskStartedTime;
130#endif
131
132
133safecall void DisplayMainScreen() {
134     string tmp;
135    // nice message to screen:
136
137    tmp =         StrCat("MotorControl ", PROGRAM_VERSION);
138    TextOut(0,LCD_LINE1, tmp, true);
139    TextOut(0,LCD_LINE2, "for FW 1.28-1.29", false);
140
141    TextOut(3,LCD_LINE4, "(C) RWTH Aachen", false);
142    TextOut(3,LCD_LINE5, "University, LfB", false);
143
144    TextOut(4,LCD_LINE7, "Press ORANGE to", false);
145    TextOut(4,LCD_LINE8, "stop all motors", false);
146
147
148    #ifdef ENABLEDEBUGGING_LCD_SLOW_ANTIBUG
149        TextOut(0, LCD_LINE8, "SLOW ANTIBUG ON  ", true);
150    #endif// - - - - - - -
151
152}//end void
153
154
155
156safecall void TaskBusySignal() {
157    PlayTone(2000, 50);
158    Wait(50);
159    PlayTone(1500, 50);
160}//end TaskBusySignal
161
162
163// tasks
164task MoveA() {
165    //TextOut(0,LCD_LINE6, "Task A started");
166    //Acquire(movingA);
167
168    taskArunning = true;
169
170    bool stoppedByDirectCmd;
171    stoppedByDirectCmd = RunMotorA(OUT_A, motorParamsA.power, motorParamsA.tacholimit, motorParamsA.speedreg, motorParamsA.holdbrake, motorParamsA.smoothstart);
172
173    // if we exited from external NXTMotor.Stop command, we might've overwritten
174    // the power value before exiting the main controller loop, so restore defined
175    // end state here again:
176    if (stoppedByDirectCmd) {
177        if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) {
178            MotorBrake(OUT_A);
179        } else {
180            MotorOff(OUT_A);
181        }//end if
182    }//end if
183
184    taskArunning = false;
185
186    //Release(movingA);
187    //TextOut(0,LCD_LINE6, "Task A stopped");
188}//MoveA
189
190
191task MoveB() {
192    //TextOut(0,LCD_LINE7, "Task B started");
193    //Acquire(movingB);
194
195    taskBrunning = true;
196
197    #ifdef ENABLEDEBUGGING_OLDLCDTIMING
198		taskStartedTime = CurrentTick() - receivedMsgTime;    // NEW DEBUG
199    #endif
200
201    bool stoppedByDirectCmd;
202    stoppedByDirectCmd = RunMotorB(OUT_B, motorParamsB.power, motorParamsB.tacholimit, motorParamsB.speedreg, motorParamsB.holdbrake, motorParamsB.smoothstart);
203
204    // if we exited from external NXTMotor.Stop command, we might've overwritten
205    // the power value before exiting the main controller loop, so restore defined
206    // end state here again:
207    if (stoppedByDirectCmd) {
208        if (MotorRegulation(OUT_B) == OUT_REGMODE_SPEED) {
209            MotorBrake(OUT_B);
210        } else {
211            MotorOff(OUT_B);
212        }//end if
213    }//end if
214
215    #ifdef ENABLEDEBUGGING_OLDLCDTIMING
216		string tmp = "";   // NEW DEBUG
217
218		tmp = NumToStr(parsedTime);
219		tmp = StrCat("Parsed ", tmp, "ms");
220		TextOut(0,LCD_LINE3, tmp, true);
221
222		tmp = NumToStr(taskStartedTime);
223		tmp = StrCat("Task ", tmp, "ms");
224		TextOut(0,LCD_LINE5, tmp);
225
226		tmp = NumToStr(motorStartedTime);
227		tmp = StrCat("Motor ", tmp, "ms");
228		TextOut(0,LCD_LINE7, tmp);
229	#endif
230
231
232    taskBrunning = false;
233    //Release(movingB);
234
235}//MoveB
236
237
238task MoveC() {
239    //TextOut(0,LCD_LINE8, "Task C started");
240    //Acquire(movingC);
241
242    taskCrunning = true;
243
244    bool stoppedByDirectCmd;
245    stoppedByDirectCmd = RunMotorC(OUT_C, motorParamsC.power, motorParamsC.tacholimit, motorParamsC.speedreg, motorParamsC.holdbrake, motorParamsC.smoothstart);
246
247    // if we exited from external NXTMotor.Stop command, we might've overwritten
248    // the power value before exiting the main controller loop, so restore defined
249    // end state here again:
250    if (stoppedByDirectCmd) {
251        if (MotorRegulation(OUT_C) == OUT_REGMODE_SPEED) {
252            MotorBrake(OUT_C);
253        } else {
254            MotorOff(OUT_C);
255        }//end if
256    }//end if
257
258    taskCrunning = false;
259
260    //Release(movingC);
261    //TextOut(0,LCD_LINE8, "Task C stopped");
262}//MoveC
263
264
265task MoveSync() {
266
267    #ifdef ENABLEDEBUGGING_SHOWTASKS
268        taskSyncRunning = true;
269    #endif
270
271    bool stoppedByDirectCmd;
272
273    if (SyncPorts == 3) { // OUT_AB
274        //Acquire(movingA);
275        //Acquire(movingB);
276            taskArunning = true;
277            taskBrunning = true;
278            stoppedByDirectCmd = RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_B);
279            if (stoppedByDirectCmd) {
280                if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) {
281                    MotorBrake2(OUT_A, OUT_B);
282                } else {
283                    MotorOff2(OUT_A, OUT_B);
284                }//end if
285            }//end if
286            taskArunning = false;
287            taskBrunning = false;
288        //Release(movingA);
289        //Release(movingB);
290    } else if (SyncPorts == 4) { // OUT_AC
291        //Acquire(movingA);
292        //Acquire(movingC);
293            taskArunning = true;
294            taskCrunning = true;
295            stoppedByDirectCmd = RunMotor2(OUT_A, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C);
296            if (stoppedByDirectCmd) {
297                if (MotorRegulation(OUT_A) == OUT_REGMODE_SPEED) {
298                    MotorBrake2(OUT_A, OUT_C);
299                } else {
300                    MotorOff2(OUT_A, OUT_C);
301                }//end if
302            }//end if
303            taskArunning = false;
304            taskCrunning = false;
305        //Release(movingA);
306        //Release(movingC);
307    } else if (SyncPorts == 5) { // OUT_BC
308        //Acquire(movingB);
309        //Acquire(movingC);
310            taskBrunning = true;
311            taskCrunning = true;
312            stoppedByDirectCmd = RunMotor2(OUT_B, motorParamsSync.power, motorParamsSync.tacholimit, false, motorParamsSync.holdbrake, motorParamsSync.smoothstart, OUT_C);
313            if (stoppedByDirectCmd) {
314                if (MotorRegulation(OUT_B) == OUT_REGMODE_SPEED) {
315                    MotorBrake2(OUT_B, OUT_C);
316                } else {
317                    MotorOff2(OUT_B, OUT_C);
318                }//end if
319            }//end if
320            taskBrunning = false;
321            taskCrunning = false;
322        //Release(movingB);
323        //Release(movingC);
324    }//end if
325
326    #ifdef ENABLEDEBUGGING_SHOWTASKS
327        taskSyncRunning = false;
328    #endif
329}//MoveSync
330
331
332
333
334inline bool IsMotorReady(const byte &port) {
335
336    // check output state info, this is simple
337    if (MotorIsRunning(port)) {
338        return false;
339    } else { // maybe braking or coasting or whatever:
340        switch(port) {
341        case OUT_A:
342            return !(taskArunning);
343            break;
344        case OUT_B:
345            return !(taskBrunning);
346            break;
347        case OUT_C:
348            return !(taskCrunning);
349            break;
350        }//end switch
351
352   }//end if
353
354}//end function
355
356
357
358/*
359task dummyA() {
360    int i = 0;
361    int j = 0;
362    while(true){
363        i++;
364        i = j;
365        j = MotorTachoCount(OUT_A);
366    }//end while
367}//end task
368
369task dummyB() {
370    int i = 0;
371    int j = 0;
372    while(true){
373        i++;
374        i = j;
375        j = MotorTachoCount(OUT_B);
376    }//end while
377}//end task
378
379task dummyC() {
380    int i = 0;
381    int j = 0;
382    while(true){
383        i++;
384        i = j;
385        j = MotorTachoCount(OUT_C);
386    }//end while
387}//end task
388
389task dummyD() {
390    int i = 0;
391    int j = 0;
392    while(true){
393        i++;
394        i = j;
395        j = MotorTachoCount(OUT_A);
396    }//end while
397}//end task
398
399task dummyE() {
400    int i = 0;
401    int j = 0;
402    while(true){
403        i++;
404        i = j;
405        j = MotorTachoCount(OUT_B);
406    }//end while
407}//end task
408*/
409
410
411
412
413
414
415// -------------------------------------------------
416// main task
417
418/*
419task TESTmain() {
420
421     byte port = OUT_B;
422
423
424// **** Init
425
426    DisplayMainScreen();
427    //Wait(9000);
428
429    // don't forget this
430    InitSpeedFromPosLUT();
431
432    // launch 3 dummy tasks
433    start dummyA;
434    start dummyB;
435    //start dummyC;
436    //start dummyD;
437    //start dummyE;
438    Wait(20);
439
440
441
442
443// **** Init / Reset motor
444    MotorOff(OUT_A);
445    MotorOff(OUT_B);
446    MotorOff(OUT_C);
447    ResetErrorCorrectionAndBlockCount(OUT_A);
448    ResetErrorCorrectionAndBlockCount(OUT_B);
449    ResetErrorCorrectionAndBlockCount(OUT_C);
450
451
452    Wait(1100);
453
454// **** Try out new function
455    //RunMotor(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart)
456    //RunMotor(port, 100, 1000, true, false, false);
457    //RunMotor2(OUT_B, 100, 1000, false, false, true, OUT_C);
458
459
460    motorParamsA.power = 100;
461    motorParamsA.tacholimit = 1000;
462    motorParamsA.speedreg = false;
463    motorParamsA.holdbrake = false;
464    motorParamsA.smoothstart = true;
465
466
467    motorParamsB.power = 100;
468    motorParamsB.tacholimit = 1000;
469    motorParamsB.speedreg = false;
470    motorParamsB.holdbrake = false;
471    motorParamsB.smoothstart = true;
472
473    motorParamsC.power = 100;
474    motorParamsC.tacholimit = 1000;
475    motorParamsC.speedreg = false;
476    motorParamsC.holdbrake = false;
477    motorParamsC.smoothstart = true;
478
479    start MoveA;
480    start MoveB;
481    start MoveC;
482
483    //Wait(4000);
484
485    //NumOut(0, LCD_LINE1, MotorTachoCount(OUT_B), true);
486    //NumOut(0, LCD_LINE2, MotorTachoCount(OUT_C));
487
488
489
490
491
492}//end task
493*/
494
495
496
497
498task main(){
499
500    // parameter initialization
501    string in = "";
502    int i = 0;
503    byte packetType;
504    byte  port;
505    int  power = 0;
506    long angle = 0;
507    int  turnratio  = 0;
508    byte modebits = 0;
509    bool speedreg = false;
510    bool holdbrake = false;
511    bool smoothstart = false;
512
513    byte port1;
514    byte port2;
515
516    const byte bit1 = 1;
517    const byte bit2 = 2;
518    const byte bit3 = 4;
519
520
521    string tmp = "";
522    //string tmp2 = "";
523
524
525    // init!
526    taskArunning = false;
527    taskBrunning = false;
528    taskCrunning = false;
529
530
531    DisplayMainScreen();
532    //Wait(9000);
533
534    // don't forget this
535    InitSpeedFromPosLUT();
536
537
538    // purge mailboxes to make sure, in case something is left over,
539    // I don't know...
540    in = "...";
541    while(StrLen(in) > 0) {
542        ReceiveRemoteString(INBOX, true, in);
543    }//end while
544    in = "...";
545    while(StrLen(in) > 0) {
546        ReceiveRemoteString(OUTBOX, true, in);
547    }//end while
548    #ifdef ENABLEDEBUGGING_REMOTELOGGING
549        in = "...";
550        while(StrLen(in) > 0) {
551            ReceiveRemoteString(DEBUG_OUTBOX, true, in);
552        }//end while
553    #endif
554
555    in = "";
556
557
558    #ifdef ENABLEDEBUGGING_REMOTELOGGING
559        SendDebugMessage(99, "Remote logging enabled!");
560    #endif
561
562    while(true){
563
564        ReceiveRemoteString(INBOX, true, in);
565
566        if(StrLen(in) > 0) {
567
568            #ifdef ENABLEDEBUGGING_OLDLCDTIMING
569				receivedMsgTime = CurrentTick();  // NEW DEBUG
570            #endif
571
572            #ifdef ENABLEDEBUGGING_ACOUSTICPROTOCOL
573                PlayTone(200, 50);
574            #endif
575
576
577            // take first value, decide what to do...
578            tmp        = SubStr(in,  0, 1);  // pos 0
579            packetType = StrToNum(tmp);
580
581            // main packet handler!
582            switch (packetType) {
583            case PROTO_CONTROLLED_MOTORCMD: // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
584
585                // parse...
586                tmp      = SubStr(in,  1, 1);  // pos 1
587                port     = StrToNum(tmp);
588
589                tmp      = SubStr(in,  2, 3);  // pos 2 3 4
590                power    = StrToNum(tmp);
591
592                tmp      = SubStr(in,  5, 6);  // pos 5 6 7 8 9 10
593                angle    = StrToNum(tmp);
594
595                tmp      = SubStr(in,  11, 1); // pos 11
596                modebits = StrToNum(tmp);
597
598
599                // process...
600                // power
601                if(power > 100) {
602                    power = -(power - 100);
603                }//end if
604
605                // bitfield
606                if (modebits & bit1) {
607                    holdbrake = true;
608                } else {
609                    holdbrake = false;
610                }//end if
611
612                if (modebits & bit2) {
613                    speedreg = true;
614                } else {
615                    speedreg = false;
616                }//end if
617
618                if (modebits & bit3) {
619                    smoothstart = true;
620                } else {
621                    smoothstart = false;
622                }//end if
623
624
625                // finally, the command
626                if (port == 0) { // OUT_A
627
628                    motorParamsA.power = power;
629                    motorParamsA.tacholimit = angle;
630                    motorParamsA.speedreg = speedreg;
631                    motorParamsA.holdbrake = holdbrake;
632                    motorParamsA.smoothstart = smoothstart;
633
634                    if (taskArunning == false) {
635                        start MoveA;
636                    } else {
637                        TaskBusySignal();
638                    } // end if
639
640                } else if (port == 1) { // OUT_B
641
642                    motorParamsB.power = power;
643                    motorParamsB.tacholimit = angle;
644                    motorParamsB.speedreg = speedreg;
645                    motorParamsB.holdbrake = holdbrake;
646                    motorParamsB.smoothstart = smoothstart;
647
648                    if (taskBrunning == false) {
649
650                        #ifdef ENABLEDEBUGGING_OLDLCDTIMING
651                            parsedTime = CurrentTick() - receivedMsgTime; // NEW DEBUG
652                        #endif
653
654                        start MoveB;
655                    } else {
656                        TaskBusySignal();
657                    } // end if
658
659                } else if (port == 2) { // OUT_C
660
661                    motorParamsC.power = power;
662                    motorParamsC.tacholimit = angle;
663                    motorParamsC.speedreg = speedreg;
664                    motorParamsC.holdbrake = holdbrake;
665                    motorParamsC.smoothstart = smoothstart;
666
667                    if (taskCrunning == false) {
668                        start MoveC;
669                    } else {
670                       TaskBusySignal();
671                    }// end if
672
673                } else { // Otherwise (OUT_AB, OUT_AC, OUT_BC, OUT_ABC?)
674
675                    SyncPorts = port;
676
677                    motorParamsSync.power = power;
678                    motorParamsSync.tacholimit = angle;
679                    motorParamsSync.turnratio = 0; //turnratio;
680                    motorParamsSync.speedreg = false; //always for sync!
681                    motorParamsSync.holdbrake = holdbrake;
682                    motorParamsSync.smoothstart = smoothstart;
683
684                    // this doesn't look elegant at all, but it works.
685                    if (  ((port == 3) && (taskArunning == false) && (taskBrunning == false))
686                       || ((port == 4) && (taskArunning == false) && (taskCrunning == false))
687                       || ((port == 5) && (taskBrunning == false) && (taskCrunning == false)) )  {
688                        start MoveSync;
689                    } else {
690                       TaskBusySignal();
691                    }//end if
692
693                }//end if
694
695                //PlayTone(440, 50);
696
697                break;
698            case PROTO_RESET_ERROR_CORRECTION: // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
699
700                tmp      = SubStr(in,  1, 1);  // pos 1
701                port     = StrToNum(tmp);
702
703                if (port <= 2) {
704                    ResetErrorCorrectionAndBlockCount(port);
705                } else if (port == 3) { // OUT_AB
706                    ResetErrorCorrectionAndBlockCount(OUT_A);
707                    ResetErrorCorrectionAndBlockCount(OUT_B);
708                } else if (port == 4) { // OUT_AC
709                    ResetErrorCorrectionAndBlockCount(OUT_A);
710                    ResetErrorCorrectionAndBlockCount(OUT_C);
711                } else if (port == 5) { // OUT_BC
712                    ResetErrorCorrectionAndBlockCount(OUT_B);
713                    ResetErrorCorrectionAndBlockCount(OUT_C);
714                } else if (port == 6) { // OUT_ABC
715                    ResetErrorCorrectionAndBlockCount(OUT_A);
716                    ResetErrorCorrectionAndBlockCount(OUT_B);
717                    ResetErrorCorrectionAndBlockCount(OUT_C);
718                }//end if
719
720                break;
721            case PROTO_ISMOTORREADY:  // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
722
723                tmp      = SubStr(in,  1, 1);  // pos 1
724                port     = StrToNum(tmp);
725
726                // send answer string: portnum, then if ready or not
727                if (IsMotorReady(port)) {
728                    tmp = StrCat(tmp, "1");
729                } else {
730                    tmp = StrCat(tmp, "0");
731                }//end if
732                SendMessage(OUTBOX, tmp);
733
734                break;
735            case PROTO_CLASSIC_MOTORCMD: // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
736
737                // parse...
738                tmp      = SubStr(in,  1, 1);  // pos 1
739                port     = StrToNum(tmp);
740
741                tmp      = SubStr(in,  2, 3);  // pos 2 3 4
742                power    = StrToNum(tmp);
743
744                tmp      = SubStr(in,  5, 6);  // pos 5 6 7 8 9 10
745                angle    = StrToNum(tmp);
746
747                tmp      = SubStr(in,  11, 1); // pos 11
748                speedreg = StrToNum(tmp);
749
750
751                // process...
752                // power
753                if(power > 100) {
754                    power = -(power - 100);
755                }//end if
756
757                //% OUT_A	    0x00
758                //% OUT_B	    0x01
759                //% OUT_C	    0x02
760                //% OUT_AB	0x03
761                //% OUT_AC	0x04
762                //% OUT_BC	0x05
763                //% OUT_ABC	0x06
764
765                if (port <= 2) {
766
767                    // if no current tacholimit and no new one, allow speed change
768                    if ((MotorTachoLimit(port) == 0) && (angle == 0)) {
769                        MotorCmdSingleReset(port, power, angle, speedreg);
770                    } else {
771                        if (IsMotorReady(port)) {
772                            MotorCmdSingleReset(port, power, angle, speedreg);
773                        } else {
774                            TaskBusySignal();
775                        }//end if
776                    }//end if
777
778                } else { // Otherwise (OUT_AB, OUT_AC, OUT_BC, OUT_ABC?)
779
780                    switch(port) {
781                    case 3: // OUT_AB
782                        port1 = 0;
783                        port2 = 1;
784                        break;
785                    case 4: // OUT_AC
786                        port1 = 0;
787                        port2 = 2;
788                        break;
789                    case 5: // OUT_BC
790                        port1 = 1;
791                        port2 = 2;
792                        break;
793                    }//end switch
794
795
796                    // if no current tacholimit and no new one, allow speed change
797                    if ((MotorTachoLimit(port1) == 0) && (MotorTachoLimit(port2) == 0)  && (angle == 0)) {
798                            //~~~~BEGIN COPY PASTE CODE~~~~~~~~~~~~~~~~~~
799                            //avoid already synced motors (that doesn't work as we know...)
800                            until((MotorRegulation(port1) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) {
801                                // repeatedly setting this is not nice, but so
802                                // we don't need a timeout...!
803                                MotorOff2(port1, port2);
804                                // make sure VM applies our settings
805                                Wait(1);
806                            }//end until
807                            MotorCmdDoubleReset(port1, power, angle, port2);
808                            //~~~~END COPY PASTE CODE~~~~~~~~~~~~~~~~~~
809                    } else {
810                        if (IsMotorReady(port1) && IsMotorReady(port2)) {
811                            //~~~~BEGIN COPY PASTE CODE~~~~~~~~~~~~~~~~~~
812                            //avoid already synced motors (that doesn't work as we know...)
813                            until((MotorRegulation(port1) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) {
814                                // repeatedly setting this is not nice, but so
815                                // we don't need a timeout...!
816                                MotorOff2(port1, port2);
817                                // make sure VM applies our settings
818                                Wait(1);
819                            }//end until
820                            MotorCmdDoubleReset(port1, power, angle, port2);
821                            //~~~~END COPY PASTE CODE~~~~~~~~~~~~~~~~~~
822                        } else {
823                           TaskBusySignal();
824                        }//end if
825
826                    }//end if
827
828                }//end if
829
830
831                break;
832            }//end switch - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
833
834
835            // reset message !!!!
836            in = "";
837
838
839        }//end if
840
841        // check cancel button:
842        if (ButtonPressed(BTNCENTER, false)) {
843            // release all motors
844            MotorOff(OUT_A);
845            MotorOff(OUT_B);
846            MotorOff(OUT_C);
847            PlayTone(440, 500);
848            Wait(100);
849
850            // release all motors again
851            MotorOff(OUT_A);
852            MotorOff(OUT_B);
853            MotorOff(OUT_C);
854
855            TextOut(5,LCD_LINE7, "EMERGENCY STOP    ", false);
856            TextOut(2,LCD_LINE8, "Restart program!  ", false);
857            Wait(2500);
858            StopAllTasks();
859
860            /*
861            // keep
862            while(ButtonPressed(BTNCENTER, false)) {
863                MotorOff(OUT_A);
864                MotorOff(OUT_B);
865                MotorOff(OUT_C);
866                PlayTone(440, 100);
867                Wait(500);
868            }//end while
869
870            // purge incoming queue...
871            in = "...";
872            while(StrLen(in) > 0) {
873                ReceiveRemoteString(INBOX, true, in);
874            }//end while
875            in = "";
876
877            // manually reset task-semaphores
878            // not very clean, but this button is for emergencies anyway...
879            taskArunning = false;
880            taskBrunning = false;
881            taskCrunning = false;
882
883            // reset counters for a nice clean start
884            ResetErrorCorrectionAndBlockCount(OUT_A);
885            ResetErrorCorrectionAndBlockCount(OUT_B);
886            ResetErrorCorrectionAndBlockCount(OUT_C);
887            */
888        }//end if
889
890
891        // debugging
892        #ifdef ENABLEDEBUGGING_SHOWTASKS
893            string taskMsg = "";
894            string tmpA = "_ ";
895            string tmpB = "_ ";
896            string tmpC = "_ ";
897            string tmpSync = "____ ";
898
899            if (taskArunning) tmpA = "A ";
900            if (taskBrunning) tmpB = "B ";
901            if (taskCrunning) tmpC = "C ";
902            if (taskSyncRunning) tmpSync = "Sync ";
903
904            taskMsg = StrCat(tmpA, tmpB, tmpC, tmpSync);
905
906            TextOut(0, LCD_LINE7, taskMsg);
907        #endif
908
909    }//end while
910
911
912    TextOut(0,LCD_LINE1, "MainLoop exit", true);
913    PlayTone(500, 1000);
914    Wait(5000);
915
916
917}//end task
918