Diff: main.cpp

Revision:

6:69c59bcab6ea

Parent:

5:dae415fb4bad

Child:

7:a6dc7ec6e4c0

--- a/main.cpp	Fri Feb 09 15:21:40 2018 +0000
+++ b/main.cpp	Fri Feb 09 16:55:06 2018 +0000
@@ -21,6 +21,9 @@
 #include "TextLCD.h"
 #include "Uvtron.h"
 
+#include "CMPS11.h"
+#include "LIDAR.h"
+
 // Defines
 #define IR_CONST 1.229
 
@@ -31,10 +34,15 @@
 DigitalOut m1(PB_7);                                // extinguisher : 12V out M1, possibly broken
 DigitalOut led1(PC_13);                             // GPIO high is 3V3
 DigitalOut led2(PC_14);
-DigitalIn sound(PA_12);                              // Sound act    : digital, active low
+DigitalIn sound(PA_12);                             // Sound act    : digital, active low
 DigitalIn uv(PB_8);                                 // UVTron       : digital, pin 2 = active low
-Serial pc(USBTX, USBRX); // tx, rx
+Serial pc(USBTX, USBRX);                            // tx, rx
 
+AnalogIn line1(PB_1);                               // Line sensor  : analog (or digital)
+AnalogIn line2(PC_4);                               // Line sensor  : analog (or digital)
+
+LIDAR lidar (PC_10, PC_11, PA_1);                   // Lidar        : tx,rx, motor PWM
+CMPS11 cmp(PB_4, PA_8, 0xC0);                       // Compass      : I2C3_SDA, I2C3_SCL
 
 /* About interrupt
 Where are the interrupt pins on NUCLEO-F411RE?
@@ -53,25 +61,52 @@
         servo.move(512);        // Midddle, thus 90 deg position
     }
     
+    float line1_o = 0;
+    float line2_o = 0;
     float meas = 0;
     int snd = 0;
     int uvo = 0;
+    int ext = 0;
+    
+    // Lidar
+    lidar.StartData();
+    int i = 0;
+    float data, speed;
+    short intensity;
+    bool invalid_flag, strength_flag;
+    
+    // Compass
+    int val;
+    int16_t mx,my,mz;
+    cmp.reset();
+    /*
+    cmp.startCalibrate(true);
+    while(button){  };
+    pc.printf("Calibrate done\n");
+    wait_ms(500);
+    cmp.stopCalibrate();
+    */
     
     while (1) {
         // LCD
-        lcd.printf("%.2fcm\n", meas);
-        pc.printf("%.2fcm ", meas);
+        lcd.printf("%.1f L%.1d%.1d\n", meas, line1_o, line2_o);
+        pc.printf("%.2fcm L1%.2f L2%.2f ", meas, line1_o, line2_o);
 
         lcd.printf("s%d u%d\n", snd, uvo);
-        pc.printf("s%d u%d\n", snd, uvo);
+        pc.printf("s%d u%d ", snd, uvo);
         
         // IR
         meas = ir.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
         meas = ((1-meas)*26+4);
         
+        // Line Sensor
+        line1_o = line1.read();
+        line2_o = line2.read();
+        
         // Extinguisher or 12V output
-        m2 = 1;
-        m1 = 1;
+        m2 = ext;
+        m1 = ext;
+        ext = !ext;
         
         // LED
         led1 = 1;
@@ -82,5 +117,39 @@
         
         // UV
         uvo = uv.read();
+        
+        // Lidar
+        // Aquire LIDAR data from angle 0, 45, 90, 135... 315
+        // Then send it to serial PC with 9600 baud
+        data = lidar.GetData(i);
+        speed = lidar.GetSpeed();
+        intensity = lidar.GetIntensity(i);
+        invalid_flag = lidar.IsDataInvalid(i);
+        strength_flag = lidar.GetStrengthFlag(i);
+        
+        pc.printf("Spd=%.1f; Sdt=%d; D=%.1f; I=%.1d; finvalid=%d; fstrength=%d\t", speed, i, data, intensity, invalid_flag, strength_flag);
+
+        /*
+        i += 90;
+        if (i >= 360) {
+            i = 0;
+        }    
+        */
+        
+        // Compass
+        val = cmp.readBearing();
+        mx = cmp.mag_x();
+        my = cmp.mag_y();
+        mz = cmp.mag_z();
+        pc.printf("%d.%d\t", val/10, val%10);
+        pc.printf("%d %d %d\t", mx, my, mz);
+        /* if(!button){
+            pc.printf("Reset/n");
+            cmp.reset();
+        } */
+        
+        pc.printf("\n");
+        
+        wait(0.2);
     }
 }   
\ No newline at end of file