main.cpp

00001 #include "mbed.h"
00002 #include "Stepper.h"
00003 #include "string"
00004 #include "hcsr04.h"
00005 #include "millis.h"
00006 //#include "SDFileSystem.h"
00007 
00008 Serial pc(USBTX, USBRX);
00009 HCSR04 pathsensor(PTC2,PTA2); //(trig,echo)
00010 HCSR04 heightsensor(PTD2,PTD3);
00011 DigitalOut in1(PTC12);
00012 DigitalOut in2(D7);
00013 
00014 //DigitalIn homeSwitch(D9);
00015 DigitalIn magnetSwitch(D10);
00016 Stepper mot(D3,D4);  //(D3 to PUL+, D4 to DIR+)
00017 DigitalOut en(D2); 
00018 
00019 volatile int plates=0;
00020 volatile unsigned int sheets=0;
00021 volatile unsigned int path_sheets=0;
00022 
00023 void plate_selection()
00024 {
00025     unsigned int dist;
00026     int er = 5;
00027     int fp = 447; //adjust if needed
00028     int lp = 64;  //adjust if needed
00029     int tp = 25;  
00030     wait(1);
00031     in1=1;
00032     in2=0;
00033     wait_ms(850);
00034     in1=1;
00035     in2=1;
00036     wait(1);
00037     while (plates!=sheets) {
00038         heightsensor.start();
00039         wait_ms(500);
00040         dist=heightsensor.get_dist_cm(); 
00041         pc.printf("\nmm:%ld",dist);
00042         
00043         if (dist<=lp-10 || dist >= fp+10){
00044         mot.stop();
00045         en = 1;
00046         sheets = plates;
00047         pc.printf("\nERROR: Selector is outside of plate range\nRestart/Reset Program");
00048         }
00049         else if (plates<sheets){
00050             en = 0;
00051             mot.setSpeed(600);
00052             mot.rotate(1);
00053         }
00054         else{
00055             en = 0;
00056             mot.setSpeed(600);
00057             mot.rotate(0);
00058         }
00059         
00060         if (dist<=fp+er && dist>=fp-er){          //450,440        
00061             sheets = 1;
00062             pc.printf("\nSheets selected:%ld",sheets);
00063         }
00064         
00065         if (dist<= fp-tp+er && dist>=fp-tp-er){   //424,414   
00066             sheets = 2;
00067             pc.printf("\nSheets selected:%ld",sheets);
00068         }
00069         
00070         if (dist<= fp-2*tp+er && dist>=fp-2*tp-er){     
00071             sheets = 3;
00072             pc.printf("\nSheets selected:%ld",sheets);
00073         }
00074         
00075         if (dist<= fp-3*tp+er && dist>=fp-3*tp-er){         
00076             sheets = 4;
00077             pc.printf("\nSheets selected:%ld",sheets);
00078         }
00079          if (dist<= fp-4*tp+er && dist>=fp-4*tp-er){         
00080             sheets = 5;
00081             pc.printf("\nSheets selected:%ld",sheets);
00082         }
00083          if (dist<= fp-5*tp+er && dist>=fp-5*tp-er){       
00084             sheets = 6;
00085             pc.printf("\nSheets selected:%ld",sheets);
00086         }
00087          if (dist<= fp-6*tp+er && dist>=fp-6*tp-er){     
00088             sheets = 7;
00089             pc.printf("\nSheets selected:%ld",sheets);
00090         }
00091          if (dist<= fp-7*tp+er && dist>=fp-7*tp-er){         
00092             sheets = 8;
00093             pc.printf("\nSheets selected:%ld",sheets);
00094         }
00095          if (dist<= fp-8*tp+er && dist>=fp-8*tp-er){       
00096             sheets = 9;
00097             pc.printf("\nSheets selected:%ld",sheets);
00098         }
00099          if (dist<= fp-9*tp+er && dist>=fp-9*tp-er){         
00100             sheets = 10;
00101             pc.printf("\nSheets selected:%ld",sheets);
00102         }
00103          if (dist<= fp-10*tp+er && dist>=fp-10*tp-er){         
00104             sheets = 11;
00105             pc.printf("\nSheets selected:%ld",sheets);
00106         }
00107          if (dist<= fp-11*tp+er && dist>=fp-11*tp-er){        
00108             sheets = 12;
00109             pc.printf("\nSheets selected:%ld",sheets);
00110         }
00111          if (dist<= fp-12*tp+er && dist>=fp-12*tp-er){         
00112             sheets = 13;
00113             pc.printf("\nSheets selected:%ld",sheets);
00114         }
00115          if (dist<= fp-13*tp+er && dist>=fp-13*tp-er){        
00116             sheets = 14;
00117             pc.printf("\nSheets selected:%ld",sheets);
00118         }
00119          if (dist<= fp-14*tp+er && dist>=fp-14*tp-er){      
00120             sheets = 15;
00121             pc.printf("\nSheets selected:%ld",sheets);
00122         }
00123          if (dist<= fp-15*tp+er && dist>=fp-15*tp-er){       // 75, 65 
00124             sheets = 16;
00125             pc.printf("\nSheets selected:%ld",sheets);
00126         }
00127     }
00128     mot.stop();
00129     en = 1;
00130     wait(1);
00131 }
00132 
00133 void beam()
00134 {
00135     
00136     unsigned int path_dist;
00137     int err = 12;
00138     int sens_pos = 131; //adjust if needed //535 0 plates
00139     int sheet_th = 25; 
00140     pathsensor.start();
00141     wait_ms(500);
00142     path_dist=pathsensor.get_dist_cm();
00143     pc.printf("\nBeam Path Sensor : %ld",path_dist);
00144     path_sheets = 17;
00145         
00146             if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){        //110,130          
00147                path_sheets = 16;
00148                 
00149             }
00150             if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ //152,168 a:
00151                path_sheets = 15;
00152                 
00153             }
00154             if (path_dist >= sens_pos-err+2*sheet_th && path_dist <= sens_pos+err+2*sheet_th){ //179-191
00155                path_sheets = 14;
00156                 
00157             }
00158             if (path_dist >= sens_pos-err+3*sheet_th && path_dist <= sens_pos+err+3*sheet_th){ //205-217
00159                path_sheets = 13;
00160                  
00161             }
00162             if (path_dist >= sens_pos-err+4*sheet_th && path_dist <= sens_pos+err+4*sheet_th){ //231-243
00163                path_sheets = 12;
00164                 
00165             }
00166             if (path_dist >= sens_pos-err+5*sheet_th && path_dist <= sens_pos+err+5*sheet_th){ //257-269
00167                path_sheets = 11;
00168                 
00169             }
00170             if (path_dist >= sens_pos-err+6*sheet_th && path_dist <= sens_pos+err+6*sheet_th){ //283-295
00171                path_sheets = 10;
00172                 
00173             }
00174             if (path_dist >= sens_pos-err+7*sheet_th && path_dist <= sens_pos+err+7*sheet_th){ //309-321
00175                path_sheets = 9;
00176                 
00177             }
00178             if (path_dist >= sens_pos-err+8*sheet_th && path_dist <= sens_pos+err+8*sheet_th){ //323-333
00179                path_sheets = 8;
00180                 
00181             }
00182             if (path_dist >= sens_pos-err+9*sheet_th && path_dist <= sens_pos+err+9*sheet_th){ //349-359
00183                path_sheets = 7;
00184                 
00185             }
00186             if (path_dist >= sens_pos-err+10*sheet_th && path_dist <= sens_pos+err+10*sheet_th){
00187                path_sheets = 6;
00188                 
00189             }
00190             if (path_dist >= sens_pos-err+11*sheet_th && path_dist <= sens_pos+err+11*sheet_th){
00191                path_sheets = 5;
00192                 
00193             }
00194             if (path_dist >= sens_pos-err+12*sheet_th && path_dist <= sens_pos+err+12*sheet_th){
00195                path_sheets = 4;
00196                 
00197             }
00198             if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ //468,478
00199                path_sheets = 3;
00200                 
00201             }
00202             if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ //494,504
00203                path_sheets = 2;
00204                 
00205             }
00206             if (path_dist >= sens_pos-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){//greater than 509 less  517
00207                path_sheets = 1;
00208                 
00209             }
00210             if (path_dist > sens_pos-err+16*sheet_th){  //greater than 534
00211                path_sheets = 0;
00212             }
00213 }
00214 
00215 void platedetect()
00216 { 
00217     if(magnetSwitch==0 && path_sheets==0) 
00218     {
00219     pc.printf("\nAll plates are in the start position");
00220     }
00221     else if (magnetSwitch==1 && path_sheets==0)
00222     {
00223     pc.printf("\nError-Plates are not fully extended");    
00224     }
00225     else 
00226     {
00227         if (plates==path_sheets)
00228         {
00229         pc.printf("\nPlates in beam path:%ld",path_sheets);
00230         }
00231         else if (plates != path_sheets && path_sheets <= 16)
00232         {
00233         pc.printf("\nError: %ld",path_sheets);
00234         pc.printf(" Plates in beam path, but user sent %ld",plates);
00235         pc.printf(" Plates");
00236         }
00237         else{
00238              pc.printf("\nERROR: Additional objects detected in beam path");
00239             }
00240     }    
00241 }
00242 void retract()
00243 {
00244     millisStart();
00245     long Duration = 0;
00246     long StartTime = millis();
00247     
00248     while(magnetSwitch == 1 && Duration <= 48000 )
00249     {
00250     in1=0;
00251     in2=1;
00252     long CurrentTime = millis();
00253     Duration = CurrentTime-StartTime;
00254     pc.printf("Duration = %d\r\n", Duration); 
00255     }
00256     in1=1;
00257     in2=1;
00258 }
00259 
00260 void send()                 
00261 { 
00262     in1=1;
00263     in2=0;
00264     wait(56);
00265     in1=1;
00266     in2=1;
00267 }
00268 
00269 void internalpullups()
00270 {
00271     magnetSwitch.mode(PullUp);
00272     wait (.01);
00273 }
00274 
00275 void send_error_check()
00276 {
00277     if (plates <= 0 || plates >= 17) 
00278     {
00279         pc.printf("\nERROR: Invalid Range |");
00280     } 
00281     else if(magnetSwitch == 1)
00282     {
00283         pc.printf("\nERROR: Not at Home Position |");
00284     }
00285     else 
00286     {
00287         plate_selection();
00288         send();
00289     }
00290 }
00291 void select_error_check()
00292 {
00293     if (magnetSwitch == 1) 
00294     {
00295         pc.printf("\nERROR: Not at Home Position |");
00296     } 
00297     else 
00298     {
00299         plate_selection();
00300     }
00301 }
00302 void retract_error_check()
00303 {
00304     if (magnetSwitch == 0)
00305     {
00306         pc.printf("\nERROR: Already at Home Position |");   
00307     }
00308     else
00309     {
00310         retract();
00311     }
00312 }
00313 
00314 int main()
00315 {   
00316     in1=1;
00317     in2=1;
00318     en = 1;
00319     wait(0.1);
00320     internalpullups();
00321     wait(0.1);  
00322     char command[15] = {0};
00323     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");
00324     
00325     while(1) 
00326     {
00327         pc.printf("\nRequests:  ");
00328         pc.scanf("%s",&command);
00329     
00330         if (strcmp (command, "s") == 0) 
00331         {   
00332             pc.scanf(" %d",&plates);
00333             pc.printf("\nresponse recieved |");
00334             send_error_check();
00335             beam();
00336             platedetect();
00337         }
00338         else if (strcmp (command, "r") == 0)
00339         {   
00340             pc.printf("\nresponse recieved |");
00341             retract_error_check();
00342         }
00343         else if (strcmp (command, "c") == 0)
00344         {   
00345             pc.scanf(" %d",&plates);
00346             pc.printf("\nresponse recieved |");
00347             select_error_check();
00348         }
00349          else if (strcmp (command, "b") == 0)
00350         {   
00351             pc.printf("\nresponse recieved |");
00352             beam();
00353             platedetect();
00354         }
00355         else
00356         {
00357             pc.printf("\nERROR: Invalid Command");       
00358         }
00359     }
00360 }