Diff: main.cpp

Revision:

15:d6edd100d7fe

Parent:

14:8620dceb3ba2

Child:

16:70fce771d828

diff -r 8620dceb3ba2 -r d6edd100d7fe main.cpp
--- a/main.cpp	Sat Oct 17 19:02:41 2015 +0000
+++ b/main.cpp	Sat Oct 17 19:30:41 2015 +0000
@@ -77,7 +77,8 @@
 bool newline_sent = false;
 
 float potValue(){
-    float pot = potentiometer.read();
+    //float pot = potentiometer.read();
+    float pot = 0.25;
     if(pot <= 0.5)
         return 2*pot;
     else
@@ -173,13 +174,17 @@
 
     //calibrate down sensor
     int down_cal = 0;
+    int down_cal2 = 0;
     float cospi = 0; //"cosine of initial pitch"
     
     unsigned int i = 0;
     int count = 0; //for calibration
     int count2 = 0;//for averaging
+    int count2_2 = 0; //for averaging second down sensor
     int differenceAvgSum = 0;
+    int differenceAvgSum2 = 0;
     int moving_ave[5] = {0};
+    int moving_ave2[5] = {0};
     while (1) {
         for(int k=0; k<5; k++) {
             char receiveData[3] = {0};
@@ -201,6 +206,9 @@
                   pulses[k] = 1;
                   intensity[k] = 0;
                 }
+                //for(int z=0;z<4;z++)
+                //    pc.printf("angle: %f radius %d is %f ",angle[k],z,radius[z]);
+                //pc.printf("\n");
                 if(distance > 0 && distance < radius[0]) {
                     pulses[k] = 5;
                     intensity[k] = 7;
@@ -221,19 +229,60 @@
         }
 
         //find UP distance
+//        char receiveData2[3] = {0};
+//        sensor.write(addresses[5], sendData, 1);
+//        i = 0;
+//        while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
+//            i++;}
+//        int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
+//        if(distance2 >= UP_MIN && distance2 < UP_MAX) {
+//            pulses[5] = 5; ///TODO WAS: 5
+//            intensity[5] = 7;   ///TODO: 7
+//        } else {
+//            pulses[5] = 1;
+//            intensity[5] = 0;
+//        }
+
+        //NEW down sensor:
         char receiveData2[3] = {0};
         sensor.write(addresses[5], sendData, 1);
         i = 0;
         while(sensor.read(addresses[5]+1, receiveData2, 3) && i < 10){
             i++;}
         int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
-        if(distance2 >= UP_MIN && distance2 < UP_MAX) {
-            pulses[5] = 5; ///TODO WAS: 5
-            intensity[5] = 7;   ///TODO: 7
-        } else {
-            pulses[5] = 1;
-            intensity[5] = 0;
+        if(count > 50) { //calibration over
+            //get allowableX and adjusted down_cal from IMU
+            imu.get_angles();
+            float pitch = imu.euler.pitch;
+            float cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
+            float allowX =  ALLOW_HEIGHT/cosp;
+            float new_down_cal = ((float)down_cal2)*cospi/cosp;
+            
+            //use moving average to find deltaX
+            int difference = abs(new_down_cal - distance2);
+            differenceAvgSum2 = differenceAvgSum2 - moving_ave2[count2_2];
+            moving_ave2[count2_2] = difference;
+            differenceAvgSum2 = differenceAvgSum2 + difference;
+            count2_2 = count2_2 + 1;
+            int ave = (int)(differenceAvgSum2/5);
+            
+            //pc.printf("distance: %d\tallowableX: %f\tdistance: %d\tpitch: %f\tdowncal: %d\tnewdowncal: %f\r\n",ave,allowX,distance3,pitch-DOWN_ANGLE,down_cal,new_down_cal);
+            
+            //pc.printf("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
+            if(ave >= allowX) {
+                pulses[5] = 5;
+                intensity[5] = 7;
+            } else {
+                pulses[5] = 1; 
+                intensity[5] = 0;
+            }
+
+            if(count2_2 >4) {
+                count2 = 0;
+            }
         }
+        else
+            down_cal2 = distance2;
 
         //find DOWN distance
         char receiveData3[3] = {0};
@@ -242,7 +291,7 @@
         while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
             i++;}
         int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
-        if(count > 100) { //calibration over
+        if(count > 50) { //calibration over
             //get allowableX and adjusted down_cal from IMU
             imu.get_angles();
             float pitch = imu.euler.pitch;