Diff: main.cpp

Revision:

3:fd2ca631ab44

Parent:

2:e8f8095464cf

Child:

4:1a4022b21dae

--- 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