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 SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // mosi, miso, sclk, cs, name
00009 Serial pc(USBTX, USBRX);
00010 HCSR04 pathsensor(PTC2,PTA2); //(trig,echo)
00011 HCSR04 heightsensor(PTD2,PTD3);
00012 DigitalOut in1(PTC12);
00013 DigitalOut in2(D7);
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 volatile int stop_error=0;
00023 volatile int error_distance;
00024 volatile int beam_distance;
00025 volatile int broken_switch;
00026 volatile int stage = 1;
00027  
00028 void switch_check(){
00029     stage = 1;
00030     pc.printf("\nSwitch Check ");
00031         if (homeSwitch == 1 && magnetSwitch == 1){
00032             stop_error = 1;
00033             broken_switch = 2; //both switches broken
00034             }
00035         else if (homeSwitch == 0){
00036             stop_error = 1;
00037             broken_switch = 1; //magnet switch isn't working
00038             }
00039         else{
00040             broken_switch = 0;
00041             stop_error = 0;
00042             }
00043 }
00044 
00045 void plate_selection(){
00046     stage = 2;
00047     unsigned int dist;
00048     int er = 5;
00049     int finp = 447;   //sensor height initial
00050     int tp = 25; 
00051     wait(1);
00052     in1=1;
00053     in2=0;
00054     wait_ms(900); //stair fix
00055     in1=1;
00056     in2=1;
00057     wait(1);
00058     millisStart();
00059     long Duration2 = 0;
00060     long StartTime2 = millis();
00061     
00062 while (plates!=sheets && Duration2 <= 55000) {
00063         
00064         heightsensor.start();
00065         wait_ms(250);
00066         dist=heightsensor.get_dist_cm(); 
00067         pc.printf("\nmm:%ld",dist);
00068         long CurrentTime2 = millis();
00069         Duration2 = CurrentTime2-StartTime2;
00070         
00071         if ( dist < 30 || dist > 500){ 
00072             error_distance = dist;
00073             pc.printf("\n Distance Error: %ld", error_distance);
00074             stop_error = 2;
00075             break;
00076             } 
00077            // pc.printf("\nin while loop");
00078         if (plates<sheets){
00079             en = 0;
00080             mot.setSpeed(500);
00081             mot.rotate(1);
00082             }
00083         if (plates>sheets){
00084             en = 0;
00085             mot.setSpeed(500);
00086             mot.rotate(0);
00087             }
00088         if (dist<=finp+er && dist>=finp-er){                 
00089             sheets = 1;
00090             }
00091         if (dist<= finp-tp+er && dist>=finp-tp-er){    
00092             sheets = 2;
00093             }
00094         if (dist<= finp-2*tp+er && dist>=finp-2*tp-er){     
00095             sheets = 3;
00096         }
00097         if (dist<= finp-3*tp+er && dist>=finp-3*tp-er){         
00098             sheets = 4;
00099         }
00100         if (dist<= finp-4*tp+er && dist>=finp-4*tp-er){         
00101             sheets = 5;
00102              
00103              }
00104          if (dist<= finp-5*tp+er && dist>=finp-5*tp-er){       
00105             sheets = 6;
00106              
00107         }
00108          if (dist<= finp-6*tp+er && dist>=finp-6*tp-er){     
00109             sheets = 7;
00110              
00111         }
00112          if (dist<= finp-7*tp+er && dist>=finp-7*tp-er){         
00113             sheets = 8;
00114              
00115         }
00116          if (dist<= finp-8*tp+er && dist>=finp-8*tp-er){       
00117             sheets = 9;
00118              
00119         }
00120          if (dist<= finp-9*tp+er && dist>=finp-9*tp-er){         
00121             sheets = 10;
00122              
00123         }
00124          if (dist<= finp-10*tp+er && dist>=finp-10*tp-er){         
00125             sheets = 11;
00126              
00127         }
00128          if (dist<= finp-11*tp+er && dist>=finp-11*tp-er){        
00129             sheets = 12;
00130              
00131         }
00132          if (dist<= finp-12*tp+er && dist>=finp-12*tp-er){         
00133             sheets = 13;
00134              
00135         }
00136          if (dist<= finp-13*tp+er && dist>=finp-13*tp-er){        
00137             sheets = 14;
00138              
00139         }
00140          if (dist<= finp-14*tp+er && dist>=finp-14*tp-er){     //102 to 112 
00141             sheets = 15;
00142              
00143         }
00144          if (dist<= 70 && dist>= 60){        //77 to 87
00145             sheets = 16;    
00146         }
00147     }
00148     mot.stop();
00149     en = 1;
00150     wait(1);
00151     if (Duration2 > 55000){
00152     stop_error = 3;    
00153     }
00154 }
00155 
00156 void beam()
00157 { 
00158     stage = 3;
00159     unsigned int path_dist;
00160     int err = 12;
00161     int sens_pos = 131; 
00162     int sheet_th = 25; 
00163     pathsensor.start();
00164     wait_ms(250);
00165     path_dist=pathsensor.get_dist_cm();
00166     pc.printf("\nBeam Path Sensor: %ld",path_dist);
00167            
00168             if (path_dist < 110){
00169                beam_distance = path_dist;
00170                stop_error = 6;          
00171             }
00172 
00173             else if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){     //119  143      
00174                path_sheets = 16;
00175                 
00176             }
00177             else if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){
00178                path_sheets = 15;
00179                 
00180             }
00181             else if (path_dist >= sens_pos-err+2*sheet_th && path_dist <= sens_pos+err+2*sheet_th){ //179-191
00182                path_sheets = 14;
00183                 
00184             }
00185             else if (path_dist >= sens_pos-err+3*sheet_th && path_dist <= sens_pos+err+3*sheet_th){ //205-217
00186                path_sheets = 13;
00187                  
00188             }
00189             else if (path_dist >= sens_pos-err+4*sheet_th && path_dist <= sens_pos+err+4*sheet_th){ //231-243
00190                path_sheets = 12;
00191                 
00192             }
00193             else if (path_dist >= sens_pos-err+5*sheet_th && path_dist <= sens_pos+err+5*sheet_th){ //257-269
00194                path_sheets = 11;
00195                 
00196             }
00197             else if (path_dist >= sens_pos-err+6*sheet_th && path_dist <= sens_pos+err+6*sheet_th){ //283-295
00198                path_sheets = 10;
00199                 
00200             }
00201             else if (path_dist >= sens_pos-err+7*sheet_th && path_dist <= sens_pos+err+7*sheet_th){ //309-321
00202                path_sheets = 9;
00203                 
00204             }
00205             else if (path_dist >= sens_pos-err+8*sheet_th && path_dist <= sens_pos+err+8*sheet_th){ //323-333
00206                path_sheets = 8;
00207                 
00208             }
00209             else if (path_dist >= sens_pos-err+9*sheet_th && path_dist <= sens_pos+err+9*sheet_th){ //349-359
00210                path_sheets = 7;
00211                 
00212             }
00213             else if (path_dist >= sens_pos-err+10*sheet_th && path_dist <= sens_pos+err+10*sheet_th){
00214                path_sheets = 6;
00215                 
00216             }
00217             else if (path_dist >= sens_pos-err+11*sheet_th && path_dist <= sens_pos+err+11*sheet_th){
00218                path_sheets = 5;
00219                 
00220             }
00221            else if (path_dist >= sens_pos-err+12*sheet_th && path_dist <= sens_pos+err+12*sheet_th){
00222                path_sheets = 4;
00223                 
00224             }
00225             else if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ 
00226                path_sheets = 3;
00227                 
00228             }
00229             else if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ 
00230                path_sheets = 2;
00231                 
00232             }
00233             else if (path_dist >= sens_pos-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){
00234                path_sheets = 1;
00235                 
00236             }
00237             else if (path_dist > sens_pos-err+16*sheet_th){  
00238                path_sheets = 0;
00239             }   
00240             else {
00241                 path_sheets = 17;
00242                 }
00243 }
00244 
00245 void retract()
00246 {
00247     stage = 4;
00248     millisStart();
00249     long Duration = 0;
00250     long StartTime = millis();
00251     
00252     while(homeSwitch == 1 && magnetSwitch == 1 && Duration <= 60000 )
00253     {
00254     in1=0;
00255     in2=1;
00256     long CurrentTime = millis();
00257     Duration = CurrentTime-StartTime;
00258     }
00259     in1=1;
00260     in2=1;
00261     if (Duration > 60000){
00262     stop_error = 5; 
00263     }
00264     else{
00265     stop_error = 0;
00266     }
00267 }
00268 
00269 void send()                 
00270 { 
00271     in1=1;
00272     in2=0;
00273     wait(58);
00274     in1=1;
00275     in2=1;
00276 }
00277 
00278 
00279 void logwriting()
00280 {
00281    FILE *fp = fopen("/sd/mydir/sdtest.txt", "a");
00282     if(fp == NULL) {
00283         pc.printf("\nError Writing to Log ");
00284         }    
00285     pc.printf("\nLogging ");
00286     pc.printf("\nStop Error: %ld ", stop_error);
00287     
00288     switch (stop_error){
00289         case 1:
00290         fprintf(fp,"\nRequested: %ld ",plates);
00291         fprintf(fp,"\nBroken Switch: %ld ", broken_switch);
00292         fprintf(fp,"\nStop Error: %ld ", stop_error);
00293         fclose(fp);
00294         exit(1);
00295         break;
00296         case 2:
00297         fprintf(fp,"\nRequested: %ld ",plates);
00298         fprintf(fp,"\nSelector Sensor Distance : %ld ", error_distance);
00299         fprintf(fp,"\nStop Error: %ld ", stop_error);
00300         fclose(fp);
00301         exit(1);
00302         break;
00303         case 3:
00304         fprintf(fp,"\nRequested: %ld ",plates);
00305         fprintf(fp,"\nSelector Timeout");
00306         fprintf(fp,"\nStop Error: %ld ", stop_error);
00307         fclose(fp);
00308         exit(1);
00309         break;
00310         case 4:
00311         fprintf(fp,"\nRequested: %ld ", plates);
00312         fprintf(fp,"\nSelected: %ld ", sheets);
00313         fprintf(fp,"\nDetected: %ld ", path_sheets);
00314         fprintf(fp,"\nStop Error: %ld ", stop_error);
00315         fclose(fp);
00316         break;
00317         case 5:
00318         fprintf(fp,"\nRequested: %ld ", plates);
00319         fprintf(fp,"\nSelected: %ld ", sheets);
00320         fprintf(fp,"\nDetected: %ld ", path_sheets);
00321         fprintf(fp,"\nStop Error: %ld ", stop_error);
00322         fclose(fp);
00323         exit(1);
00324         break;
00325         case 6:
00326         fprintf(fp,"\nRequested: %ld ", plates);
00327         fprintf(fp,"\nSelected: %ld ", sheets);
00328         fprintf(fp,"\nBeam Sensor Distance: %ld ", beam_distance);
00329         fprintf(fp,"\nStop Error: %ld ", stop_error);
00330         fclose(fp);
00331         exit(1);
00332         break;
00333         case 7:
00334         fprintf(fp,"\nRequested: %ld ", plates);
00335         fprintf(fp,"\nNot at Start Position ");
00336         fprintf(fp,"\nStop Error: %ld ", stop_error);
00337         fclose(fp);
00338         exit(1);
00339         default: 
00340         pc.printf("\nstage: %ld ", stage);
00341             if (stage == 1){
00342                 pc.printf("\nRequested: %ld ", plates);
00343                 fprintf(fp,"\nRequested: %ld ",plates);
00344                 fclose(fp);
00345                 }
00346             else if (stage == 2) {
00347                 pc.printf("\nSelected: %ld ", sheets);
00348                 fprintf(fp,"\nSelected: %ld ", sheets);
00349                 fclose(fp);
00350                 }
00351             else if (stage == 3){
00352                 pc.printf("\nDetected: %ld ", path_sheets);
00353                 fprintf(fp,"\nDetected: %ld ", path_sheets);
00354                 fclose(fp);
00355                 }
00356             else{
00357                 pc.printf("\nPass ");
00358                 fprintf(fp,"\nPass ");
00359                 fclose(fp);
00360                 }     
00361         break;
00362   }
00363 }
00364 
00365 int main()
00366 {   
00367     homeSwitch.mode(PullUp);
00368     wait(.01);
00369     magnetSwitch.mode(PullUp);
00370     wait (.01);
00371     mkdir("/sd/mydir", 0777);
00372     in1=1;
00373     in2=1;
00374     en = 1;
00375     wait(0.1);
00376    
00377     while(stop_error==0) 
00378     {
00379             plates = (rand() % 16) + 1;
00380             wait(1);
00381             switch_check();     //stage 1
00382             wait(1);
00383             logwriting();       // plates requestd
00384             wait(1);
00385             plate_selection();  //stage 2
00386             wait(1);
00387             logwriting();       //paltes selected
00388             wait(1);
00389             send();
00390             wait(1);
00391             beam();             //stage 3
00392             wait(1);
00393             logwriting();       //plates detected
00394             wait(1);
00395             retract();          //stage 4
00396             wait(1);
00397             logwriting();       //nothing     
00398     }   
00399     pc.printf("\nError was detected. Stopping Program");
00400 }
00401