update
Dependencies: Stepper mbed SDFileSystem Ultrasonic PinDetect millis
main.cpp
- Committer:
- rschimpf78
- Date:
- 2019-01-10
- Revision:
- 7:ede305192e3b
- Parent:
- 6:354264d1e4bb
- Child:
- 8:3f4d78017fd9
File content as of revision 7:ede305192e3b:
#include "mbed.h" #include "Stepper.h" #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 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 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(){ stage = 2; unsigned int dist; int er = 5; 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 && Duration2 <= 55000) { heightsensor.start(); wait_ms(250); dist=heightsensor.get_dist_cm(); pc.printf("\nmm:%ld",dist); long CurrentTime2 = millis(); Duration2 = CurrentTime2-StartTime2; 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(500); mot.rotate(1); } if (plates>sheets){ en = 0; mot.setSpeed(500); mot.rotate(0); } if (dist<=finp+er && dist>=finp-er){ sheets = 1; } if (dist<= finp-tp+er && dist>=finp-tp-er){ sheets = 2; } if (dist<= finp-2*tp+er && dist>=finp-2*tp-er){ sheets = 3; } if (dist<= finp-3*tp+er && dist>=finp-3*tp-er){ sheets = 4; } 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; } if (dist<= finp-6*tp+er && dist>=finp-6*tp-er){ sheets = 7; } if (dist<= finp-7*tp+er && dist>=finp-7*tp-er){ sheets = 8; } if (dist<= finp-8*tp+er && dist>=finp-8*tp-er){ sheets = 9; } if (dist<= finp-9*tp+er && dist>=finp-9*tp-er){ sheets = 10; } if (dist<= finp-10*tp+er && dist>=finp-10*tp-er){ sheets = 11; } if (dist<= finp-11*tp+er && dist>=finp-11*tp-er){ sheets = 12; } if (dist<= finp-12*tp+er && dist>=finp-12*tp-er){ sheets = 13; } if (dist<= finp-13*tp+er && dist>=finp-13*tp-er){ sheets = 14; } if (dist<= finp-14*tp+er && dist>=finp-14*tp-er){ //102 to 112 sheets = 15; } 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 = 12; int sens_pos = 131; int sheet_th = 25; pathsensor.start(); wait_ms(250); path_dist=pathsensor.get_dist_cm(); 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){ path_sheets = 15; } if (path_dist >= sens_pos-err+2*sheet_th && path_dist <= sens_pos+err+2*sheet_th){ //179-191 path_sheets = 14; } if (path_dist >= sens_pos-err+3*sheet_th && path_dist <= sens_pos+err+3*sheet_th){ //205-217 path_sheets = 13; } if (path_dist >= sens_pos-err+4*sheet_th && path_dist <= sens_pos+err+4*sheet_th){ //231-243 path_sheets = 12; } if (path_dist >= sens_pos-err+5*sheet_th && path_dist <= sens_pos+err+5*sheet_th){ //257-269 path_sheets = 11; } if (path_dist >= sens_pos-err+6*sheet_th && path_dist <= sens_pos+err+6*sheet_th){ //283-295 path_sheets = 10; } if (path_dist >= sens_pos-err+7*sheet_th && path_dist <= sens_pos+err+7*sheet_th){ //309-321 path_sheets = 9; } if (path_dist >= sens_pos-err+8*sheet_th && path_dist <= sens_pos+err+8*sheet_th){ //323-333 path_sheets = 8; } if (path_dist >= sens_pos-err+9*sheet_th && path_dist <= sens_pos+err+9*sheet_th){ //349-359 path_sheets = 7; } if (path_dist >= sens_pos-err+10*sheet_th && path_dist <= sens_pos+err+10*sheet_th){ path_sheets = 6; } if (path_dist >= sens_pos-err+11*sheet_th && path_dist <= sens_pos+err+11*sheet_th){ path_sheets = 5; } if (path_dist >= sens_pos-err+12*sheet_th && path_dist <= sens_pos+err+12*sheet_th){ path_sheets = 4; } 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){ path_sheets = 2; } 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){ path_sheets = 0; } } void retract() { stage = 4; millisStart(); long Duration = 0; long StartTime = millis(); while(homeSwitch == 1 && magnetSwitch == 1 && Duration <= 55000 ) { in1=0; in2=1; long CurrentTime = millis(); Duration = CurrentTime-StartTime; } in1=1; in2=1; if (Duration > 55000){ stop_error = 5; } } void send() { in1=1; in2=0; wait(55); in1=1; in2=1; } void internalpullups() { homeSwitch.mode(PullUp); wait(.01); magnetSwitch.mode(PullUp); wait (.01); } void logwriting() { 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(); while(stop_error==0) { plates = (rand() % 16) + 1; 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"); }