File content as of revision 8:5e1854c119ba:

/*****************************************************
- 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
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(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, Addr

DigitalIn button(USER_BUTTON);                      // Button

/* About interrupt
Where are the interrupt pins on NUCLEO-F411RE?

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. */

int main()
{   
    // Servos
    for (int i = 1; i <= 18; i++) {
        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 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();
    int i = 0;
    float data, speed;
    short intensity;
    bool invalid_flag, strength_flag;
    
    // Compass
    int val;
    int16_t mx,my,mz;
    cmp.reset();
    
    while (1) {
        // LCD
        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", (int)snd_o, (int)uv_o);
        pc.printf("s%.2f u%.2f ", snd_o, uv_o);
        
        // IR
        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 (12V output)
        m2 = ext;
        m1 = ext;
        ext = !ext; // Switching test
        
        // LED
        led1 = 1;
        led2 = 1;
        
        // Sound Activator
        snd_o = sound.read();
        
        // UV
        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);
        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);
        //}
        
        // 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);
        
        // 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);
    }
}