File content as of revision 0:41afd907b7bd:

#include "mbed.h"
#include "LSM9DS1.h"
#include "Servo.h"
#include "EncoderCounter.h"
#include "Stepper.h"
#include "SDFileSystem.h"

DigitalOut myled(LED1);

// create object IMU
LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C);

// create servo objects
Servo  S0(PB_2);
Servo  S1(PC_8);
Servo  S2(PC_6);

DigitalOut enable(PB_15);

// initialise PWM
PwmOut pwm_motor1(PB_13);
PwmOut pwm_motor2(PA_9);
PwmOut pwm_motor3(PA_10);

// crete Encoder read objects
EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
EncoderCounter counter2(PB_6, PB_7);
EncoderCounter counter3(PA_0, PA_1);

// initialise PWM
Stepper stepperMotor(PB_14,PC_4); // step, dir
Stepper stepperMotor2(PB_12,PA_15); // config 2

DigitalIn user_button(USER_BUTTON);

SDFileSystem sd(PC_12, PC_11, PC_10, PD_2, "sd"); // mosi miso clk cs
AnalogIn   ain(PC_2);
DigitalOut   enableStepper(PB_1);

// define write function
void sdWrite(float a){
    printf("start write SD\r\n");   
    
    FILE *fp = fopen("/sd/mydir/sdtest.txt", "a");
    if(fp == NULL) {
        error("Could not open file for write\r\n");
    }
    fprintf(fp, "analog measure %f\r\n",a);
    fclose(fp); 
 
    printf("write succesfull\r\n\n");
}

