main.cpp

00001 #include "mbed.h"
00002 #include "Stepper.h"
00003 #include "string"
00004 #include "hcsr04.h"
00005 Serial pc(USBTX, USBRX);
00006 HCSR04 heightsensor(PTA2,PTC2); //(trig,echo)
00007 HCSR04 pathsensor(PTD3,PTD2);
00008 DigitalOut in1(PTC12);
00009 DigitalOut in2(D7);
00010 DigitalIn homeSwitch(D9);
00011 Stepper mot(D3,D4);  //(D3 to PUL+, D4 to DIR+)
00012 DigitalOut en(D2); 
00013 
00014 volatile int plates=0;
00015 volatile unsigned int sheets=0;
00016 volatile unsigned int path_sheets=0;
00017 
00018 void plate_selection()
00019 {
00020     unsigned int dist;
00021     int er = 5;
00022     int fp = 470; //adjust if needed
00023     int lp = 75;  //adjust if needed
00024     int tp = 26;  
00025     
00026     while (plates!=sheets) {
00027         heightsensor.start();
00028         wait_ms(500);
00029         dist=heightsensor.get_dist_cm(); 
00030         pc.printf("\nmm:%ld",dist);
00031         
00032         if (dist<=lp-10 || dist >= fp+10){
00033         mot.stop();
00034         en = 1;
00035         sheets = plates;
00036         pc.printf("\nERROR: Selector is outside of plate range\nRestart/Reset Program");
00037         }
00038         else if (plates<sheets){
00039             en = 0;
00040             mot.setSpeed(600);
00041             mot.rotate(1);
00042         }
00043         else{
00044             en = 0;
00045             mot.setSpeed(600);
00046             mot.rotate(0);
00047         }
00048         
00049         if (dist<=fp+er && dist>=fp-er){                  
00050             sheets = 1;
00051             pc.printf("\nSheets selected:%ld",sheets);
00052         }
00053         
00054         if (dist<= fp-tp-er && dist>=fp-tp-er){       
00055             sheets = 2;
00056             pc.printf("\nSheets selected:%ld",sheets);
00057         }
00058         
00059         if (dist<= fp-2*tp-er && dist>=fp-2*tp-er){     
00060             sheets = 3;
00061             pc.printf("\nSheets selected:%ld",sheets);
00062         }
00063         
00064         if (dist<= fp-3*tp-er && dist>=fp-3*tp-er){         
00065             sheets = 4;
00066             pc.printf("\nSheets selected:%ld",sheets);
00067         }
00068          if (dist<= fp-4*tp-er && dist>=fp-4*tp-er){         
00069             sheets = 5;
00070             pc.printf("\nSheets selected:%ld",sheets);
00071         }
00072          if (dist<= fp-5*tp-er && dist>=fp-5*tp-er){       
00073             sheets = 6;
00074             pc.printf("\nSheets selected:%ld",sheets);
00075         }
00076          if (dist<= fp-6*tp-er && dist>=fp-6*tp-er){     
00077             sheets = 7;
00078             pc.printf("\nSheets selected:%ld",sheets);
00079         }
00080          if (dist<= fp-7*tp-er && dist>=fp-7*tp-er){         
00081             sheets = 8;
00082             pc.printf("\nSheets selected:%ld",sheets);
00083         }
00084          if (dist<= fp-8*tp-er && dist>=fp-8*tp-er){       
00085             sheets = 9;
00086             pc.printf("\nSheets selected:%ld",sheets);
00087         }
00088          if (dist<= fp-9*tp-er && dist>=fp-9*tp-er){         
00089             sheets = 10;
00090             pc.printf("\nSheets selected:%ld",sheets);
00091         }
00092          if (dist<= fp-10*tp-er && dist>=fp-10*tp-er){         
00093             sheets = 11;
00094             pc.printf("\nSheets selected:%ld",sheets);
00095         }
00096          if (dist<= fp-11*tp-er && dist>=fp-11*tp-er){        
00097             sheets = 12;
00098             pc.printf("\nSheets selected:%ld",sheets);
00099         }
00100          if (dist<= fp-12*tp-er && dist>=fp-12*tp-er){         
00101             sheets = 13;
00102             pc.printf("\nSheets selected:%ld",sheets);
00103         }
00104          if (dist<= fp-13*tp-er && dist>=fp-13*tp-er){        
00105             sheets = 14;
00106             pc.printf("\nSheets selected:%ld",sheets);
00107         }
00108          if (dist<= fp-14*tp-er && dist>=fp-14*tp-er){      
00109             sheets = 15;
00110             pc.printf("\nSheets selected:%ld",sheets);
00111         }
00112          if (dist<= fp-15*tp-er && dist>=fp-15*tp-er){         
00113             sheets = 16;
00114             pc.printf("\nSheets selected:%ld",sheets);
00115         }
00116     }
00117     mot.stop();
00118     en = 1;
00119     wait(1);
00120 }
00121 
00122 void beam()
00123 {
00124     
00125     unsigned int path_dist;
00126     int err = 5;
00127     int sens_pos = 133; //adjust if needed
00128     int sheet_th = 26; 
00129     pathsensor.start();
00130     wait_ms(500);
00131     path_dist=pathsensor.get_dist_cm();
00132     pc.printf("\nmm:%ld",path_dist);
00133     path_sheets = 17;
00134         
00135             if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){                   //127-139
00136                path_sheets = 16;
00137                 
00138             }
00139             if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ // 153-165 
00140                path_sheets = 15;
00141                 
00142             }
00143             if (path_dist >= sens_pos-err+2*sheet_th && path_dist <= sens_pos+err+2*sheet_th){ //179-191
00144                path_sheets = 14;
00145                 
00146             }
00147             if (path_dist >= sens_pos-err+3*sheet_th && path_dist <= sens_pos+err+3*sheet_th){ //205-217
00148                path_sheets = 13;
00149                  
00150             }
00151             if (path_dist >= sens_pos-err+4*sheet_th && path_dist <= sens_pos+err+4*sheet_th){ //231-243
00152                path_sheets = 12;
00153                 
00154             }
00155             if (path_dist >= sens_pos-err+5*sheet_th && path_dist <= sens_pos+err+5*sheet_th){ //257-269
00156                path_sheets = 11;
00157                 
00158             }
00159             if (path_dist >= sens_pos-err+6*sheet_th && path_dist <= sens_pos+err+6*sheet_th){ //283-295
00160                path_sheets = 10;
00161                 
00162             }
00163             if (path_dist >= sens_pos-err+7*sheet_th && path_dist <= sens_pos+err+7*sheet_th){ //309-321
00164                path_sheets = 9;
00165                 
00166             }
00167             if (path_dist >= sens_pos-err+8*sheet_th && path_dist <= sens_pos+err+8*sheet_th){ //323-333
00168                path_sheets = 8;
00169                 
00170             }
00171             if (path_dist >= sens_pos-err+9*sheet_th && path_dist <= sens_pos+err+9*sheet_th){ //349-359
00172                path_sheets = 7;
00173                 
00174             }
00175             if (path_dist >= sens_pos-err+10*sheet_th && path_dist <= sens_pos+err+10*sheet_th){
00176                path_sheets = 6;
00177                 
00178             }
00179             if (path_dist >= sens_pos-err+11*sheet_th && path_dist <= sens_pos+err+11*sheet_th){
00180                path_sheets = 5;
00181                 
00182             }
00183             if (path_dist >= sens_pos-err+12*sheet_th && path_dist <= sens_pos+err+12*sheet_th){
00184                path_sheets = 4;
00185                 
00186             }
00187             if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ //465-477
00188                path_sheets = 3;
00189                 
00190             }
00191             if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){
00192                path_sheets = 2;
00193                 
00194             }
00195             if (path_dist >= sens_pos-err+15*sheet_th && path_dist <= sens_pos+err+15*sheet_th){
00196                path_sheets = 1;
00197                 
00198             }
00199             if (path_dist > sens_pos-err+16*sheet_th){
00200                path_sheets = 0;
00201             }
00202 }
00203 void platedetect()
00204 { 
00205     if(homeSwitch==0 && path_sheets==0) 
00206     {
00207     pc.printf("\nAll plates are in the start position");
00208     }
00209     else if (homeSwitch==1 && path_sheets==0)
00210     {
00211     pc.printf("\nError-Plates are not fully extended");    
00212     }
00213     else 
00214     {
00215         if (plates==path_sheets)
00216         {
00217         pc.printf("\nPlates in beam path:%ld",path_sheets);
00218         }
00219         else if (plates != path_sheets && path_sheets <= 16)
00220         {
00221         pc.printf("\nError: %ld",path_sheets);
00222         pc.printf(" Plates in beam path, but user sent %ld",plates);
00223         pc.printf(" Plates");
00224         }
00225         else{
00226              pc.printf("\nERROR: Additional objects detected in beam path");
00227             }
00228     }    
00229 }
00230 void retract()
00231 {
00232     while(homeSwitch == 1)
00233     {
00234     in1=0;
00235     in2=1;
00236     }
00237     in1=1;
00238     in2=1;
00239 }
00240 
00241 void send()                 
00242 { 
00243     in1=1;
00244     in2=0;
00245     wait(17);
00246     in1=1;
00247     in2=1;
00248 }
00249 void internalpullups()
00250 {
00251     homeSwitch.mode(PullUp);
00252     wait (.01);
00253 }
00254 
00255 void send_error_check()
00256 {
00257     if (plates <= 0 || plates >= 17) 
00258     {
00259         pc.printf("\nERROR: Invalid Range |");
00260     } 
00261     else if(homeSwitch == 1)
00262     {
00263         pc.printf("\nERROR: Not at Home Position |");
00264     }
00265     else 
00266     {
00267         plate_selection();
00268         send();
00269     }
00270 }
00271 
00272 int main() {
00273   
00274    while(1){
00275         plates = (rand() % 16) + 1;
00276         pc.printf("\nSending: ",plates);
00277         send_error_check();
00278         beam();
00279         platedetect();
00280         wait(2);
00281         retract();
00282        }
00283    
00284    
00285 }