main.cpp

00001 #include "mbed.h"
00002 #include "Stepper.h"
00003 #include "string"
00004 #include "hcsr04.h"
00005 #include "millis.h"
00006 
00007 Serial pc(USBTX, USBRX);
00008 HCSR04 heightsensor(PTD2,PTD3);
00009 HCSR04 pathsensor(PTC2,PTA2); //(trig,echo)
00010 DigitalOut in1(PTC12);
00011 DigitalOut in2(D7);
00012 DigitalIn magnetSwitch(D10);
00013 DigitalIn homeSwitch(D9);
00014 Stepper mot(D3,D4);  
00015 DigitalOut en(D2); 
00016 
00017 int main(){
00018     
00019    
00020      en = 1;
00021     in1 = 1;
00022     in2 = 1;
00023     magnetSwitch.mode(PullUp);
00024     wait (.01);
00025     homeSwitch.mode(PullUp);
00026     wait (0.01);
00027     
00028 while(1){
00029      
00030      LOOPY:
00031      
00032 for ( int command = 1; command < 4; command++){          
00033 
00034     if (command==1){
00035         
00036         int plates = 0;
00037         unsigned int dist;
00038         int platedist;
00039         
00040         plates = (rand() % 16) + 1;
00041         
00042         if (magnetSwitch == 1){
00043             pc.printf("\nNot in Home Position"); 
00044             exit(1);
00045             }
00046         else{
00047             wait(1);
00048             in1=1;
00049             in2=0;
00050             wait_ms(900);
00051             in1=1;
00052             in2=1;
00053             }
00054         pc.printf("\nRequested Plates:%ld",plates);
00055         platedist = (468-24*plates);  
00056         millisStart();
00057         long durationSM = 0;
00058         long starttimeSM = millis();  
00059         
00060         SENSORREAD:
00061         heightsensor.start();
00062         wait_ms(250); 
00063         dist = heightsensor.get_dist_cm(); 
00064          
00065         
00066         while ((dist < platedist-2 && durationSM < 60000 || dist > platedist+2 && durationSM < 60000)){
00067              
00068              
00069               heightsensor.start();
00070               long currenttimeSM = millis();
00071               durationSM = currenttimeSM-starttimeSM;
00072               en = 0;
00073               mot.setSpeed(500);
00074               dist = heightsensor.get_dist_cm(); 
00075               
00076               if(dist >= 480 || dist <= 40 ){
00077                  pc.printf("\nBad Read at :%ld", durationSM); 
00078                  goto SENSORREAD;
00079               }
00080               
00081               if(dist<platedist-2){ //397 403
00082                  mot.rotate(1);
00083                  wait(0.5);
00084                 }
00085               if(dist>platedist+2){ 
00086                 mot.rotate(0);
00087                 wait(0.5);
00088                }
00089               mot.stop();
00090         }
00091         
00092         pc.printf("\nSelector Height:%ld", dist);
00093         pc.printf("\nSelector Motor Duration:%ld", durationSM);
00094         mot.stop(); 
00095         en = 1;
00096         wait(1);
00097         
00098         if (dist >= 480 || dist <= 40){
00099             pc.printf("\nSelector Distance out of Range");
00100             exit(1);
00101             }
00102         else if (durationSM >= 60000){
00103             pc.printf("\nSelector Motor Time Out");
00104             exit(1);
00105             }
00106         else{
00107             in1=1;
00108             in2=0;
00109             wait(56);
00110             in1=1;
00111             in2=1;
00112             pc.printf("\nSending Finished");
00113             }         
00114     }
00115   
00116     else if (command==2){
00117             
00118             unsigned int pathdist;
00119             int path_sheets;
00120             int detectionTOL = 12;
00121             int sens_pos = 131;          
00122             int sheet_th = 25; 
00123             pathsensor.start();
00124             wait_ms(500);
00125             pathdist=pathsensor.get_dist_cm();
00126                 
00127             if (pathdist >= sens_pos-detectionTOL && pathdist <= sens_pos+detectionTOL){        //110,130          
00128                 path_sheets = 16; 
00129                 }
00130             else if (pathdist >= sens_pos-detectionTOL+sheet_th && pathdist <= sens_pos+detectionTOL+sheet_th){ //152,168 a:
00131                 path_sheets = 15;   
00132                 }
00133             else if (pathdist >= sens_pos-detectionTOL+2*sheet_th && pathdist <= sens_pos+detectionTOL+2*sheet_th){ //179-191
00134                 path_sheets = 14;
00135                 }
00136             else if (pathdist >= sens_pos-detectionTOL+3*sheet_th && pathdist <= sens_pos+detectionTOL+3*sheet_th){ //205-217
00137                 path_sheets = 13;                 
00138                 }
00139             else if (pathdist >= sens_pos-detectionTOL+4*sheet_th && pathdist <= sens_pos+detectionTOL+4*sheet_th){ //231-243
00140                 path_sheets = 12;
00141                 }
00142             else if (pathdist >= sens_pos-detectionTOL+5*sheet_th && pathdist <= sens_pos+detectionTOL+5*sheet_th){ //257-269
00143                 path_sheets = 11;
00144                 }
00145             else if (pathdist >= sens_pos-detectionTOL+6*sheet_th && pathdist <= sens_pos+detectionTOL+6*sheet_th){ //283-295
00146                path_sheets = 10;
00147                 
00148             }
00149             else if (pathdist >= sens_pos-detectionTOL+7*sheet_th && pathdist <= sens_pos+detectionTOL+7*sheet_th){ //309-321
00150                path_sheets = 9;
00151                 
00152             }
00153             else if (pathdist >= sens_pos-detectionTOL+8*sheet_th && pathdist <= sens_pos+detectionTOL+8*sheet_th){ //323-333
00154                path_sheets = 8;
00155                 
00156             }
00157             else if (pathdist >= sens_pos-detectionTOL+9*sheet_th && pathdist <= sens_pos+detectionTOL+9*sheet_th){ //349-359
00158                path_sheets = 7;
00159                 
00160             }
00161             else if (pathdist >= sens_pos-detectionTOL+10*sheet_th && pathdist <= sens_pos+detectionTOL+10*sheet_th){
00162                path_sheets = 6;
00163                 
00164             }
00165             else if (pathdist >= sens_pos-detectionTOL+11*sheet_th && pathdist <= sens_pos+detectionTOL+11*sheet_th){
00166                path_sheets = 5;
00167                 
00168             }
00169             else if (pathdist >= sens_pos-detectionTOL+12*sheet_th && pathdist <= sens_pos+detectionTOL+12*sheet_th){
00170                path_sheets = 4;
00171                 
00172             }
00173             else if (pathdist >= sens_pos-detectionTOL+13*sheet_th && pathdist <= sens_pos+detectionTOL+13*sheet_th){ //468,478
00174                path_sheets = 3;
00175                 
00176             }
00177             else if (pathdist >= sens_pos-detectionTOL+14*sheet_th && pathdist <= sens_pos+detectionTOL+14*sheet_th){ //494,504
00178                path_sheets = 2;
00179                 
00180             }
00181             else if (pathdist >= sens_pos-10-detectionTOL+15*sheet_th && pathdist <= sens_pos-10+detectionTOL+15*sheet_th){//greater than 509 less  517
00182                path_sheets = 1;
00183                 
00184             }
00185             else if (pathdist > sens_pos-detectionTOL+16*sheet_th){  //greater than 534
00186                path_sheets = 0;
00187               } 
00188             else {
00189                  pc.printf("\nBeam Path Detector distance out of range : %ld", pathdist);
00190                  wait(2);
00191                  path_sheets = 111;
00192                  }
00193             pc.printf("\nDetected Plates: %ld", path_sheets);
00194             wait(2);
00195         }
00196         
00197 else if (command==3){
00198         
00199         millisStart();
00200         long Duration = 0;
00201         long StartTime = millis();
00202     
00203         while(magnetSwitch == 1 && homeSwitch == 1 && Duration <= 52000 ){
00204              in1=0;
00205              in2=1;
00206              long CurrentTime = millis();
00207              Duration = CurrentTime-StartTime;
00208         }
00209         
00210         in1=1;
00211         in2=1;
00212         
00213         if (Duration > 52000){
00214              pc.printf("\nRetraction Time Out Error: %d\r\n", Duration);
00215              exit(1);
00216             }
00217         else if (homeSwitch == 0){
00218              pc.printf("\nMagnet Switch Broken");
00219              exit(1);
00220             }
00221         else{
00222             pc.printf("\nRetraction Complete");
00223             goto LOOPY;
00224             }
00225      }
00226      
00227 else {
00228       pc.printf("\nInvalid Command");
00229     }
00230 }
00231 }
00232 }