int main() {
    // test comunication
    imu.begin();
    if (!imu.begin()) {
        printf("Failed to communicate with LSM9DS1.\r\n");
    }
    
    // initialise and test Servo
    S0.Enable(1000,20000);
    S1.Enable(1000,20000);
    S2.Enable(1000,20000);
    
    // initialise PWM
    pwm_motor1.period(0.0005f);// 0.5ms 2KHz
    pwm_motor1.write(0.5f);
    pwm_motor2.period(0.0005f);// 0.5ms 2KHz
    pwm_motor2.write(0.5f);
    pwm_motor3.period(0.0005f);// 0.5ms 2KHz
    pwm_motor3.write(0.5f);
    
    // initialise stepper
    stepperMotor.setSpeed(10000);
    stepperMotor.rotate(stepperMotor.CW);
    stepperMotor.stop();
    stepperMotor.setPositionZero();
    
    stepperMotor2.setSpeed(10000);
    stepperMotor2.rotate(stepperMotor.CW);
    stepperMotor2.stop();
    stepperMotor2.setPositionZero();
    
    int i = 0;
    
    bool buttonNow = false;
    bool buttonBefore = false;
    bool done = false;
    
    enableStepper = 1;
    
    
    while(1) {
        
        
        buttonNow = !user_button;
        
        if(buttonNow && !buttonBefore){i+=1; done = false;}
        if(i>7){i=0;}
        
        switch (i){
            case 1:
            // ===================
            // IMU
            // ===================
            if(!done){
                printf("\r\n\r\n======================\r\nIMU test\r\n======================\r\n\r\n");
                for(int i=0; i<5; i++){
                    // do a measure
                    imu.readAccel();
                    imu.readMag();
                    imu.readGyro();
                    
                    // print values to console       
                    printf("gyro: %.5f %.5f %.5f [rad/s]\r\n", imu.gyroX,imu.gyroY,imu.gyroZ);
                    printf("accel: %.5f %.5f %.5f [m/s^2]\r\n", imu.accX,imu.accY,imu.accZ);
                    printf("mag: %.5f %.5f %.5f [Gauss]\r\n\n", imu.magX,imu.magY,imu.magZ);
                    wait(0.5f);   
                }
                done = true;
                break;
            } else {
                break;
                }
            
            case 2:
            // ===================
            // Servo
            // ===================
            if(!done){
                printf("\r\n\r\n======================\r\nServo test\r\n======================\r\n\r\n");
                for(int j=0; j<2000; j+=50){
                      printf("position %d\r\n",j);
                    S0.SetPosition(j);
                    S1.SetPosition(j);
                    S2.SetPosition(j);
                    
                    wait(0.1f);
                }
                done = true;
                break;
            } else {
                break;
            }
            
            case 3:
            // ===================
            // DC - Motor1
            // ===================
            if(!done){
                printf("\r\n\r\n======================\r\nDC - Motor1 test\r\n======================\r\n\r\n");
                enable = 1;
                for(int j=0; j<20; j+=1){
                    pwm_motor1.write(0.7f);
                    pwm_motor2.write(0.7f);
                    pwm_motor3.write(0.7f);
                    // display encoder values to console
                    printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
                    wait(0.1f);
                }
                enable = 0;
                done = true;
                break;
            } else {
                break;
            }
            
            case 4:
            // ===================
            // DC - Motor2
            // ===================
            if(!done){
                printf("\r\n\r\n======================\r\nDC - Motor2 test\r\n======================\r\n\r\n");
                enable = 1;
                for(int j=0; j<20; j+=1){
                    pwm_motor1.write(0.7f);
                    pwm_motor2.write(0.7f);
                    pwm_motor3.write(0.7f);
                    // display encoder values to console
                    printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
                    wait(0.1f);
                }
                enable = 0;
                done = true;
                break;
            } else {
                break;
            }
            
            case 5:
            // ===================
            // DC - Motor3
            // ===================
            if(!done){
                printf("\r\n\r\n======================\r\nDC - Motor3 test\r\n======================\r\n\r\n");
                enable = 1;
                for(int j=0; j<20; j+=1){
                    pwm_motor1.write(0.7f);
                    pwm_motor2.write(0.7f);
                    pwm_motor3.write(0.7f);
                    // display encoder values to console
                    printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
                    wait(0.1f);
                }
                enable = 0;
                done = true;
                break;
            } else {
                break;
            }
            
            case 6:
            // ===================
            // Stepper
            // ===================
            if(!done){
                printf("\r\n\r\n======================\r\nStepper test\r\n======================\r\n\r\n");
                    // test stepper motor
                    enableStepper = 0;
                    wait(0.5f);
                    printf("stepper enabled\r\n");
                    stepperMotor.goesTo(20*2048);
                    stepperMotor2.goesTo(20*2048);
                    wait(3.0f);
                    stepperMotor.stop();
                    stepperMotor2.stop();
                    stepperMotor.setPositionZero();
                    stepperMotor2.setPositionZero();
                    enableStepper = 1;
                    printf("stepper done\r\n");
                done = true;
                break;
            } else {
                break;
            }
            
            case 7:
            // ===================
            // SD
            // ===================
            if(!done){
                printf("\r\n\r\n======================\r\nSD test\r\n======================\r\n\r\n");
                    
                    // crearte folder
                    mkdir("/sd/mydir", 0777);
                    
                    // create file
                    printf("start write SD\r\n");   
                    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
                    if(fp == NULL) {
                        error("Could not open file for write\r\n");
                    }
                    fprintf(fp, "analog measure\r\n");
                    fprintf(fp, "==============\r\n");
                    fclose(fp);
                    
                    // write 2 times an analog value
                    for(int i=0; i<2; i++){
                        sdWrite(ain.read());
                        wait(2.0f);
                    }
                    // read from SD card
                    printf("Reading from SD card...");
                    fp = fopen("/sd/mydir/sdtest.txt", "r");
                    if (fp != NULL) {
                        char c = fgetc(fp);
                        if (c == 'a')
                            printf("success!\n");
                        else
                            printf("incorrect char (%c)!\n", c);
                        fclose(fp);
                    } else {
                        printf("failed!\n");
                    }
                    
                done = true;
                break;
            } else {
                break;
            }
            
            
            default:
            printf("\r\n\r\n======================\r\nPause\r\n======================\r\n\r\n");
            break;
        }

        
        myled = 1;
        
        wait(0.1); // 0.5s
        
        myled = 0;
    }
}