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