File content as of revision 7:a6dc7ec6e4c0:

/*****************************************************
- Description: mbed to Dynamixel connection test using
    the library
- Requirements: 
    Dynamixel (i.e. DX116, RX28)
    MAX3088/MAX485 (RS485 transceiver)
- Connections:
    MAX3088     --  mbed
    ======================
    Pin 1       --  Pin 14
    Pin 2       --  Pin 15
    Pin 4       --  Pin 13
    
- Comments:
    See schematic for wiring details and class 
    documentation for available methods.
*****************************************************/

#include "mbed.h"
#include "Dynamixel.h"
#include "TextLCD.h"
#include "Uvtron.h"

#include "CMPS11.h"
#include "LIDAR.h"

// Defines
#define IR_CONST 1.229

// 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, possibly broken
DigitalOut m1(PB_7);                                // extinguisher : 12V out M1
DigitalOut led1(PC_13);                             // GPIO high is 3V3
DigitalOut led2(PC_14);
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

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?

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()
{   
    // 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 line1_o = 0;
    float line2_o = 0;
    float meas = 0;
    float snd = 0;
    float 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("%.1f L%.2f|%.2f\n", meas, line1_o, line2_o);
        pc.printf("%.2fcm L%.2f|%.2f ", meas, line1_o, line2_o);

        lcd.printf("s%d u%d\n", (int)snd, (int)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 = ext;
        m1 = ext;
        ext = !ext;
        
        // LED
        led1 = 1;
        led2 = 1;
        
        // Sound Activator
        snd = sound.read();
        
        // 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);
    }
}