File content as of revision 3:d22942631cd7:

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "SDBlockDevice.h"
#include "FATFileSystem.h"
#include "EncoderCounter.h"
#include "Servo.h"
#include "SpeedController.h"

int main()
{
    
    DigitalIn user_button(USER_BUTTON);
    
    // initialise PWM
    PwmOut pwm_motor_0(PB_13);
    PwmOut pwm_motor_1(PA_9);
    PwmOut pwm_motor_2(PA_10);
    
    // crete Encoder read objects
    EncoderCounter encoderCounter_0(PA_6, PC_7);
    EncoderCounter encoderCounter_1(PB_6, PB_7);
    EncoderCounter encoderCounter_2(PA_0, PA_1);
    
    // create controller
    SpeedController speedController_0(pwm_motor_0, encoderCounter_0);
    SpeedController speedController_1(pwm_motor_1, encoderCounter_1);
        
    DigitalOut enable_dc_motors(PB_15);
    
    // create servo objects
    Servo  servo_0(PB_2);
    Servo  servo_1(PC_8);
    Servo  servo_2(PC_6);
    
    SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
    printf("BlockDevice created\r\n"); 
    FATFileSystem fs("fs", &sd);
    
    // Initialise the digital pin LED1 as an output
    DigitalOut myled(LED1);
    
    // initialise PWM
    pwm_motor_0.period(0.00005f);// 0.05ms 20KHz
    pwm_motor_0.write(0.5f);
    pwm_motor_1.period(0.00005f);// 0.05ms 20KHz
    pwm_motor_1.write(0.5f);
    pwm_motor_2.period(0.00005f);// 0.05ms 20KHz
    pwm_motor_2.write(0.5f);
    
    // initialise and test Servo
    servo_0.Enable(1000, 20000); // 1 ms / 20 ms
    servo_1.Enable(1000, 20000);
    servo_2.Enable(1000, 20000);
    int servo_desval_0 = 0;
    int servo_desval_1 = 0;
    int servo_desval_2 = 0;
    
    /*
    printf("Test writing... ");
    FILE* fp = fopen("/fs/data.csv", "w");
    fprintf(fp, "test %.5f\r\n",1.23);
    fclose(fp);
    printf("done\r\n");
    
    printf("Test reading... ");
    // read from SD card
    fp = fopen("/fs/data.csv", "r");
    if (fp != NULL) {
        char c = fgetc(fp);
        if (c == 't')
            printf("done\r\n");
        else
            printf("incorrect char (%c)!\n", c);
        fclose(fp);
    } else {
        printf("Reading failed!\n");
    }
    */
    
    // enable driver DC motors
    enable_dc_motors = 1;
    
    while (true) {
        
        if(!user_button) {
            // LED off, set speedController speed, pwm2, position servo
            myled = 0;
            
            speedController_0.setDesiredSpeed(  60.0f);
            speedController_1.setDesiredSpeed(-120.0f);
            pwm_motor_2.write(0.7f);
            
            servo_0.SetPosition(servo_desval_0);
            servo_1.SetPosition(servo_desval_1);
            servo_2.SetPosition(servo_desval_2);
            if(servo_desval_0 < 10000) servo_desval_0 += 100;
            if(servo_desval_1 < 10000) servo_desval_1 += 100;
            if(servo_desval_2 < 10000) servo_desval_2 += 100;

        } else {
            // LED on, reset speedController speed, pwm2, position servo
            myled = 1;
            
            speedController_0.setDesiredSpeed(0.0f);
            speedController_1.setDesiredSpeed(0.0f);
            pwm_motor_2.write(0.5f);

            servo_desval_0 = 0.0f;
            servo_desval_1 = 0.0f;
            servo_desval_2 = 0.0f;
            servo_0.SetPosition(servo_desval_0);
            servo_1.SetPosition(servo_desval_1);
            servo_2.SetPosition(servo_desval_2);

        }
        
        // printf("speedLeft: %f, speedRight: %f\r\n",controller.getSpeedLeft(), controller.getSpeedRight());
        // printf("counter1 = %d counter2 = %d counter3 = %d\r\n", encoderCounter_0.read(), encoderCounter_1.read(), encoderCounter_2.read());
        printf("speedController_0 = %d speedController_1 = %d encoderCounter_2 = %d\r\n", static_cast<int>(speedController_0.getSpeed()*1000.0f), 
                                                                                          static_cast<int>(speedController_1.getSpeed()*1000.0f), 
                                                                                          encoderCounter_2.read());
        
        thread_sleep_for(500);
    }
}