Diff: main.cpp

Revision:

5:dae415fb4bad

Parent:

4:9932af380e56

Child:

6:69c59bcab6ea

--- a/main.cpp	Fri Feb 02 15:46:42 2018 +0000
+++ b/main.cpp	Fri Feb 09 15:21:40 2018 +0000
@@ -24,17 +24,17 @@
 // Defines
 #define IR_CONST 1.229
 
-// Settings
-Dynamixel* servos[3*6];                             // Servo comm   : tx, rx, txEn, id, baud
-TextLCD lcd(PB_2, PB_1, PB_15, PB_14, PB_13, PC_4); // LCD          : rs, e, d4-d7
-AnalogIn ir(PA_5);                                  // Sharp IR     : analog
+// Settings                                         // Servo comm   : tx, rx, txEn, id, baud
+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
 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_6);                              // Sound act    : digital
+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
 
-Uvtron uv(PA_11);                                   // UVTron       : interrupt
 
 /* About interrupt
 Where are the interrupt pins on NUCLEO-F411RE?
@@ -46,29 +46,28 @@
 
 int main()
 {   
+    // Servos
+    for (int i = 1; i <= 18; i++) {
+        Dynamixel servo(PC_6, PC_7, PC_4, i, 1000000);
+        servo.setSpeed(100);
+        servo.move(512);        // Midddle, thus 90 deg position
+    }
+    
     float meas = 0;
     int snd = 0;
-    
-    // Servos
-    for (int i = 1; i <= 20; i++) {
-        servos[i] = new Dynamixel(PC_6, PC_7, PC_4, i, 1000000);
-    }
+    int uvo = 0;
     
     while (1) {
         // LCD
-        lcd.printf("%.0f cm\n", meas);
-        lcd.printf("sound %d | uv %d", snd, uv.Flag);
-        
-        // Servos
-        for (int i = 1; i <= 20; i++) {
-            servos[i]->setSpeed(100);
-            servos[i]->move(512);    // Midddle, thus 90 deg position
-        }
+        lcd.printf("%.2fcm\n", meas);
+        pc.printf("%.2fcm ", meas);
+
+        lcd.printf("s%d u%d\n", snd, uvo);
+        pc.printf("s%d u%d\n", snd, uvo);
         
         // IR
         meas = ir.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
-        meas = 5000000 / powf(meas, IR_CONST)+1;
-        lcd.printf("%.0f cm\n", meas);
+        meas = ((1-meas)*26+4);
         
         // Extinguisher or 12V output
         m2 = 1;
@@ -80,5 +79,8 @@
         
         // Sound Activator
         snd = sound.read();
+        
+        // UV
+        uvo = uv.read();
     }
 }   
\ No newline at end of file