![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
update
Dependencies: Stepper mbed SDFileSystem Ultrasonic PinDetect millis
Diff: main.cpp
- Revision:
- 3:fd2ca631ab44
- Parent:
- 2:e8f8095464cf
- Child:
- 4:63f0989c8d1e
--- a/main.cpp Fri Sep 14 19:10:45 2018 +0000 +++ b/main.cpp Sat Oct 13 02:19:16 2018 +0000 @@ -1,134 +1,297 @@ #include "mbed.h" #include "Stepper.h" #include "string" -DigitalOut in1(D10);///in1 d9 and in2 d10 -DigitalOut in2(D9); -DigitalIn endSwitch(D6); -DigitalIn homeSwitch(D7); -DigitalIn bottomSwitch(D5); -Stepper mot(D3,D4); //(D5 to PUL+, D4 to DIR+) +#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); -Serial pc(USBTX, USBRX); +volatile int plates=0; +volatile unsigned int sheets=0; +volatile unsigned int path_sheets=0; -volatile int plates = 1; - -void position() +void plate_selection() { - char str1[20]; - char str2[20]; - char str3[20]; - strcpy(str1,"HOME"); - strcpy(str2,"ERROR"); - strcpy(str3,"IN BEAM PATH"); + unsigned int dist; + int er = 5; + int fp = 448; //adjust if needed + int lp = 64; //adjust if needed + int tp = 25; - if(homeSwitch==0 && endSwitch==1) + 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){ //450,440 + sheets = 1; + pc.printf("\nSheets selected:%ld",sheets); + } + + if (dist<= fp-tp+er && dist>=fp-tp-er){ //424,414 + 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){ // 75, 65 + sheets = 16; + pc.printf("\nSheets selected:%ld",sheets); + } + } + mot.stop(); + en = 1; + wait(1); +} + +void beam() +{ + + unsigned int path_dist; + int err = 8; + int sens_pos = 148; //adjust if needed //535 0 plates + int sheet_th = 25; + pathsensor.start(); + wait_ms(500); + 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){ //127,143 a:152 + path_sheets = 16; + + } + if (path_dist >= sens_pos-err+sheet_th && path_dist <= sens_pos+err+sheet_th){ //152,168 a: + 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){ //468,478 + path_sheets = 3; + + } + if (path_dist >= sens_pos-err+14*sheet_th && path_dist <= sens_pos+err+14*sheet_th){ //494,504 + 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(" pmma state - %s\n",str1); + pc.printf("\nAll plates are in the start position"); } - else if (homeSwitch==1 && endSwitch==0) + else if (homeSwitch==1 && path_sheets==0) { - pc.printf(" pmma state - %s\n",str3); - } - else if (homeSwitch==1 && endSwitch==1) - { - pc.printf(" pmma state - %s\n",str2); + pc.printf("\nError-Plates are not fully extended"); } else { - pc.printf(" pmma state - else"); - } -} -void retractPMMA() + 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=1; - in2=0; - } - in1=1; - in2=1; -} -void sendPMMA() //Command: Send plates -{ - in1=0; in2=1; - wait(9); + } in1=1; in2=1; } -void autoCalibrate() //if machine is plugged in when the plate selector isn't at the origin... -{ - while(homeSwitch==1) - { - retractPMMA(); - } - +void send() +{ + in1=1; + in2=0; + wait(17); in1=1; in2=1; - en = 0; //...it automatically lowers it to the origin position - mot.setSpeed(600); - mot.rotate(1); - while(bottomSwitch); //While bottom switch is unpressed - mot.stop(); //Stop rotation when switch is pressed (pressed = logical level low) //Disable driver - mot.setPositionZero(); - wait(1); - mot.goesTo(-800); - while(!mot.stopped()); - mot.stop(); - mot.setPositionZero(); - en=1; //Set absolute origin for plate selector at switch hit } + void internalpullups() { - bottomSwitch.mode(PullUp); homeSwitch.mode(PullUp); - endSwitch.mode(PullUp); wait (.01); } -void numberofPlates() // Command: Number of PMMA to send (plates) -{ - int stepheight; - - if (plates <= 0 || plates >= 17) + +void send_error_check() +{ + if (plates <= 0 || plates >= 17) { - pc.printf(" ERROR invalid range |"); + pc.printf("\nERROR: Invalid Range |"); } - else if(endSwitch == 1 && homeSwitch==0) - { - en = 0; - mot.goesTo((plates-1)*-1680); //plates per step - while(!mot.stopped()); - stepheight = mot.getPosition(); - pc.printf(" selector height %d steps |",stepheight); - mot.stop(); - en = 1; - wait(1); - sendPMMA(); + else if(homeSwitch == 1) + { + pc.printf("\nERROR: Not at Home Position |"); + } + else + { + plate_selection(); + send(); + } +} +void select_error_check() +{ + 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 { - pc.printf("Position Error - Cannot Send"); + retract(); } } -void selectionStatus() -{ - int stepperposition; - double selectedplates; - stepperposition = mot.getPosition(); - selectedplates = stepperposition/1621; - pc.printf("\n Number of plates currently selected: %d ",selectedplates); -} -void deviceinfo() -{ - printf("\nUnique ID: 0240000041114e4500513007bcf9000a9e51000097969900"); - printf("\nBootloader Version: 0244"); - printf("\nBUILD: 1:4BDA04E+"); - printf("\nBUILD TIMESTAMP=3/18/2018\n\n"); - -} int main() { @@ -137,50 +300,43 @@ en = 1; wait(0.1); internalpullups(); - wait(0.1); - autoCalibrate(); - + wait(0.1); char command[15] = {0}; - pc.printf("\nCommands:\n\n\tsend [N]\t**places N PMMA sheets in beam path\n\tretract\t\t**removes all PMMA from beam path\n\tlocation\t**returns location of PMMA\n\tinfo\t\t**returns device and firmware information\n\n"); + pc.printf("\nCommands:\n\n\ts [N]\t**moves N PMMA sheets in beam path\n\tr\t\t**removes all PMMA from beam path\n\tc [N]\t**raises bar to \n\tb\t\t**reports number of plates in beam path\n\n"); while(1) { - pc.printf("\nRequests: "); pc.scanf("%s",&command); - if (strcmp (command, "send") == 0) + if (strcmp (command, "s") == 0) { pc.scanf(" %d",&plates); pc.printf("\nresponse recieved |"); - numberofPlates(); - position(); + send_error_check(); + beam(); + platedetect(); } - else if (strcmp (command, "retract") == 0) + else if (strcmp (command, "r") == 0) { pc.printf("\nresponse recieved |"); - retractPMMA(); - position(); - } - else if (strcmp (command, "location") == 0) - { - pc.printf("\nresponse recieved |"); - position(); + retract_error_check(); } - else if (strcmp (command, "info") == 0) - { + else if (strcmp (command, "c") == 0) + { + pc.scanf(" %d",&plates); pc.printf("\nresponse recieved |"); - deviceinfo(); + select_error_check(); } - else if (strcmp (command, "height") == 0) - { + else if (strcmp (command, "b") == 0) + { pc.printf("\nresponse recieved |"); - numberofPlates(); + beam(); + platedetect(); } - else { - pc.printf("\nERROR-command invalid"); + pc.printf("\nERROR: Invalid Command"); } } } \ No newline at end of file