Diff: main.cpp

Revision:

4:9932af380e56

Parent:

1:ef90d942ce78

Child:

5:dae415fb4bad

diff -r e5577be2fe70 -r 9932af380e56 main.cpp
--- a/main.cpp	Wed Aug 19 05:42:25 2015 +0000
+++ b/main.cpp	Fri Feb 02 15:46:42 2018 +0000
@@ -15,25 +15,70 @@
     See schematic for wiring details and class 
     documentation for available methods.
 *****************************************************/
+
 #include "mbed.h"
 #include "Dynamixel.h"
+#include "TextLCD.h"
+#include "Uvtron.h"
+
+// 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
+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
+
+Uvtron uv(PA_11);                                   // UVTron       : interrupt
+
+/* 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):
+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. */
 
 int main()
-{
-    Dynamixel DX116(p13, p14, p15, 9, 57600);
+{   
+    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);
+    }
     
-    DX116.toggleLED(1);
-    wait(0.5);
-    DX116.toggleLED(0);
-    DX116.setSpeed(0);
-    wait(0.5);
-    DX116.move(0);          // MOVE
-    wait(1);
-    DX116.setSpeed(100);
-    wait(0.5);
-    DX116.move(1023);       // MOVE
-    wait(1);
-    DX116.setSpeed(1023);
-    wait(1);
-    DX116.move(0);          // MOVE
+    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
+        }
+        
+        // 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);
+        
+        // Extinguisher or 12V output
+        m2 = 1;
+        m1 = 1;
+        
+        // LED
+        led1 = 1;
+        led2 = 1;
+        
+        // Sound Activator
+        snd = sound.read();
+    }
 }   
\ No newline at end of file