Diff: main.cpp

Revision:

8:5e1854c119ba

Parent:

7:a6dc7ec6e4c0

Child:

9:ba07c0b8899f

--- a/main.cpp	Fri Feb 09 17:23:27 2018 +0000
+++ b/main.cpp	Tue Feb 13 16:29:00 2018 +0000
@@ -27,7 +27,7 @@
 // Defines
 #define IR_CONST 1.229
 
-// Settings                                         // Servo comm   : tx, rx, txEn, id, baud
+// Settings
 TextLCD lcd(PA_5, PA_11, PA_6, PB_12, PA_7, PB_6);  // LCD          : rs, e, d4-d7
 AnalogIn ir(PC_5);                                  // Sharp IR     : analog
 DigitalOut m2(PA_15);                               // extinguisher : 12V out M2, possibly broken
@@ -39,15 +39,17 @@
 Serial pc(USBTX, USBRX);                            // tx, rx
 
 AnalogIn line1(PB_1);                               // Line sensor  : analog (or digital)
-AnalogIn line2(PC_4);                               // Line sensor  : analog (or digital)
+AnalogIn line2(PA_0);                               // 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
+CMPS11 cmp(PB_4, PA_8, 0xC0);                       // Compass      : I2C3_SDA, I2C3_SCL, Addr
+
+DigitalIn button(USER_BUTTON);                      // Button
 
 /* About interrupt
 Where are the interrupt pins on NUCLEO-F411RE?
 
-If you use a recent version of the mbed lib (right mouse button, update in the online compiler):
+If you're using a recent version of the mbed lib (right mouse button, update in the online compiler):
 Every unique numbered pin. That means you can use any pin as InterruptIn, but you cannot use multiple
 pins with the same number on a different port as InterruptIn. So you can use PA_1, PB_2, PA_3, PC_4, etc.
 But in this example you could not use also PE_1. */
@@ -56,17 +58,24 @@
 {   
     // Servos
     for (int i = 1; i <= 18; i++) {
-        Dynamixel servo(PC_6, PC_7, PC_4, i, 1000000);
+        Dynamixel servo(PC_6, PC_7, PC_4, i, 1000000);  // Servo comm   : tx, rx, txEn, id, baud
         servo.setSpeed(100);
         servo.move(512);        // Midddle, thus 90 deg position
     }
     
     float line1_o = 0;
     float line2_o = 0;
-    float meas = 0;
-    float snd = 0;
-    float uvo = 0;
-    int ext = 0;
+    float ir_o = 0;
+    float snd_o = 0;
+    float uv_o = 0;
+    int ext = 0;        // Extinguisher state
+    
+    int tcal = 0;   // Toggle calibration
+    int fcal = 0;   // Force calibration
+    int ptcal = 0;  // Prev toggle calibration
+    int calib = 0;  // Calibration state
+    
+    char ser_i;     // Get data from serial input buffer
     
     // Lidar
     lidar.StartData();
@@ -79,48 +88,44 @@
     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("%.1f L%.2f|%.2f\n", meas, line1_o, line2_o);
-        pc.printf("%.2fcm L%.2f|%.2f ", meas, line1_o, line2_o);
+        lcd.printf("%.1f L%.2f|%.2f\n", ir_o, line1_o, line2_o);
+        pc.printf("%.2fcm L%.2f|%.2f ", ir_o, line1_o, line2_o);
 
-        lcd.printf("s%d u%d\n", (int)snd, (int)uvo);
-        pc.printf("s%d u%d ", snd, uvo);
+        lcd.printf("s%d u%d", (int)snd_o, (int)uv_o);
+        pc.printf("s%.2f u%.2f ", snd_o, uv_o);
         
         // IR
-        meas = ir.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
-        meas = ((1-meas)*26+4);
+        ir_o = ((1-ir.read())*26+4);  // Convert and read the analog input value (value from 0.0 to 1.0)
         
         // Line Sensor
         line1_o = line1.read();
         line2_o = line2.read();
         
-        // Extinguisher or 12V output
+        // Extinguisher (12V output)
         m2 = ext;
         m1 = ext;
-        ext = !ext;
+        ext = !ext; // Switching test
         
         // LED
         led1 = 1;
         led2 = 1;
         
         // Sound Activator
-        snd = sound.read();
+        snd_o = sound.read();
         
         // UV
-        uvo = uv.read();
+        uv_o = uv.read();
         
         // Lidar
         // Aquire LIDAR data from angle 0, 45, 90, 135... 315
         // Then send it to serial PC with 9600 baud
+        
+        // i += 90;
+        // if (i >= 360) {
+        //    i = 0;
         data = lidar.GetData(i);
         speed = lidar.GetSpeed();
         intensity = lidar.GetIntensity(i);
@@ -128,26 +133,52 @@
         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();
+        lcd.printf(" %.1f", (float)val/10);
         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();
-        } */
+        
+        // Standard serial management
+        while(pc.readable()) {
+            // Read serial buffer until it's empty
+            ser_i = pc.getc();
+            switch(ser_i) {
+                case 'r':
+                    pc.printf("Compass Reset\n");
+                    cmp.reset();
+                    break;
+                case 'c':
+                    pc.printf("Compass Calib\n");
+                    fcal = !fcal;
+                    break;
+            }
+            // pc.putc(ser_i);  // Display serial input
+        }
         
+        // Standard button management
+        //tcal = !button.read();
+        //tcal = !uv_o;   // Using UVTron to avoid using button
+        tcal = snd_o;
+        
+        // cmp.reset();
+        if((!tcal && (tcal != ptcal)) || fcal) {  // Rising or forced
+            if(!calib) {
+                cmp.startCalibrate(1);
+                calib = 1;
+            } else cmp.stopCalibrate();
+            fcal = 0;
+        }
+        ptcal = tcal;
+        
+        pc.printf(" B%d|%d", tcal, ptcal);
+        
+        lcd.printf("\n");
         pc.printf("\n");
         
         wait(0.2);