File content as of revision 20:7e7325edcf5c:

#include "mbed.h"
#include "PM2_Libary.h"

InterruptIn user_button(PC_13);
DigitalOut  led(LED1);


bool  do_execute_main = false;
Timer user_button_timer, loop_timer;
int   loop_period_ms = 50;


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


// SHARP GP2Y0A21Y IR Sensor
/* create analog input object */
AnalogIn analog_in(PC_2);
float    dist_ir_sensor = 0.0f;


// 78:1 Metal Gearmotor 20Dx43L mm 12V CB
/* create enable dc motor digital out object */
DigitalOut enable_motors(PB_15);

/* create pwm objects */
FastPWM pwm_M1(PB_13);
FastPWM pwm_M2(PA_9);
FastPWM pwm_M3(PA_10);
float   pwm_period_s = 0.00005f;

/* create encoder read objects */
EncoderCounter  encoder_M1(PA_6, PC_7);
EncoderCounter  encoder_M2(PB_6, PB_7);
EncoderCounter  encoder_M3(PA_0, PA_1);

/* create speed and position controller objects, default parametrization is for 78.125:1 gear */
float max_voltage = 12.0f;               // adjust this to 6.0f if only one batterypack is used
float counts_per_turn = 20.0f * 78.125f; // counts/turn * gearratio
float kn = 180.0f / 12.0f;               // motor constant (RPM/V)

float k_gear = 25.0f / 78.125f; // in case you are using a dc motor with a different gear box
float kp = 0.1f;                // this is the default speed controller gain for gear box 78.125:1

SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2); // default 78.125:1 gear with default contoller parameters
// SpeedController speedController_M2(counts_per_turn * k_gear, kn / k_gear, max_voltage, pwm_M2, encoder_M2); // parameters adjusted to 25:1 gear

PositionController positionController_M3(counts_per_turn, kn, max_voltage, pwm_M3, encoder_M3); // default 78.125:1 gear with default contoller parameters
// PositionController positionController_M3(counts_per_turn * k_gear, kn / k_gear, kp * k_gear, max_voltage, pwm_M3, encoder_M3); // parameters adjusted to 25:1 gear, we need a different speed controller gain here
float max_speed_rps = 0.5f;              // has to be smaller or equal to kn * max_voltage


// Futaba Servo S3001 20mm 3kg Analog
/* 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 servo_position_S1_mus = 0;
int servo_position_S2_mus = 0;
int servo_period_mus = 20000;
int servo_counter = 0;
int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)loop_period_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 range_finder(PC_6, 5782.0f, 0.02f, 17500); // 1/loop_period_ms = 20 Hz parametrization
RangeFinder range_finder(PB_6, 5782.0f, 0.02f,  7000); // 1/loop_period_ms = 50 Hz parametrization
float       dist_us_sensor = 0.0f;


// LSM9DS1 IMU
// create imu comunication objects, carefull: not all PES boards have an imu (chip shortage)
LSM9DS1 imu(PC_9, PA_8);

int main()
{    
    /* attach button fall and rise functions */
    user_button.fall(&button_fall);
    user_button.rise(&button_rise);

    /* start loop_timer */
    loop_timer.start();

    /* enable hardwaredriver dc motors: 0 -> disabled, 1 -> enabled */
    enable_motors = 1;

    /* initialize pwm for motor M1*/
    pwm_M1.period(pwm_period_s);
    /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max , 0.5 -> 0 V */
    pwm_M1.write(0.5);

    /* enable servos, you can also disable them */
    servo_S1.Enable(servo_position_S1_mus, servo_period_mus);
    servo_S2.Enable(servo_position_S2_mus, servo_period_mus);

    while (true) {

        loop_timer.reset();

        if (do_execute_main) {

            /* read analog input */
            dist_ir_sensor = analog_in.read() * 3.3f;

            /* write output voltage to motor M1 */
            pwm_M1.write(0.75);

            /* set a desired speed for speed controlled dc motors M2 */
            speedController_M2.setDesiredSpeedRPS(0.5f);

            /* set a desired rotation for position controlled dc motors M3 */
            positionController_M3.setDesiredRotation(1.5f, max_speed_rps);


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

            /* read ultra sonic distance sensor */
            dist_us_sensor = range_finder.read_cm();

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

        } else {

            dist_ir_sensor = 0.0f;

            pwm_M1.write(0.5);
            speedController_M2.setDesiredSpeedRPS(0.0f);
            positionController_M3.setDesiredRotation(0.0f, max_speed_rps);

            servo_position_S1_mus = 0;
            servo_position_S2_mus = 0;
            servo_S1.SetPosition(servo_position_S1_mus);
            servo_S2.SetPosition(servo_position_S2_mus);

            dist_us_sensor = 0.0f;

            led = 0;
        }

        /* do only output via serial what's really necessary (this makes your code slow)*/
        /*
        printf("%3.3f, %3d, %3.3f, %3.3f, %3d, %3d, %3.3f;\r\n",
               dist_ir_sensor * 1e3,
               encoder_M1.read(),
               speedController_M2.getSpeedRPS(),
               positionController_M3.getRotation(),
               servo_position_S1_mus,
               servo_position_S2_mus,
               dist_us_sensor);
        */

        imu.updateGyro();
        imu.updateAcc();
        imu.updateMag();
        printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f\r\n", imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(),
        imu.readAccX(), imu.readAccY(), imu.readAccZ(), imu.readMagX(), imu.readMagY(), imu.readMagZ());

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

        int loop_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(loop_timer.elapsed_time()).count();
        thread_sleep_for(loop_period_ms - loop_time_ms);
    }
}

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

void button_rise()
{
    int user_button_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(user_button_timer.elapsed_time()).count();
    user_button_timer.stop();
    if (user_button_time_ms > 200) {
        do_execute_main = !do_execute_main;
    }
}