File content as of revision 8:9bb806a7f585:

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

/* 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);

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

/* 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 servoHolePeriod_mus = 20000;
int servoPeriod_mus_0 = 0;
int servoPeriod_mus_1 = 0;
int counter = 0;
int loops_per_second = (int)ceilf(1.0f/(0.001f*(float)Ts_ms));

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

int main()
{   
    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(servoPeriod_mus_0, servoHolePeriod_mus); // 1 ms / 20 ms
    servo_1.Enable(servoPeriod_mus_0, servoHolePeriod_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( 0.5f);
            speedController_m1.setDesiredSpeedRPS( 0.5f);
            pwmOut_m2.write(0.75f);

            servo_0.SetPosition(servoPeriod_mus_0);
            servo_1.SetPosition(servoPeriod_mus_1);
            if (servoPeriod_mus_0 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
                servoPeriod_mus_0 += 100;
            }
            if (servoPeriod_mus_1 <= servoHolePeriod_mus & counter%loops_per_second == 0 & counter != 0) {
                servoPeriod_mus_1 += 100;
            }
            counter++;

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

            dist = 0.0f;

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

            servoPeriod_mus_0 = 0;
            servoPeriod_mus_1 = 0;
            servo_0.SetPosition(servoPeriod_mus_0);
            servo_1.SetPosition(servoPeriod_mus_1);

            dist = analogIn.read() * 3.3f;

            led = 0;
        }

        /* do only output what's really necessary*/
        printf("%3.3e, %3.3e, %3d, %3d; \r\n", speedController_m0.getSpeedRPS(),
                                               speedController_m1.getSpeedRPS(),
                                               servoPeriod_mus_0,
                                               servoPeriod_mus_1);

        /* ------------- 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 > 200) {
        executeMainTask = !executeMainTask;
    }
}