Diff: calibrateFunc.cpp

Revision:

5:dfea493f7a12

Parent:

4:f1670eec4681

Child:

6:a652deaae134

diff -r f1670eec4681 -r dfea493f7a12 calibrateFunc.cpp
--- a/calibrateFunc.cpp	Wed Mar 07 19:27:13 2018 +0000
+++ b/calibrateFunc.cpp	Sun Mar 11 13:15:34 2018 +0000
@@ -1,5 +1,5 @@
-#include "mbed.h"
-#include "funcdef.h"
+#include <mbed.h>
+#include <funcdef.h>
 
 void CalibrateFunc()
 {
@@ -13,14 +13,13 @@
         sensorMaxValue1[i]=sensorValue[i];
     }
     
-    //LEFT WHEEL STARTS TURN  
-    leftMotorVal.write(0.00125*100);
-    rightMotorVal.write(0.00125*0);  
+    //LEFT WHEEL STARTS TURN      
+    leftMotorVal.pulsewidth_us(300); 
+    rightMotorVal.pulsewidth_us(0);  
     
     //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET AND A SMIDGE MORE
-    /////////////////////////////////////////////  
-      
-    int doneYet=0;  
+    /////////////////////////////////////////////       
+    int doneYet[] = {0,0,0,0,0,0,0,0};
     while(1)
     {    
         for (int i=0;i<=7;i++)
@@ -30,23 +29,31 @@
             {    
                 sensorMaxValue1[i]=sensorValue[i];      
             }
-            else if ((sensorValue[i]+70)<=sensorMaxValue1[i]) 
+            else if ((sensorValue[i]+250)<=sensorMaxValue1[i]) 
             {
-                doneYet++;
-                if (doneYet==8)
+                doneYet[i]=1;                
+                if (((doneYet[6]) == 1) && ((doneYet[4]) == 1) && ((doneYet[2]) == 1) && ((doneYet[0]) == 1) && ((doneYet[1]) == 1) && ((doneYet[3]) == 1) && ((doneYet[5]) == 1) && ((doneYet[7]) == 1))
                 {
-                  goto endOfScan1;
+                    goto endOfScan1;
                 }
             }
         }            
     }
     endOfScan1:
-    wait(0.1);
+    wait_ms(50);
     /////////////////////////////////////////////
   
     //STOP WHEELS
-    leftMotorVal.write(0.00125*0);
-    rightMotorVal.write(0.00125*0);
+    leftMotorVal.pulsewidth_us(0); 
+    rightMotorVal.pulsewidth_us(0); 
+    
+    //SLOW BEEPER TO 4HZ
+    counter=0;    
+    delayBetweenPulses_ms=250;    
+    //beeper.attach(&beep, 0.01);
+    LEDVal=0; 
+    
+    wait_ms(500);
       
     //TAKE MIN/MAX VALUES 2ND TIME
     for (int i=0;i<=7;i++)
@@ -57,11 +64,11 @@
     }
     
     //RIGHT WHEEL STARTS TURN  
-    leftMotorVal.write(0.00125*0);
-    rightMotorVal.write(0.00125*100); 
+    leftMotorVal.pulsewidth_us(0); 
+    rightMotorVal.pulsewidth_us(300);  
   
     //TURN UNTIL MIN AND MAX OF ALL SENSORS ARE SET IN NEW VARIABLES
-    doneYet=0;  
+    int doneYet2[] = {0,0,0,0,0,0,0,0};
     while(1)
     {    
         for (int i=0;i<=7;i++)
@@ -71,10 +78,10 @@
             {    
                 sensorMaxValue2[i]=sensorValue[i];      
             }
-            else if ((sensorValue[i]+70)<=sensorMaxValue2[i]) 
-            {
-                doneYet++;
-                if (doneYet==8)
+            else if ((sensorValue[i]+250)<=sensorMaxValue2[i]) 
+            {   
+                doneYet2[i]=1;                
+                if (((doneYet2[6]) == 1) && ((doneYet2[4]) == 1) && ((doneYet2[2]) == 1) && ((doneYet2[0]) == 1) && ((doneYet2[1]) == 1) && ((doneYet2[3]) == 1) && ((doneYet2[5]) == 1) && ((doneYet2[7]) == 1))
                 {
                     goto endOfScan2;
                 }
@@ -83,25 +90,27 @@
     }
     endOfScan2:
     
-    wait(0.1);
+    wait_ms(150);
     ///////////////////////////////////////////// 
     
     //STOP WHEELS
-    leftMotorVal.write(0.00125*0);
-    rightMotorVal.write(0.00125*0);
+    leftMotorVal.pulsewidth_us(0); 
+    rightMotorVal.pulsewidth_us(0);
+    
+    wait_ms(5); 
     
     //AVERAGE THESE VALUES       
-    for (int i=1;i<=8;i++)
+    for (int i=0;i<=7;i++)
     {
         sensorMinValue[i]=(sensorMinValue1[i]+sensorMinValue2[i])/2;
         sensorMaxValue[i]=(sensorMaxValue1[i]+sensorMaxValue2[i])/2;
     }  
     
     //SET THRESHOLDS FOR ERROR OF 0
-    for (int i=1;i<=8;i++)
+    for (int i=0;i<=7;i++)
     {
-        sensorThreshold[i]=sensorMaxValue[i]-100;
+        sensorThreshold[i]=sensorMaxValue[i]-250;
     } 
-    
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////     
     return;
 }
\ No newline at end of file