Diff: main.cpp

Revision:

13:b22fd9c4fbb4

Parent:

12:6deb3b41c9e3

Child:

14:8620dceb3ba2

--- a/main.cpp	Fri Oct 16 19:42:06 2015 +0000
+++ b/main.cpp	Fri Oct 16 20:10:10 2015 +0000
@@ -2,33 +2,33 @@
 #include "MODSERIAL.h"
 #include <math.h>
 #include "BNO055.h"
+//for calculating allowable change in height for the down lidar
+#define DOWN_ANGLE -5.6
+#define ALLOW_HEIGHT 120
+
+//Up Delta
+#define UP_MIN 500
+#define UP_MAX 1100
 
 //Initial LONG Range Settings
-#define RANGE_01 750
-#define RANGE_02 1100
-#define RANGE_03 1700
-#define RANGE_04 2750
-#define UP_MIN 600
-#define UP_MAX 1300
-#define DOWN_DIFF 500
+//#define RANGE_01 750
+//#define RANGE_02 1100
+//#define RANGE_03 1700
+//#define RANGE_04 2750
+//#define UP_MIN 600
+//#define UP_MAX 1300
 
 // Mid range settings
-//#define RANGE_01 450
-//#define RANGE_02 750
-//#define RANGE_03 1150
-//#define RANGE_04 1550
-//#define UP_MIN 600
-//#define UP_MAX 1300
-//#define DOWN_DIFF 260
+#define RANGE_01 450
+#define RANGE_02 750
+#define RANGE_03 1150
+#define RANGE_04 1550
 
 // Short range settings
 //#define RANGE_01 400
 //#define RANGE_02 650
 //#define RANGE_03 1000
 //#define RANGE_04 1350
-//#define UP_MIN 600
-//#define UP_MAX 1300
-//#define DOWN_DIFF 260
 
 
 #define PC_BAUD 9600
@@ -47,9 +47,7 @@
 MODSERIAL bt(TX_PIN, RX_PIN);
 MODSERIAL pc(USBTX,USBRX);
 
-//for calculating allowable change in height for the down lidar
-float allowHeight = 120; //120 mm
-float downAngle = -5.6;
+
 //for calibrating IMU
 //for IMU1:
 //char cal_vals[22] = {255, 255, 220, 255, 13, 0, 83, 255, 36, 1, 80, 0, 253, 255, 0, 0, 1, 0, 232, 3, 235, 2};
@@ -181,8 +179,8 @@
             i++;}
         int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
         if(distance2 >= UP_MIN && distance2 < UP_MAX) {
-            pulses[5] = 1; ///TODO WAS: 5
-            intensity[5] = 0;   ///TODO: 7
+            pulses[5] = 5; ///TODO WAS: 5
+            intensity[5] = 7;   ///TODO: 7
         } else {
             pulses[5] = 1;
             intensity[5] = 0;
@@ -199,8 +197,8 @@
             //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 cosp = cos((pitch-DOWN_ANGLE) * PI/180.0);
+            float allowX =  ALLOW_HEIGHT/cosp;
             float new_down_cal = ((float)down_cal)*cospi/cosp;
             
             //use moving average to find deltaX
@@ -211,7 +209,7 @@
             count2 = count2 + 1;
             int ave = (int)(differenceAvgSum/5);
             
-            //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("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) {
@@ -230,7 +228,7 @@
             down_cal = distance3;
             imu.get_angles();
             float pitch = imu.euler.pitch;
-            cospi = cos((pitch-downAngle) * PI/180.0);
+            cospi = cos((pitch-DOWN_ANGLE) * PI/180.0);
             count = count+1;
         }
         //pc.printf("num: %d \t pulses: %d \t intensity: %d \n",6,pulses[6],intensity[6]);