for cycle testing
Dependencies: HCSR04 PinDetect Stepper
Diff: main.cpp
- Revision:
- 0:5848e605bae5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Oct 13 02:26:02 2018 +0000 @@ -0,0 +1,285 @@ +#include "mbed.h" +#include "Stepper.h" +#include "string" +#include "hcsr04.h" +Serial pc(USBTX, USBRX); +HCSR04 heightsensor(PTA2,PTC2); //(trig,echo) +HCSR04 pathsensor(PTD3,PTD2); +DigitalOut in1(PTC12); +DigitalOut in2(D7); +DigitalIn homeSwitch(D9); +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; + +void plate_selection() +{ + unsigned int dist; + int er = 5; + int fp = 470; //adjust if needed + int lp = 75; //adjust if needed + int tp = 26; + + while (plates!=sheets) { + heightsensor.start(); + wait_ms(500); + dist=heightsensor.get_dist_cm(); + pc.printf("\nmm:%ld",dist); + + 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){ + en = 0; + mot.setSpeed(600); + mot.rotate(1); + } + else{ + en = 0; + mot.setSpeed(600); + mot.rotate(0); + } + + if (dist<=fp+er && dist>=fp-er){ + sheets = 1; + pc.printf("\nSheets selected:%ld",sheets); + } + + if (dist<= fp-tp-er && dist>=fp-tp-er){ + sheets = 2; + pc.printf("\nSheets selected:%ld",sheets); + } + + if (dist<= fp-2*tp-er && dist>=fp-2*tp-er){ + sheets = 3; + pc.printf("\nSheets selected:%ld",sheets); + } + + if (dist<= fp-3*tp-er && dist>=fp-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){ + sheets = 6; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-6*tp-er && dist>=fp-6*tp-er){ + sheets = 7; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-7*tp-er && dist>=fp-7*tp-er){ + sheets = 8; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-8*tp-er && dist>=fp-8*tp-er){ + sheets = 9; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-9*tp-er && dist>=fp-9*tp-er){ + sheets = 10; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-10*tp-er && dist>=fp-10*tp-er){ + sheets = 11; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-11*tp-er && dist>=fp-11*tp-er){ + sheets = 12; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-12*tp-er && dist>=fp-12*tp-er){ + sheets = 13; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-13*tp-er && dist>=fp-13*tp-er){ + sheets = 14; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-14*tp-er && dist>=fp-14*tp-er){ + sheets = 15; + pc.printf("\nSheets selected:%ld",sheets); + } + if (dist<= fp-15*tp-er && dist>=fp-15*tp-er){ + sheets = 16; + pc.printf("\nSheets selected:%ld",sheets); + } + } + mot.stop(); + en = 1; + wait(1); +} + +void beam() +{ + + unsigned int path_dist; + int err = 5; + int sens_pos = 133; //adjust if needed + int sheet_th = 26; + pathsensor.start(); + wait_ms(500); + path_dist=pathsensor.get_dist_cm(); + pc.printf("\nmm:%ld",path_dist); + path_sheets = 17; + + if (path_dist >= sens_pos-err && path_dist <= sens_pos+err){ //127-139 + path_sheets = 16; + + } + if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ // 153-165 + 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){ //465-477 + 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-err+15*sheet_th && path_dist <= sens_pos+err+15*sheet_th){ + path_sheets = 1; + + } + 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() +{ + while(homeSwitch == 1) + { + in1=0; + in2=1; + } + in1=1; + in2=1; +} + +void send() +{ + in1=1; + in2=0; + wait(17); + in1=1; + in2=1; +} +void internalpullups() +{ + homeSwitch.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(); + } +} + +int main() { + + while(1){ + plates = (rand() % 16) + 1; + pc.printf("\nSending: ",plates); + send_error_check(); + beam(); + platedetect(); + wait(2); + retract(); + } + + +} \ No newline at end of file