![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
update
Dependencies: Stepper mbed SDFileSystem Ultrasonic PinDetect millis
main.cpp
- Committer:
- rschimpf78
- Date:
- 2018-10-13
- Revision:
- 3:fd2ca631ab44
- Parent:
- 2:e8f8095464cf
- Child:
- 4:63f0989c8d1e
File content as of revision 3:fd2ca631ab44:
#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 = 448; //adjust if needed int lp = 64; //adjust if needed int tp = 25; 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("\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(); } } 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 { retract(); } } int main() { in1=1; in2=1; en = 1; wait(0.1); internalpullups(); wait(0.1); char command[15] = {0}; 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, "s") == 0) { pc.scanf(" %d",&plates); pc.printf("\nresponse recieved |"); send_error_check(); beam(); platedetect(); } else if (strcmp (command, "r") == 0) { pc.printf("\nresponse recieved |"); retract_error_check(); } else if (strcmp (command, "c") == 0) { pc.scanf(" %d",&plates); pc.printf("\nresponse recieved |"); select_error_check(); } else if (strcmp (command, "b") == 0) { pc.printf("\nresponse recieved |"); beam(); platedetect(); } else { pc.printf("\nERROR: Invalid Command"); } } }