File content as of revision 16:1be4a1c2d08a:

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

/* PM2_Libary */
#include "EncoderCounter.h"
#include "Servo.h"
#include "SpeedController.h"
#include "PositionController.h"
#include "FastPWM.h"
#include "RangeFinder.h"

using namespace std::chrono;

InterruptIn user_button(PC_13);
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();

// SHARP GP2Y0A21Y IR Sensor
// https://www.digitec.ch/de/s1/product/sharp-distanz-sensor-1-st-gp2y0a21y-aktive-bauelemente-8425699?gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hMUr4sljdd8LfsdhBBlhxKY5gyDmZQ49ghgiIRZaKWdj85ISUw5r4oaAmM9EALw_wcB&gclsrc=aw.ds
/* create analog input object */
AnalogIn analogIn(PC_2);
float    dist_IRSensor = 0.0f;

// 78:1 Metal Gearmotor 20Dx43L mm 12V CB
// https://www.pololu.com/product/3477
/* create enable dc motor digital out object */
DigitalOut enable_motors(PB_15);
/* create pwm objects */
FastPWM pwmOut_M1(PB_13);
FastPWM pwmOut_M2(PA_9);
FastPWM pwmOut_M3(PA_10);
double  Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
/* create encoder read objects */
EncoderCounter  encoderCounter_M1(PA_6, PC_7);
EncoderCounter  encoderCounter_M2(PB_6, PB_7);
EncoderCounter  encoderCounter_M3(PA_0, PA_1);
/* create speed controller objects, only M1 and M2, M3 is used open-loop */
float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
float kn = 180.0f/12.0f;               // (RPM/V)
float max_voltage = 12.0f;             // adjust this to 6.0f if only one batterypack is used
SpeedController    speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
PositionController positionController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
float max_speed_rps = 1.0f;            // has to be smaller or equal to kn * max_voltage

// Futaba Servo S3001 20mm 3kg Analog 
// https://www.modellmarkt24.ch/pi/RC-Elektronik/Servos/Standard-Servo-20mm/futaba-servo-s3001-20mm-3kg-analog.html?gclid=CjwKCAjw3pWDBhB3EiwAV1c5rK_-x_Bt19_wIY-IcS2C-RULXKBtYfY0byxejkZLjASro-EMPBUhrxoCgaQQAvD_BwE
/* create servo objects */
Servo servo_S1(PB_2);
Servo servo_S2(PC_8);
// Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below
int Ts_pwm_mus = 20000;
int servoOutput_mus_S1 = 0;
int servoOutput_mus_S2 = 0;
int servo_counter = 0;
int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));

// Groove Ultrasonic Ranger V2.0
// https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds 
/* create range finder object (ultra sonic distance sensor) */
RangeFinder rangeFinder(PC_6, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
// RangeFinder rangefinder(PB_6, 5782.0f, 0.02f,  7000); // 1/Ts_ms = 50 Hz parametrization
float       dist_USSensor = 0.0f;

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

    /* enable hardwaredriver dc motors */
    enable_motors = 1;
    /* initialize pwm for motor M3*/
    pwmOut_M3.period(Ts_pwm_s);
    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
    pwmOut_M3.write(0.5);

    /* enable servos, you can also disable them */
    servo_S1.Enable(servoOutput_mus_S1, Ts_pwm_mus);
    servo_S2.Enable(servoOutput_mus_S2, Ts_pwm_mus);

    while (true) {

        loop_timer.reset();

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

        if (executeMainTask) {

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

            /* command a speed to dc motors M1 and M2*/
            speedController_M1.setDesiredSpeedRPS( 1.0f);
            positionController_M2.setDesiredRotation(0.5f, max_speed_rps);
            /* write output voltage to motor M3 */
            pwmOut_M3.write(0.75);

            /* command servo position via output time, this needs to be calibrated */
            servo_S1.SetPosition(servoOutput_mus_S1);
            servo_S2.SetPosition(servoOutput_mus_S2);
            if (servoOutput_mus_S1 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
                servoOutput_mus_S1 += 100;
            }
            if (servoOutput_mus_S2 <= Ts_pwm_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
                servoOutput_mus_S2 += 100;
            }
            servo_counter++;

            /* read ultra sonic distance sensor */
            dist_USSensor = rangeFinder.read_cm();

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

        } else {

            dist_IRSensor = 0.0f;

            speedController_M1.setDesiredSpeedRPS(0.0f);
            positionController_M2.setDesiredRotation(0.0f, max_speed_rps);
            pwmOut_M3.write(0.5);

            servoOutput_mus_S1 = 0;
            servoOutput_mus_S2 = 0;
            servo_S1.SetPosition(servoOutput_mus_S1);
            servo_S2.SetPosition(servoOutput_mus_S2);
            
            dist_USSensor = 0.0f;

            led = 0;
        }

        /* do only output via serial what's really necessary (this makes your code slow)*/
        printf("%3.3f, %3d, %3d, %3.3f, %3.3f, %3d, %3.3f;\r\n",
               dist_IRSensor,
               servoOutput_mus_S1,
               servoOutput_mus_S2,
               speedController_M1.getSpeedRPS(),
               positionController_M2.getRotation(),
               encoderCounter_M3.read(),
               dist_USSensor);
               
        /* ------------- 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;
    }
}