File content as of revision 6:e1fa1a2d7483:

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "string"

/* PM2_Libary */
#include "EncoderCounter.h"
#include "Servo.h"
#include "SpeedController.h"
// #include "FATFileSystem.h"
// #include "SDBlockDevice.h"

using namespace std::chrono;

InterruptIn    user_button(USER_BUTTON);
DigitalOut     led(LED1);
BufferedSerial bufferedSerial(USBTX, USBRX);
char buffer[100];

bool  executeMainTask = false;
Timer user_button_timer, loop_timer;
int   Ts_ms = 2;

/* declaration of custom button functions */
void button_fall();
void button_rise();

/* create analog input object */
AnalogIn analogIn(PC_2);
float    dist = 0.0f;

/* create pwm objects */
PwmOut     pwmOut_m0(PB_13);
PwmOut     pwmOut_m1(PA_9);
PwmOut     pwmOut_m2(PA_10);
float  Ts_pwm = 0.00005f;
/* create enable dc motor digital out object */
DigitalOut enable_motors(PB_15);
/* create encoder read objects */
EncoderCounter  encoderCounter_m0(PA_6, PC_7);
EncoderCounter  encoderCounter_m1(PB_6, PB_7);
EncoderCounter  encoderCounter_m2(PA_0, PA_1);
/* create speed controller objects, only m0 and m1, m2 is used open-loop */
SpeedController speedController_m0(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m0, encoderCounter_m0);
SpeedController speedController_m1(1562.5f, 15.0f, 0.1f, 12.0f, pwmOut_m1, encoderCounter_m1);

/* create servo objects */
Servo servo_0(PB_2);
Servo servo_1(PC_8);
Servo servo_2(PC_6); // not used in this example
int Ts_servo_mus = 20000;
int servo_desval_0 = 0;
int servo_desval_1 = 0;

/* create sd object */
// SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2);
// FATFileSystem fs("fs", &sd);

int main()
{
    bufferedSerial.set_baud(115200);
    
    user_button.fall(&button_fall);
    user_button.rise(&button_rise);
    loop_timer.start();

    /* initialize pwm */
    pwmOut_m0.period(Ts_pwm);
    pwmOut_m1.period(Ts_pwm);
    pwmOut_m2.period(Ts_pwm);
    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
    pwmOut_m1.write(0.5f);
    pwmOut_m0.write(0.5f);
    pwmOut_m2.write(0.5f);
    /* enable driver DC motors */
    enable_motors = 1;

    /* initialize servo */
    servo_0.Enable(servo_desval_0, Ts_servo_mus); // 1 ms / 20 ms
    servo_1.Enable(servo_desval_0, Ts_servo_mus);
    
    /*
    // example code for sd card, not tested from pmic, 02.04.2021
    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");
    }
    */

    while (true) {

        loop_timer.reset();

        /* ------------- start hacking ------------- -------------*/

        if (executeMainTask) {

            /* read analog input */
            dist = analogIn.read() * 3.3f;

            speedController_m0.setDesiredSpeedRPS( 1.0f);
            speedController_m1.setDesiredSpeedRPS(-2.0f);
            pwmOut_m2.write(0.75f);

            servo_0.SetPosition(servo_desval_0);
            servo_1.SetPosition(servo_desval_1);
            if (servo_desval_0 < 10000)
                servo_desval_0 += 100;
            if (servo_desval_1 < 10000)
                servo_desval_1 += 100;

            /* visual feedback that the main task is executed */
            led = !led;

            /* write output via serial buffer */
            int act_buffer_length = snprintf (buffer, 100,
                                              "%3.6e, %3.6e; \r\n",
                                              speedController_m0.getSpeedRPM(),
                                              speedController_m1.getSpeedRPM());
            bufferedSerial.write(buffer, act_buffer_length);
            
        } else {

            dist = 0.0f;

            speedController_m0.setDesiredSpeedRPS(0.0f);
            speedController_m1.setDesiredSpeedRPS(0.0f);
            pwmOut_m2.write(0.5f);

            servo_desval_0 = 0;
            servo_desval_1 = 0;
            servo_0.SetPosition(servo_desval_0);
            servo_1.SetPosition(servo_desval_1);

            dist = analogIn.read() * 3.3f;

            led = 0;
        }

        /* ------------- stop hacking ------------- -------------*/

        int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
        int dT_loop_ms = Ts_ms - T_loop_ms;
        thread_sleep_for(dT_loop_ms);
    }
}

void button_fall()
{
    user_button_timer.reset();
    user_button_timer.start();
}

void button_rise()
{
    int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
    user_button_timer.stop();
    if (t_button_ms > 1)
        executeMainTask = !executeMainTask;
}