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 = 457; //adjust if needed
00023     int lp = 64;  //adjust if needed
00024     int tp = 25;  
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){          //450,440        
00050             sheets = 1;
00051             pc.printf("\nSheets selected:%ld",sheets);
00052         }
00053         
00054         if (dist<= fp-tp+er && dist>=fp-tp-er){   //424,414   
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){       // 75, 65 
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 = 11;
00127     int sens_pos = 126; //adjust if needed //535 0 plates
00128     int sheet_th = 25; 
00129     pathsensor.start();
00130     wait_ms(500);
00131     path_dist=pathsensor.get_dist_cm();
00132     pc.printf("\nBeam Path Sensor : %ld",path_dist);
00133     path_sheets = 17;
00134         
00135             if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){        //110,130          
00136                path_sheets = 16;
00137                 
00138             }
00139             if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ //152,168 a:
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){ //468,478
00188                path_sheets = 3;
00189                 
00190             }
00191             if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ //494,504
00192                path_sheets = 2;
00193                 
00194             }
00195             if (path_dist >= sens_pos-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){//greater than 509 less  517
00196                path_sheets = 1;
00197                 
00198             }
00199             if (path_dist > sens_pos-err+16*sheet_th){  //greater than 534
00200                path_sheets = 0;
00201             }
00202 }
00203 
00204 void platedetect()
00205 { 
00206     if(homeSwitch==0 && path_sheets==0) 
00207     {
00208     pc.printf("\nAll plates are in the start position");
00209     }
00210     else if (homeSwitch==1 && path_sheets==0)
00211     {
00212     pc.printf("\nError-Plates are not fully extended");    
00213     }
00214     else 
00215     {
00216         if (plates==path_sheets)
00217         {
00218         pc.printf("\nPlates in beam path:%ld",path_sheets);
00219         }
00220         else if (plates != path_sheets && path_sheets <= 16)
00221         {
00222         pc.printf("\nError: %ld",path_sheets);
00223         pc.printf(" Plates in beam path, but user sent %ld",plates);
00224         pc.printf(" Plates");
00225         }
00226         else{
00227              pc.printf("\nERROR: Additional objects detected in beam path");
00228             }
00229     }    
00230 }
00231 void retract()
00232 {
00233     while(homeSwitch == 1)
00234     {
00235     in1=0;
00236     in2=1;
00237     }
00238     in1=1;
00239     in2=1;
00240 }
00241 
00242 void send()                 
00243 { 
00244     in1=1;
00245     in2=0;
00246     wait(17);
00247     in1=1;
00248     in2=1;
00249 }
00250 
00251 void internalpullups()
00252 {
00253     homeSwitch.mode(PullUp);
00254     wait (.01);
00255 }
00256 
00257 void send_error_check()
00258 {
00259     if (plates <= 0 || plates >= 17) 
00260     {
00261         pc.printf("\nERROR: Invalid Range |");
00262     } 
00263     else if(homeSwitch == 1)
00264     {
00265         pc.printf("\nERROR: Not at Home Position |");
00266     }
00267     else 
00268     {
00269         plate_selection();
00270         send();
00271     }
00272 }
00273 void select_error_check()
00274 {
00275     if (homeSwitch == 1) 
00276     {
00277         pc.printf("\nERROR: Not at Home Position |");
00278     } 
00279     else 
00280     {
00281         plate_selection();
00282     }
00283 }
00284 void retract_error_check()
00285 {
00286     if (homeSwitch == 0)
00287     {
00288         pc.printf("\nERROR: Already at Home Position |");   
00289     }
00290     else
00291     {
00292         retract();
00293     }
00294 }
00295 
00296 int main()
00297 {   
00298     in1=1;
00299     in2=1;
00300     en = 1;
00301     wait(0.1);
00302     internalpullups();
00303     wait(0.1);  
00304     char command[15] = {0};
00305     pc.printf("\nCommands:\n\n\ts [N]\t**moves N PMMA sheets in beam path\n\tr\t\t**removes all PMMA from beam path\n\tc [N]\t**raises bar to \n\tb\t\t**reports number of plates in beam path\n\n");
00306     
00307     while(1) 
00308     {
00309         pc.printf("\nRequests:  ");
00310         pc.scanf("%s",&command);
00311     
00312         if (strcmp (command, "s") == 0) 
00313         {   
00314             pc.scanf(" %d",&plates);
00315             pc.printf("\nresponse recieved |");
00316             send_error_check();
00317             beam();
00318             platedetect();
00319         }
00320         else if (strcmp (command, "r") == 0)
00321         {   
00322             pc.printf("\nresponse recieved |");
00323             retract_error_check();
00324         }
00325         else if (strcmp (command, "c") == 0)
00326         {   
00327             pc.scanf(" %d",&plates);
00328             pc.printf("\nresponse recieved |");
00329             select_error_check();
00330         }
00331          else if (strcmp (command, "b") == 0)
00332         {   
00333             pc.printf("\nresponse recieved |");
00334             beam();
00335             platedetect();
00336         }
00337         else
00338         {
00339             pc.printf("\nERROR: Invalid Command");       
00340         }
00341     }
00342 }