main.cpp

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