main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "PinDetect.h"
00004 #include "soundboard.h"
00005 
00006 #include "ultrasonic.h"
00007 
00008 //Needs to be define to be able to read from the Ultrasonic sensor
00009 void dist(int);
00010 ultrasonic mu(PB_0, PA_0, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
00011                                         //have updates every .1 seconds and a timeout after 1
00012                                         //second, and call dist when the distance changes
00013 
00014 soundboard sndBoard(PG_1, PF_9, PF_7, PF_8); // Defines the WTV020M01 Soundboard
00015 Serial pc(USBTX, USBRX); // tx, rx
00016 //MidMagnetSensors
00017 PinDetect pb3(D10);
00018 PinDetect pb4(D11);
00019 
00020 //BottomMagnetSensors
00021 PinDetect pb1(D12);
00022 PinDetect pb2(D13);
00023 
00024 //TopMagnetSensors
00025 PinDetect pb5(D8);
00026 PinDetect pb6(D9);
00027 
00028 
00029 
00030 //Analog Pins
00031 //Bottomengine
00032 DigitalOut IN1(A0);
00033 DigitalOut IN2(A1);
00034 DigitalOut IN3(A2);
00035 DigitalOut IN4(A3);
00036 //SecondEngine
00037 DigitalOut IN5(D2);
00038 DigitalOut IN6(D3);
00039 DigitalOut IN7(A4);
00040 DigitalOut IN8(A5);
00041 //TopEngine
00042 DigitalOut IN9(D4);
00043 DigitalOut IN10(D5);
00044 DigitalOut IN11(D6);
00045 DigitalOut IN12(D7);
00046 
00047 //User button
00048 DigitalIn  SW(USER_BUTTON);
00049 
00050 
00051 static int speed = 1000;
00052 
00053 static char m_cmd = 'x';
00054 static char cmd;
00055 static bool e1 = true;
00056 static bool e2 = true;
00057 static bool e3 = true;
00058 static int i1 = 0;
00059 static int i2 = 0;
00060 static int i3 = 0;
00061 
00062 static int steps1 = 0;
00063 static int steps2 = 0;
00064 static int steps3 = 0;
00065 
00066 
00067 //Distance stored from sensor
00068 static int sDistance = 0;
00069 //Delayed distance to compare with first meassurement
00070 static int dDistance = 0;
00071 static bool isPlaying = false; 
00072 
00073  
00074 
00075 
00076 void step2Left3Right()
00077 {
00078         //engine 2
00079     IN5=1;
00080     IN6=0;
00081     IN7=0;
00082     IN8=1;
00083             //engine 3
00084     IN9=0;
00085     IN10=1;
00086     IN11=0;
00087     IN12=1;
00088 
00089     wait_us(speed);
00090     IN5=1;
00091     IN6=0;
00092     IN7=1;
00093     IN8=0;
00094     IN9=0;
00095     IN10=1;
00096     IN11=1;
00097     IN12=0;
00098     wait_us(speed);
00099     IN5=0;
00100     IN6=1;
00101     IN7=1;
00102     IN8=0;
00103     IN9=1;
00104     IN10=0;
00105     IN11=1;
00106     IN12=0;
00107     wait_us(speed);
00108     IN5=0;
00109     IN6=1;
00110     IN7=0;
00111     IN8=1;
00112     IN9=1;
00113     IN10=0;
00114     IN11=0;
00115     IN12=1;
00116     wait_us(speed);
00117 }
00118 
00119 void stepAllRight()
00120 {
00121     //engine 1
00122     IN1=0;
00123     IN2=1;
00124     IN3=0;
00125     IN4=1;
00126     //engine 2
00127     IN5=0;
00128     IN6=1;
00129     IN7=0;
00130     IN8=1;
00131     //engine 3
00132     IN9=0;
00133     IN10=1;
00134     IN11=0;
00135     IN12=1;
00136     wait_us(speed); //legg som global variabel "fart" 
00137     //engine 1
00138     IN1=0;
00139     IN2=1;
00140     IN3=1;
00141     IN4=0;
00142     //engine 2
00143     IN5=0;
00144     IN6=1;
00145     IN7=1;
00146     IN8=0;
00147     //engine 3
00148     IN9=0;
00149     IN10=1;
00150     IN11=1;
00151     IN12=0;
00152     wait_us(speed);
00153     //engine 1
00154     IN1=1;
00155     IN2=0;
00156     IN3=1;
00157     IN4=0;
00158     //engine 2
00159     IN5=1;
00160     IN6=0;
00161     IN7=1;
00162     IN8=0;
00163     //engine 3
00164     IN9=1;
00165     IN10=0;
00166     IN11=1;
00167     IN12=0;
00168     wait_us(speed);
00169     //engine 1
00170     IN1=1;
00171     IN2=0;
00172     IN3=0;
00173     IN4=1;
00174     //engine 2
00175     IN5=1;
00176     IN6=0;
00177     IN7=0;
00178     IN8=1;
00179     //engine 3
00180     IN9=1;
00181     IN10=0;
00182     IN11=0;
00183     IN12=1;
00184     wait_us(speed);
00185 }
00186 void stepAllLeft()
00187 {
00188     //engine 1
00189     IN1=1;
00190     IN2=0;
00191     IN3=0;
00192     IN4=1;
00193     //engine 2
00194     IN5=1;
00195     IN6=0;
00196     IN7=0;
00197     IN8=1;
00198     //engine 3
00199     IN9=1;
00200     IN10=0;
00201     IN11=0;
00202     IN12=1;
00203     wait_us(speed);
00204     //engine 1
00205     IN1=1;
00206     IN2=0;
00207     IN3=1;
00208     IN4=0;
00209     //engine 2
00210     IN5=1;
00211     IN6=0;
00212     IN7=1;
00213     IN8=0;
00214     //engine 3
00215     IN9=1;
00216     IN10=0;
00217     IN11=1;
00218     IN12=0;
00219     wait_us(speed);
00220     //engine 1
00221     IN1=0;
00222     IN2=1;
00223     IN3=1;
00224     IN4=0;
00225     //engine 2
00226     IN5=0;
00227     IN6=1;
00228     IN7=1;
00229     IN8=0;
00230     //engine 3
00231     IN9=0;
00232     IN10=1;
00233     IN11=1;
00234     IN12=0;
00235     wait_us(speed);
00236     //engine 1
00237     IN1=0;
00238     IN2=1;
00239     IN3=0;
00240     IN4=1;
00241     //engine 2
00242     IN5=0;
00243     IN6=1;
00244     IN7=0;
00245     IN8=1;
00246     //engine 3
00247     IN9=0;
00248     IN10=1;
00249     IN11=0;
00250     IN12=1;
00251     wait_us(speed);
00252 }
00253 
00254 void stepEngine1Left()
00255 {
00256     //engine 1
00257     IN1=1;
00258     IN2=0;
00259     IN3=0;
00260     IN4=1;
00261     wait_us(speed);
00262     IN1=1;
00263     IN2=0;
00264     IN3=1;
00265     IN4=0;
00266     wait_us(speed);
00267     IN1=0;
00268     IN2=1;
00269     IN3=1;
00270     IN4=0;
00271     wait_us(speed);
00272     IN1=0;
00273     IN2=1;
00274     IN3=0;
00275     IN4=1;
00276     wait_us(speed);
00277 }
00278 
00279 void stepEngine1Right()
00280 {
00281     //engine 1
00282     IN1=0;
00283     IN2=1;
00284     IN3=0;
00285     IN4=1;
00286     wait_us(speed);
00287     IN1=0;
00288     IN2=1;
00289     IN3=1;
00290     IN4=0;
00291     wait_us(speed);
00292     IN1=1;
00293     IN2=0;
00294     IN3=1;
00295     IN4=0;
00296     wait_us(speed);
00297     IN1=1;
00298     IN2=0;
00299     IN3=0;
00300     IN4=1;
00301     wait_us(speed);
00302 }
00303 
00304 void stepEngine2Right()
00305 {
00306     //engine 1
00307     IN5=0;
00308     IN6=1;
00309     IN7=0;
00310     IN8=1;
00311     wait_us(speed);
00312     IN5=0;
00313     IN6=1;
00314     IN7=1;
00315     IN8=0;
00316     wait_us(speed);
00317     IN5=1;
00318     IN6=0;
00319     IN7=1;
00320     IN8=0;
00321     wait_us(speed);
00322     IN5=1;
00323     IN6=0;
00324     IN7=0;
00325     IN8=1;
00326     wait_us(speed);
00327 }
00328 
00329 void stepEngine2Left()
00330 {
00331     //engine 1
00332     IN5=1;
00333     IN6=0;
00334     IN7=0;
00335     IN8=1;
00336     wait_us(speed);
00337     IN5=1;
00338     IN6=0;
00339     IN7=1;
00340     IN8=0;
00341     wait_us(speed);
00342     IN5=0;
00343     IN6=1;
00344     IN7=1;
00345     IN8=0;
00346     wait_us(speed);
00347     IN5=0;
00348     IN6=1;
00349     IN7=0;
00350     IN8=1;
00351     wait_us(speed);
00352 }
00353 
00354 void stepEngine3Right()
00355 {
00356     //engine 1
00357     IN9=0;
00358     IN10=1;
00359     IN11=0;
00360     IN12=1;
00361     wait_us(speed);
00362     IN9=0;
00363     IN10=1;
00364     IN11=1;
00365     IN12=0;
00366     wait_us(speed);
00367     IN9=1;
00368     IN10=0;
00369     IN11=1;
00370     IN12=0;
00371     wait_us(speed);
00372     IN9=1;
00373     IN10=0;
00374     IN11=0;
00375     IN12=1;
00376     wait_us(speed);
00377 }
00378 
00379 void stepEngine3Left()
00380 {
00381     //engine 1
00382     IN9=1;
00383     IN10=0;
00384     IN11=0;
00385     IN12=1;
00386     wait_us(speed);
00387     IN9=1;
00388     IN10=0;
00389     IN11=1;
00390     IN12=0;
00391     wait_us(speed);
00392     IN9=0;
00393     IN10=1;
00394     IN11=1;
00395     IN12=0;
00396     wait_us(speed);
00397     IN9=0;
00398     IN10=1;
00399     IN11=0;
00400     IN12=1;
00401     wait_us(speed);
00402 }
00403 
00404 //Method to release all engines.
00405 void stopAll()
00406 {
00407     IN1=0;
00408     IN2=0;
00409     IN3=0;
00410     IN4=0;
00411     IN5=0;
00412     IN6=0;
00413     IN7=0;
00414     IN8=0;
00415     IN9=0;
00416     IN10=0;
00417     IN11=0;
00418     IN12=0;
00419 }
00420 
00421 //Methods to release single engines.
00422 void stopE1()
00423 {
00424     IN1=0;
00425     IN2=0;
00426     IN3=0;
00427     IN4=0;
00428     
00429 }
00430 void stopE2()
00431 {
00432     IN5=0;
00433     IN6=0;
00434     IN7=0;
00435     IN8=0;
00436     
00437 }
00438 void stopE3()
00439 {
00440     IN9=0;
00441     IN10=0;
00442     IN11=0;
00443     IN12=0;
00444     
00445 }
00446 
00447 void hold()
00448 {
00449     IN1=1;
00450     IN2=0;
00451     IN3=0;
00452     IN4=1;
00453     IN5=1;
00454     IN6=0;
00455     IN7=0;
00456     IN8=1;
00457     IN9=1;
00458     IN10=0;
00459     IN11=0;
00460     IN12=1;
00461 }
00462 
00463 
00464 void input(void const *args)
00465 {
00466     while(true)
00467     {
00468 
00469         if(pc.readable()) 
00470         { 
00471             m_cmd=pc.getc(); 
00472         }        
00473         Thread::wait(100);
00474         //pc.printf("%d", steps);
00475         //pc.printf("%c", m_cmd);    
00476     }
00477 }
00478 
00479 //Tråd for ultralyd
00480 void ultraSound(void const *args)
00481 {
00482     while(true)
00483     {
00484         
00485         sDistance = mu.getCurrentDistance();
00486         Thread::wait(100);
00487         dDistance = mu.getCurrentDistance();
00488         
00489         printf("Current Distance: %dmm\r\n", sDistance);
00490         printf("Current Change in Distance: %dmm\r\n", abs(sDistance-dDistance));
00491         if(abs(sDistance-dDistance) > 30 && abs(sDistance-dDistance) < 1000){
00492            if(dDistance >= 1000){
00493                
00494             }else if(dDistance < 1000 & dDistance >=750) {
00495                 sndBoard.play(0000);
00496             }
00497             else if(dDistance < 750 & dDistance >=300) {
00498                 sndBoard.play(0001);
00499             }
00500             else if(dDistance < 300 & dDistance >=50) {
00501                 sndBoard.play(0002);
00502             }else{
00503                 sndBoard.play(0003);
00504             } 
00505         }   
00506         Thread::wait(200);
00507     }
00508 }
00509 
00510 
00511                 
00512 //MOTOR 1 LEFT SIDE BUTTON
00513 void pb1_hit_callback (void) 
00514 {
00515 
00516         e1 = false;
00517         
00518         speed = 1000;
00519 
00520         wait(2);
00521         int stepsToCenter = i1/2;
00522                     
00523         while (stepsToCenter >= 0)
00524         {
00525             stepEngine1Right();
00526             stepsToCenter --;
00527         }
00528         steps1 = 0;
00529             //break;
00530         //end if
00531         m_cmd = 'x';  
00532     
00533 }
00534 
00535 //MOTOR 1 RIGHT SIDE BUTTON
00536 void pb2_hit_callback (void) 
00537 {
00538 
00539         e1 = false;
00540         //m_cmd = 'x';
00541     
00542         speed = 1000;
00543 
00544         wait(2);
00545         int stepsToCenter = i1/2;
00546                     
00547         while (stepsToCenter >= 0)
00548         {
00549             stepEngine1Left();
00550             stepsToCenter --;
00551         }
00552         steps1 = 0;
00553         m_cmd = 'x';
00554 }
00555 
00556 
00557 
00558 //MOTOR 2 LEFT SIDE BUTTON
00559 void pb3_hit_callback (void) 
00560 {
00561 
00562     {
00563         e2 = false;
00564         //m_cmd = 'x';
00565         speed = 1000;
00566         wait(1);
00567         int stepsToCenter = i2/2;
00568                     
00569         while (stepsToCenter >= 0)
00570         {
00571             stepEngine2Left();
00572             stepsToCenter --;
00573         }
00574         steps2 = 0;
00575         //m_cmd = 'x';
00576         stopE2();
00577     }
00578 
00579 }
00580 
00581 //MOTOR 2 RIGHT SIDE BUTTON
00582 void pb4_hit_callback (void) 
00583 {
00584 
00585     {
00586         e2 = false;
00587         //m_cmd = 'x';
00588         speed = 1000;
00589         wait(1);
00590         int stepsToCenter = i2/2;
00591                     
00592         while (stepsToCenter >= 0)
00593         {
00594             stepEngine2Right();
00595             stepsToCenter --;
00596         }
00597         steps2 = 0;
00598         //m_cmd = 'x';
00599         stopE2();
00600     }
00601 
00602 
00603 }
00604 
00605 
00606 //MOTOR 3 LEFT SIDE BUTTON
00607 void pb5_hit_callback (void) 
00608 {
00609 
00610         e3 = false;
00611         //m_cmd = 'x';
00612         speed = 1000;
00613         wait(1);
00614         int stepsToCenter = i3/2;
00615                     
00616         while (stepsToCenter >= 0)
00617         {
00618             if(stepsToCenter < i3/10)
00619             speed = 1500;
00620             
00621             stepEngine3Left();
00622             stepsToCenter --;
00623         }
00624         steps3 = 0;
00625         m_cmd = 'x';
00626 
00627 }
00628 //MOTOR 3 RIGHT SIDE BUTTON
00629 void pb6_hit_callback (void) 
00630 {
00631     
00632 
00633         e3 = false;
00634         //m_cmd = 'x';
00635         speed = 1000;
00636         wait(1);
00637         int stepsToCenter = i3/2;
00638                     
00639         while (stepsToCenter >= 0)
00640         {
00641             if(stepsToCenter < i3/10)
00642             speed = 1500;
00643             
00644             stepEngine3Right();
00645             stepsToCenter --;
00646         }
00647         //reset steps and stop engine
00648         steps3 = 0;
00649         m_cmd = 'x';
00650 
00651 }
00652  void dist(int distance)
00653 {
00654     //put code here to happen when the distance is changed
00655     //stepEngine2Right();
00656 }
00657 
00658 
00659 int main()
00660 {
00661     
00662 
00663 
00664         mu.startUpdates();//Start mesuring the distance
00665         sndBoard.reset(); //Resets the sound board
00666 
00667 
00668         pb1.mode(PullUp);
00669         pb2.mode(PullUp);
00670         pb3.mode(PullUp);
00671         pb4.mode(PullUp);
00672         pb5.mode(PullUp);
00673         pb6.mode(PullUp);
00674 
00675         //Set up buttons 1 and 2
00676         pb1.attach_deasserted(&pb1_hit_callback);
00677         pb1.setSampleFrequency();
00678         pb2.attach_deasserted(&pb2_hit_callback);
00679         pb2.setSampleFrequency();        
00680   
00681 
00682         //Thread 1 has constant feed from usb
00683         Thread t1(input);
00684         
00685         //Thread 2 - Ultrasound reading
00686         Thread t2(ultraSound);
00687         
00688         //static char global_direction;
00689         printf("Started");
00690   /*      
00691   //Prosedyre for å midtstille laveste ledd
00692   
00693         while (true)
00694         {
00695 
00696 
00697             stepEngine1Left();
00698             if (e1 == false) 
00699             {
00700                 Thread::wait(2000);
00701                 e1 = true;
00702                 while(true)
00703                 {
00704                     
00705                     stepEngine1Right(); 
00706                     i1++;
00707                     if(e1 == false)
00708                     break;
00709     
00710                 }
00711 
00712                 Thread::wait(1000);
00713                 break; 
00714             }//end if
00715            
00716         }//end while      
00717         
00718   */      
00719       
00720         //Set up buttons 3 and 4
00721         pb3.attach_deasserted(&pb3_hit_callback);
00722         pb3.setSampleFrequency();
00723         pb4.attach_deasserted(&pb4_hit_callback);
00724         pb4.setSampleFrequency();
00725  
00726  //midstille midterste ledd       
00727         while (true)
00728         {
00729             
00730             speed = 1000;
00731       /*      while(bootStep2 >= 0)
00732             {
00733                 bootStep2--;
00734                 stepEngine2Right();
00735                 
00736             } */
00737 
00738             stepEngine2Left();
00739             if (e2 == false) 
00740             {
00741                 e2 = true;
00742                 while(true)
00743                 {
00744                     
00745                     stepEngine2Right(); 
00746                     i2++;
00747                     if(e2 == false)
00748                     break;
00749     
00750                 }            
00751                 
00752                 Thread::wait(1000);
00753                 stopAll();
00754                 break;
00755             }//end if
00756         }
00757         
00758  
00759      //checking buttons 5 and 6   
00760         pb5.attach_deasserted(&pb5_hit_callback);
00761         pb5.setSampleFrequency();
00762         pb6.attach_deasserted(&pb6_hit_callback);
00763         pb6.setSampleFrequency();
00764     //Midstille toppledd
00765     
00766         while (true)
00767         {
00768             stepEngine3Left();
00769             if (e3 == false) 
00770             {
00771                 e3 = true;
00772                 while(true)
00773                 {
00774                     
00775                     stepEngine3Right(); 
00776                     i3++;
00777                     if(e3 == false)
00778                     break;
00779     
00780                 }
00781                 //pc.printf("HALLO");
00782                 //m_cmd = 'z';
00783                 Thread::wait(1000);
00784                 stopAll();
00785                 break;
00786             }//end if
00787         }
00788 stopAll();
00789         //normal runtime
00790         while (true) 
00791         {   
00792         
00793             //user button to turn off engines
00794             if(SW==0)
00795             {
00796                 while(true)
00797                 {
00798                     e3=true;
00799        //stopAll();
00800                         //Do something else here
00801      //mu.checkDistance();     //call checkDistance() as much as possible, as this is where
00802                                 //the class checks if dist needs to be called.
00803                 //put engines to sleep
00804               stepEngine3Right();
00805               if(e3==false)
00806               break;
00807               }
00808               
00809               stopAll();
00810                //stepEngine2Right();
00811              //  speed=1000;
00812              //   m_cmd ='z';
00813                 
00814                 
00815             }
00816     /*        //set the speed. Higher = slower
00817             speed = 1000;
00818             //speed = 1500;
00819 
00820             switch (m_cmd)
00821             {
00822                 case '7':
00823                 cmd = '7';
00824                 break;
00825                 
00826                 case '9':
00827                 cmd = '9';
00828                 break;
00829                 
00830                 case '4':
00831                 cmd= '4';
00832                 break;
00833                 
00834                 case '6':
00835                 cmd = '6';
00836                 break;
00837                 
00838                 case '1':
00839                 cmd = '1';
00840                 break;
00841                 
00842                 case '3':
00843                 cmd = '3';
00844                 break;
00845                 
00846                 case 'x':
00847                 cmd = 'x';
00848                 break;
00849                 
00850                 //release motors
00851                 case 'z':
00852                 
00853                 //step all motors to the middle
00854                 
00855                 while(steps1 > 0)
00856                 {
00857                     stepEngine1Left();
00858                     steps1--;
00859                 }
00860                 while(steps1 < 0)
00861                 {
00862                     stepEngine1Right();
00863                     steps1++;
00864                 }
00865                 
00866                 while(steps2 > 0)
00867                 {
00868                     stepEngine2Left();
00869                     steps2--;
00870                 }
00871                 while(steps2 < 0)
00872                 {
00873                     stepEngine2Right();
00874                     steps2++;
00875                 }
00876                 
00877                 while(steps3 > 0)
00878                 {
00879                     stepEngine3Left();
00880                     steps3--;
00881                 }
00882                 while(steps3 < 0)
00883                 {
00884                     stepEngine3Right();
00885                     steps3++;
00886                 }
00887 
00888                 cmd = 'z';
00889 
00890 
00891                 break;
00892                 
00893             }
00894             
00895             //Release all motors / Set all pins to 0
00896             if (cmd == 'z')
00897             {
00898                 stopAll();
00899             } 
00900             //Make all engines halt and hold still. 
00901             if (cmd == 'x')
00902             {
00903                 hold();
00904                 Thread::wait(1000);
00905                 m_cmd = 'z';
00906 
00907             }
00908             
00909             //Rest mode. Make the engines ignore pushbuttons.
00910             
00911             if(m_cmd == 'r')
00912             {
00913                 rest = true;
00914                 //Step motors to rest position
00915                 
00916             }
00917             */
00918   
00919   /*
00920             //Controlling each motor seperately.
00921             //ENGINE 3
00922             if(cmd == '7' && steps3 > -200)
00923             {
00924 
00925                 
00926                 steps3 --;
00927                 stepEngine3Left();
00928                 
00929             }
00930             else if (cmd == '9' && steps3 < 280)
00931             { 
00932 
00933                 stepEngine3Right();
00934                 steps3 ++;
00935             }
00936             //ENGINE 2
00937             else if (cmd == '4' && steps2 > -242)
00938             {
00939 
00940                 steps2 --;
00941                 stepEngine2Left();
00942             }
00943             else if (cmd == '6' && steps2 < 242)
00944             {
00945 
00946                 steps2 ++;
00947                 stepEngine2Right();
00948             }
00949             //ENGINE 3
00950             else if (cmd == '1' && steps1 > -242)
00951             {
00952 
00953                 steps1--;
00954                 stepEngine1Left();
00955             }
00956             else if (cmd == '3' && steps1 < 242)
00957             {
00958 
00959                 steps1++;
00960                 stepEngine1Right();
00961             }
00962             
00963             
00964             if(cmd == '7')
00965             {
00966 
00967                 
00968                 steps3 --;
00969                 stepEngine3Left();
00970                 
00971             }
00972             else if (cmd == '9')
00973             { 
00974 
00975                 stepEngine3Right();
00976                 steps3 ++;
00977             }
00978             //ENGINE 2
00979             else if (cmd == '4')
00980             {
00981 
00982                 steps2 --;
00983                 stepEngine2Left();
00984             }
00985             else if (cmd == '6')
00986             {
00987 
00988                 steps2 ++;
00989                 stepEngine2Right();
00990             }
00991             //ENGINE 3
00992             else if (cmd == '1')
00993             {
00994 
00995                 steps1--;
00996                 stepEngine1Left();
00997             }
00998             else if (cmd == '3')
00999             {
01000 
01001                 steps1++;
01002                 stepEngine1Right();
01003             }
01004             
01005             if (cmd == 'y')
01006             {
01007                 int step = 100;
01008                 while (step >= 0)
01009                 {
01010                     step--;
01011                     step2Left3Right();
01012                 }
01013                 m_cmd = 'x';
01014                 
01015             }
01016             
01017             if (cmd == 't')
01018             {
01019                 int step1, step2, step3;
01020                 step1 = 100;
01021                 step2 = 60;
01022                 step3 = 70;
01023                 
01024                 while (step1 > 0)
01025                 {
01026                     stepEngine1Left();
01027                     step1--;
01028                     //global stepcounter
01029                     steps1--;
01030                 }
01031                 while(step2 > 0)
01032                 {
01033                     stepEngine2Right();
01034                     step2--;
01035                     //global stepcounter
01036                     steps2++;
01037                 }
01038                 while(step3 > 0)
01039                 {
01040                     stepEngine3Left();
01041                     step3--;
01042                     //global stepcounter
01043                     steps3--;
01044                 }
01045                 
01046                 //m_cmd = 'x';    
01047                 
01048             }
01049 
01050         //Thread::wait(10);
01051         //pc.printf("%d", steps2);
01052 */
01053 
01054         }  //END WHILE TRUE
01055             
01056 } //END Main