Diff: main.cpp

Revision:

7:660e8ddb231e

Parent:

3:a0ccaf565e8d

Child:

8:2ddeec5d8f84

--- a/main.cpp	Fri Sep 25 15:14:10 2015 +0000
+++ b/main.cpp	Wed Oct 14 20:33:38 2015 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include <math.h>
+#include "BNO055.h"
 
 #define PC_BAUD 9600
 #define BT_BAUD 9600
@@ -7,14 +9,33 @@
 #define RX_PIN p14
 #define SDA_PIN p9   //SDA pin on LPC1768
 #define SCL_PIN p10  //SCL pin on LPC1768
+#define IMU_SDA p28
+#define IMU_SCL p27
+#define PI 3.14159265
 
+BNO055 imu(p28,p27);
 I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1
 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 = -7.0;
+//for calibrating IMU
+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};
+
+//for encoder:
+//need to set pins to DigitalIn with internal pullup resistors
+//DigitalIn mySwitch(p21);
+//mySwitch.mode(PullUp);
+
 bool newline_detected = false;
 bool newline_sent = false;
 
+void setCal(){
+    imu.write_calibration_data();
+}
+
 // Called everytime a new character goes into
 // the RX buffer. Test that character for \n
 // Note, rxGetLastChar() gets the last char that
@@ -44,6 +65,16 @@
     pc.attach(&rxCallback, MODSERIAL::RxIrq);
     bt.attach(&txCallback, MODSERIAL::TxIrq);
     
+    //set up IMU
+    imu.reset();
+    imu.setmode(OPERATION_MODE_NDOF);
+    setCal();
+    imu.get_calib();
+    while (imu.calib == 0)
+    {
+        imu.get_calib();
+    }
+    
     sensor.frequency(100000);
 
     char sendData[1] = {0x00};
@@ -130,15 +161,25 @@
         while(sensor.read(addresses[6]+1, receiveData3, 3) && i < 10){
             i++;}
         int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
-        if(count > 200) {
+        if(count > 200) { //calibration over
             int difference = abs(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("down_cal: %d \t diff: %d \t distance: %d\n",down_cal, ave, distance3);
-            if(ave >= 160) {
+            if(ave >= allowX) {
                 pulses[6] = 5;
                 intensity[6] = 7;
             } else {