Diff: main.cpp

Revision:

8:2ddeec5d8f84

Parent:

7:660e8ddb231e

Child:

9:59d23ab8d73b

--- a/main.cpp	Wed Oct 14 20:33:38 2015 +0000
+++ b/main.cpp	Thu Oct 15 14:58:35 2015 +0000
@@ -95,6 +95,7 @@
 
     //calibrate down sensor
     int down_cal = 0;
+    float cospi = 0; //"cosine of initial pitch"
     
     unsigned int i = 0;
     int count = 0; //for calibration
@@ -161,22 +162,23 @@
         while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
             i++;}
         int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
-        if(count > 200) { //calibration over
-            int difference = abs(down_cal - distance3);
+        if(count > 100) { //calibration over
+            //get allowableX and adjusted down_cal from IMU
+            imu.get_angles();
+            float pitch = imu.euler.pitch;
+            float cosp = cos((pitch-downAngle) * PI/180.0);
+            float allowX =  allowHeight/cosp;
+            float new_down_cal = ((float)down_cal)*cospi/cosp;
+            
+            //use moving average to find deltaX
+            int difference = abs(new_down_cal - distance3);
             differenceAvgSum = differenceAvgSum - moving_ave[count2];
             moving_ave[count2] = difference;
             differenceAvgSum = differenceAvgSum + difference;
             count2 = count2 + 1;
             int ave = (int)(differenceAvgSum/5);
             
-            //get allowableX from IMU
-            imu.get_angles();
-            float roll = imu.euler.roll;
-            float pitch = imu.euler.pitch;
-            float yaw = imu.euler.yaw;
-            float cosp = cos((pitch-downAngle) * PI/180.0);
-            float allowX =  allowHeight/cosp;
-            pc.printf("distance: %d\tallowableX: %f\r\n",ave,allowX);
+            pc.printf("distance: %d\tallowableX: %f\tdistance: %d\tpitch: %f\tdowncal: %d\tnewdowncal: %f\r\n",ave,allowX,distance3,pitch-downAngle,down_cal,new_down_cal);
             
             //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
             if(ave >= allowX) {
@@ -191,7 +193,11 @@
                 count2 = 0;
             }
         } else {
+            
             down_cal = distance3;
+            imu.get_angles();
+            float pitch = imu.euler.pitch;
+            cospi = cos((pitch-downAngle) * PI/180.0);
             count = count+1;
         }
         //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);