Diff: main.cpp

Revision:

0:ce4f790399d9

Child:

1:5b1d88d69aa2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 24 02:44:46 2015 +0000
@@ -0,0 +1,173 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+#define PC_BAUD 9600
+#define BT_BAUD 9600
+#define TX_PIN p13
+#define RX_PIN p14
+#define SDA_PIN p9   //SDA pin on LPC1768
+#define SCL_PIN p10  //SCL pin on LPC1768
+
+I2C sensor(SDA_PIN, SCL_PIN); //Define LIDAR Lite sensor 1
+MODSERIAL bt(TX_PIN, RX_PIN);
+MODSERIAL pc(USBTX,USBRX);
+
+bool newline_detected = false;
+bool newline_sent = false;
+
+// Called everytime a new character goes into
+// the RX buffer. Test that character for \n
+// Note, rxGetLastChar() gets the last char that
+// we received but it does NOT remove it from
+// the RX buffer.
+void rxCallback(MODSERIAL_IRQ_INFO *q)
+{
+    MODSERIAL *serial = q->serial;
+    if ( serial->rxGetLastChar() == '\n') {
+        newline_detected = true;
+    }
+
+}
+
+void txCallback(MODSERIAL_IRQ_INFO *q)
+{
+    MODSERIAL *serial = q->serial;
+    if ( serial->txGetLastChar() == '\0') {
+        newline_sent = true;
+    }
+}
+
+int main()
+{
+    pc.baud(PC_BAUD);
+    bt.baud(BT_BAUD);
+    pc.attach(&rxCallback, MODSERIAL::RxIrq);
+    bt.attach(&txCallback, MODSERIAL::TxIrq);
+    
+    sensor.frequency(100000);
+
+    char sendData[1] = {0x00};
+
+    int addresses[7];
+    addresses[0] = 0x60; //0x60
+    addresses[1] = 0x64; //0x64
+    addresses[2] = 0x68; //middle
+    addresses[3] = 0x6C;
+    addresses[4] = 0x70;
+    addresses[5] = 0x80; //up
+    addresses[6] = 0x84; //down
+
+    uint8_t pulses[7] = {0};
+    uint8_t intensity[7] = {0};
+
+    char btData[12] = {'a','b','c','d','e','f','g','\n','\0'};
+
+    //calibrate down sensor
+    int down_cal = 0;
+    
+    unsigned int i = 0;
+    int count = 0; //for calibration
+    int count2 = 0;//for averaging
+    int differenceAvgSum = 0;
+    int moving_ave[5] = {0};
+    while (1) {
+        for(int k=0; k<5; k++) {
+            char receiveData[3] = {0};
+            if(sensor.write(addresses[k], sendData, 1)){
+                pc.printf("writing to sensor %d failed\n", k);
+                }
+            //write ---> 0 on success, 1 on failure
+            i = 0;
+            while(sensor.read(addresses[k], receiveData, 3) || i < 10) {
+                i++;
+                pc.printf("reading from sensor %d failed\n",k);}
+                //while(!twi_master_transfer(addresses[k], sendData, 1, TWI_ISSUE_STOP)){;}
+                //while(!twi_master_transfer(addresses[k] + 1, receiveData, 3, TWI_ISSUE_STOP)){;}
+                int distance = ((int)receiveData[0]<<8 )+ (int)receiveData[1];
+                if(distance == 0){
+                  pulses[k] = 1;
+                  intensity[k] = 0;
+                }
+                if(distance > 0 && distance < 650) {
+                    pulses[k] = 5;
+                    intensity[k] = 7;
+                } else if(distance >= 650 && distance < 900) {
+                    pulses[k] = 4;
+                    intensity[k] = 6;
+                } else if(distance >= 900 && distance < 1350) {
+                    pulses[k] = 3;
+                    intensity[k] = 5;
+                } else if(distance >= 1350 && distance < 1850) {
+                    pulses[k] = 2;
+                    intensity[k] = 2;
+                } else if(distance >= 1800) {
+                    pulses[k] = 1;
+                    intensity[k] = 0;
+                }
+            pc.printf("num: %d \t pulses: %d \t intensity: %d \n",k,pulses[k],intensity[k]);
+        }
+
+        //find UP distance
+        char receiveData2[3] = {0};
+        sensor.write(addresses[5], sendData, 1);
+        wait(0.1);
+        i = 0;
+        while(sensor.read(addresses[5]+1, receiveData2, 3) || i < 10){
+            i++;}
+        int distance2 = (receiveData2[0]<<8 )+ receiveData2[1];
+        if(distance2 >= 500 && distance2 < 1000) {
+            pulses[5] = 5;
+            intensity[5] = 7;
+        } else {
+            pulses[5] = 1;
+            intensity[5] = 0;
+        }
+
+        //find DOWN distance
+        char receiveData3[3] = {0};
+        sensor.write(addresses[6], sendData, 1);
+        wait(0.1);
+        while(sensor.read(addresses[6]+1, receiveData3, 3) || i < 10){
+            i++;}
+        int distance3 = (receiveData3[0]<<8 )+ receiveData3[1];
+        if(count > 400) {
+            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);
+            if(ave >= 160) {
+                pulses[6] = 5;
+                intensity[6] = 7;
+            } else {
+                pulses[6] = 1;
+                intensity[6] = 0;
+            }
+
+            if(count2 >4) {
+                count2 = 0;
+            }
+        } else {
+            down_cal = distance3;
+            count = count+1;
+        }
+
+        pc.printf("about to send data\n");
+        btData[0] = (pulses[0] << 5) | (intensity[0] << 2);
+        btData[1] = (pulses[1] << 4) | (intensity[1] << 1);
+        btData[2] = (pulses[2] << 3) | (intensity[2]);
+        btData[3] = (pulses[3] << 2) | (intensity[3] >> 1);
+        btData[4] = (intensity[3] << 7) | (pulses[4] << 1) | (intensity[4] >> 2);
+        btData[5] = (intensity[4] << 6) | (0x3);
+        btData[6] = (pulses[5] << 5) | (intensity[5] << 2);
+        btData[7] = (pulses[6] << 5) | (intensity[6] << 2);
+        btData[8] = '\0';
+        for(int j=0;j<9;j++){
+            bt.putc(btData[j]);
+            wait(0.01);
+        }
+        pc.printf("finished sending data\n");
+        //ble_uart_c_write_string(&m_ble_uart_c, (uint8_t *)btData, 9);
+    }
+}