update hcsr04.cpp by changing 20000 to 2000
Dependencies: Stepper mbed SDFileSystem Ultrasonic PinDetect millis
Diff: main.cpp
- Revision:
- 7:ede305192e3b
- Parent:
- 6:354264d1e4bb
- Child:
- 8:3f4d78017fd9
--- a/main.cpp Wed Dec 19 19:44:16 2018 +0000 +++ b/main.cpp Thu Jan 10 01:32:38 2019 +0000 @@ -3,141 +3,177 @@ #include "string" #include "hcsr04.h" #include "millis.h" +#include "SDFileSystem.h" + +SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // mosi, miso, sclk, cs, name Serial pc(USBTX, USBRX); -HCSR04 heightsensor(PTA2,PTC2); //(trig,echo) -HCSR04 pathsensor(PTD3,PTD2); +HCSR04 pathsensor(PTC2,PTA2); //(trig,echo) +HCSR04 heightsensor(PTD2,PTD3); DigitalOut in1(PTC12); DigitalOut in2(D7); DigitalIn homeSwitch(D9); +DigitalIn magnetSwitch(D10); Stepper mot(D3,D4); //(D3 to PUL+, D4 to DIR+) DigitalOut en(D2); -volatile int plates=0; +volatile int plates = 0; volatile unsigned int sheets=0; volatile unsigned int path_sheets=0; +volatile int stop_error=0; +volatile int error_distance; +volatile int beam_distance; +volatile int broken_switch; +volatile int stage = 1; + +void switch_check(){ + stage = 1; + pc.printf("\nSwitch Check "); + if (homeSwitch == 1 && magnetSwitch == 1){ + stop_error = 1; + broken_switch = 2; //both switches broken + } + else if (homeSwitch == 0){ + stop_error = 1; + broken_switch = 1; //magnet switch isn't working + } + else{ + broken_switch = 0; + } +} -void plate_selection() -{ +void plate_selection(){ + stage = 2; unsigned int dist; int er = 5; - int fp = 457; //adjust if needed - int lp = 64; //adjust if needed - int tp = 25; + int finp = 447; //sensor height initial + int tp = 25; + wait(1); + in1=1; + in2=0; + wait_ms(900); //stair fix + in1=1; + in2=1; + wait(1); + millisStart(); + long Duration2 = 0; + long StartTime2 = millis(); - while (plates!=sheets) { +while (plates!=sheets && Duration2 <= 55000) { + heightsensor.start(); - wait_ms(500); + wait_ms(250); dist=heightsensor.get_dist_cm(); pc.printf("\nmm:%ld",dist); + long CurrentTime2 = millis(); + Duration2 = CurrentTime2-StartTime2; - if (dist<=lp-10 || dist >= fp+10){ - mot.stop(); - en = 1; - sheets = plates; - pc.printf("\nERROR: Selector is outside of plate range\nRestart/Reset Program"); - } - else if (plates<sheets){ + if ( dist < 30 || dist > 500){ + error_distance = dist; + pc.printf("\n Distance Error: %ld", error_distance); + stop_error = 2; + break; + } + // pc.printf("\nin while loop"); + if (plates<sheets){ en = 0; - mot.setSpeed(600); + mot.setSpeed(500); mot.rotate(1); - } - else{ + } + if (plates>sheets){ en = 0; - mot.setSpeed(600); + mot.setSpeed(500); mot.rotate(0); - } - - if (dist<=fp+er && dist>=fp-er){ //450,440 + } + if (dist<=finp+er && dist>=finp-er){ sheets = 1; - pc.printf("\nSheets selected:%ld",sheets); - } - - if (dist<= fp-tp+er && dist>=fp-tp-er){ //424,414 + } + if (dist<= finp-tp+er && dist>=finp-tp-er){ sheets = 2; - pc.printf("\nSheets selected:%ld",sheets); - } - - if (dist<= fp-2*tp+er && dist>=fp-2*tp-er){ + } + if (dist<= finp-2*tp+er && dist>=finp-2*tp-er){ sheets = 3; - pc.printf("\nSheets selected:%ld",sheets); } - - if (dist<= fp-3*tp+er && dist>=fp-3*tp-er){ + if (dist<= finp-3*tp+er && dist>=finp-3*tp-er){ sheets = 4; - pc.printf("\nSheets selected:%ld",sheets); - } - if (dist<= fp-4*tp+er && dist>=fp-4*tp-er){ - sheets = 5; - pc.printf("\nSheets selected:%ld",sheets); } - if (dist<= fp-5*tp+er && dist>=fp-5*tp-er){ + if (dist<= finp-4*tp+er && dist>=finp-4*tp-er){ + sheets = 5; + + } + if (dist<= finp-5*tp+er && dist>=finp-5*tp-er){ sheets = 6; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-6*tp+er && dist>=fp-6*tp-er){ + if (dist<= finp-6*tp+er && dist>=finp-6*tp-er){ sheets = 7; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-7*tp+er && dist>=fp-7*tp-er){ + if (dist<= finp-7*tp+er && dist>=finp-7*tp-er){ sheets = 8; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-8*tp+er && dist>=fp-8*tp-er){ + if (dist<= finp-8*tp+er && dist>=finp-8*tp-er){ sheets = 9; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-9*tp+er && dist>=fp-9*tp-er){ + if (dist<= finp-9*tp+er && dist>=finp-9*tp-er){ sheets = 10; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-10*tp+er && dist>=fp-10*tp-er){ + if (dist<= finp-10*tp+er && dist>=finp-10*tp-er){ sheets = 11; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-11*tp+er && dist>=fp-11*tp-er){ + if (dist<= finp-11*tp+er && dist>=finp-11*tp-er){ sheets = 12; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-12*tp+er && dist>=fp-12*tp-er){ + if (dist<= finp-12*tp+er && dist>=finp-12*tp-er){ sheets = 13; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-13*tp+er && dist>=fp-13*tp-er){ + if (dist<= finp-13*tp+er && dist>=finp-13*tp-er){ sheets = 14; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-14*tp+er && dist>=fp-14*tp-er){ + if (dist<= finp-14*tp+er && dist>=finp-14*tp-er){ //102 to 112 sheets = 15; - pc.printf("\nSheets selected:%ld",sheets); + } - if (dist<= fp-15*tp+er && dist>=fp-15*tp-er){ // 75, 65 - sheets = 16; - pc.printf("\nSheets selected:%ld",sheets); + if (dist<= 70 && dist>= 60){ //77 to 87 + sheets = 16; } } mot.stop(); en = 1; wait(1); + if (Duration2 > 55000){ + stop_error = 3; + } } void beam() -{ - +{ + stage = 3; unsigned int path_dist; - int err = 11; - int sens_pos = 126; //adjust if needed //535 0 plates + int err = 12; + int sens_pos = 131; int sheet_th = 25; pathsensor.start(); - wait_ms(500); + wait_ms(250); path_dist=pathsensor.get_dist_cm(); - pc.printf("\nBeam Path Sensor : %ld",path_dist); - path_sheets = 17; - - if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){ //110,130 + pc.printf("\nBeam Path Sensor: %ld",path_dist); + + if (path_dist < 110){ + beam_distance = path_dist; + stop_error = 6; + } + + if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){ //119 143 path_sheets = 16; } - if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ //152,168 a: + if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ path_sheets = 15; } @@ -185,142 +221,178 @@ path_sheets = 4; } - if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ //468,478 + if (path_dist >= sens_pos-err+13*sheet_th && path_dist <= sens_pos+err+13*sheet_th){ path_sheets = 3; } - if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ //494,504 + if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ path_sheets = 2; } - if (path_dist >= sens_pos-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){//greater than 509 less 517 + if (path_dist >= sens_pos-10-err+15*sheet_th && path_dist <= sens_pos-10+err+15*sheet_th){ path_sheets = 1; } - if (path_dist > sens_pos-err+16*sheet_th){ //greater than 534 + if (path_dist > sens_pos-err+16*sheet_th){ path_sheets = 0; - } -} - -void platedetect() -{ - if(homeSwitch==0 && path_sheets==0) - { - pc.printf("\nAll plates are in the start position"); - } - else if (homeSwitch==1 && path_sheets==0) - { - pc.printf("\nError-Plates are not fully extended"); - } - else - { - if (plates==path_sheets) - { - pc.printf("\nPlates in beam path:%ld",path_sheets); - } - else if (plates != path_sheets && path_sheets <= 16) - { - pc.printf("\nError: %ld",path_sheets); - pc.printf(" Plates in beam path, but user sent %ld",plates); - pc.printf(" Plates"); - } - else{ - pc.printf("\nERROR: Additional objects detected in beam path"); - } - } + } } void retract() { + stage = 4; millisStart(); long Duration = 0; long StartTime = millis(); - while(homeSwitch == 1 && Duration <= 48000 ) + while(homeSwitch == 1 && magnetSwitch == 1 && Duration <= 55000 ) { in1=0; in2=1; long CurrentTime = millis(); Duration = CurrentTime-StartTime; - pc.printf("Duration = %d\r\n", Duration); } in1=1; in2=1; + if (Duration > 55000){ + stop_error = 5; + } } void send() { in1=1; in2=0; - wait(48); + wait(55); in1=1; in2=1; } + void internalpullups() { homeSwitch.mode(PullUp); + wait(.01); + magnetSwitch.mode(PullUp); wait (.01); } + -void send_error_check() -{ - if (plates <= 0 || plates >= 17) - { - pc.printf("\nERROR: Invalid Range |"); - } - else if(homeSwitch == 1) - { - pc.printf("\nERROR: Not at Home Position |"); - } - else - { - plate_selection(); - send(); - } -} -void select_error_check() +void logwriting() { - if (homeSwitch == 1) - { - pc.printf("\nERROR: Not at Home Position |"); - } - else - { - plate_selection(); - } -} -void retract_error_check() -{ - if (homeSwitch == 0) - { - pc.printf("\nERROR: Already at Home Position |"); - } - else - { - retract(); - } + FILE *fp = fopen("/sd/mydir/sdtest.txt", "a"); + if(fp == NULL) { + pc.printf("\nError Writing to Log "); + } + pc.printf("\nLogging "); + pc.printf("\nStop Error: %ld ", stop_error); + + switch (stop_error){ + case 1: + fprintf(fp,"\nRequested: %ld ",plates); + fprintf(fp,"\nBroken Switch: %ld ", broken_switch); + fprintf(fp,"\nStop Error: %ld ", stop_error); + fclose(fp); + exit(1); + break; + case 2: + fprintf(fp,"\nRequested: %ld ",plates); + fprintf(fp,"\nSelector Sensor Distance : %ld ", error_distance); + fprintf(fp,"\nStop Error: %ld ", stop_error); + fclose(fp); + exit(1); + break; + case 3: + fprintf(fp,"\nRequested: %ld ",plates); + fprintf(fp,"\nSelector Timeout"); + fprintf(fp,"\nStop Error: %ld ", stop_error); + fclose(fp); + exit(1); + break; + case 4: + fprintf(fp,"\nRequested: %ld ",plates); + fprintf(fp,"\nSelected: %ld ", sheets); + fprintf(fp,"\nDetected: %ld ", path_sheets); + fprintf(fp,"\nStop Error: %ld ", stop_error); + fclose(fp); + break; + case 5: + fprintf(fp,"\nRequested: %ld ",plates); + fprintf(fp,"\nSelected: %ld ", sheets); + fprintf(fp,"\nDetected: %ld ", path_sheets); + fprintf(fp,"\nStop Error: %ld ", stop_error); + fclose(fp); + exit(1); + break; + case 6: + fprintf(fp,"\nRequested: %ld ",plates); + fprintf(fp,"\nSelected: %ld ", sheets); + fprintf(fp,"\nBeam Sensor Distance: %ld ", beam_distance); + fprintf(fp,"\nStop Error: %ld ", stop_error); + fclose(fp); + exit(1); + break; + case 7: + fprintf(fp,"\nRequested: %ld ",plates); + fprintf(fp,"\nNot at Start Position "); + fprintf(fp,"\nStop Error: %ld ", stop_error); + fclose(fp); + exit(1); + default: + pc.printf("\nstage: %ld ", stage); + if (stage == 1){ + pc.printf("\nRequested: %ld ", plates); + fprintf(fp,"\nRequested: %ld ",plates); + fclose(fp); + } + else if (stage == 2) { + pc.printf("\nSelected: %ld ", sheets); + fprintf(fp,"\nSelected: %ld ", sheets); + fclose(fp); + } + else if (stage == 3){ + pc.printf("\nDetected: %ld ", path_sheets); + fprintf(fp,"\nDetected: %ld ", path_sheets); + fclose(fp); + } + else{ + pc.printf("\nPass "); + fprintf(fp,"\nPass "); + fclose(fp); + } + break; + } } int main() { + mkdir("/sd/mydir", 0777); in1=1; in2=1; en = 1; wait(0.1); internalpullups(); - wait(0.1); - - while(1) + while(stop_error==0) { - plates = (rand() % 16) + 1; - wait(2); - pc.printf("\nresponse recieved |"); - send_error_check(); - beam(); - platedetect(); - wait(2); - retract_error_check(); - wait(2); - } - } \ No newline at end of file + wait(1); + switch_check(); //stage 1 + wait(1); + logwriting(); // plates requestd + wait(1); + plate_selection(); //stage 2 + wait(1); + logwriting(); //paltes selected + wait(1); + send(); + wait(1); + beam(); //stage 3 + wait(1); + logwriting(); //plates detected + wait(1); + retract(); //stage 4 + wait(1); + logwriting(); //nothing + } + pc.printf("\nError was detected. Stopping Program"); +} +