GE Attenuator
update
Dependencies: Stepper mbed SDFileSystem Ultrasonic PinDetect millis
Diff: main.cpp
- Revision:
- 7:ede305192e3b
- Parent:
- 6:354264d1e4bb
- Child:
- 8:3f4d78017fd9
diff -r 354264d1e4bb -r ede305192e3b main.cpp
--- 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");
+}
+