File content as of revision 10:c5d85e35758c:

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

/* PM2_Libary */
#include "EncoderCounter.h"
#include "Servo.h"
#include "SpeedController.h"
#include "FastPWM.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 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);
SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);

/* create servo objects */
Servo servo_S1(PB_2);
Servo servo_S2(PC_8);
// Servo servo_S3(PC_6); // not needed in this example
int servoPeriod_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)));

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, servoPeriod_mus);
    servo_S2.Enable(servoOutput_mus_S2, servoPeriod_mus);

    while (true) {

        loop_timer.reset();

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

        if (executeMainTask) {

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

            /* command a speed to dc motors M1 and M2*/
            speedController_M1.setDesiredSpeedRPS( 1.0f);
            speedController_M2.setDesiredSpeedRPS(-0.5f);
            /* 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 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
                servoOutput_mus_S1 += 100;
            }
            if (servoOutput_mus_S2 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
                servoOutput_mus_S2 += 100;
            }
            servo_counter++;

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

        } else {

            dist = 0.0f;

            speedController_M1.setDesiredSpeedRPS(0.0f);
            speedController_M2.setDesiredSpeedRPS(0.0f);
            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);

            led = 0;
        }

        /* do only output via serial what's really necessary (this makes your code slow)*/
        printf("%3.3f, %3d, %3d, %3d, %3.3f, %3.3f;\r\n",
               dist,
               servoOutput_mus_S1,
               servoOutput_mus_S2,
               encoderCounter_M3.read(),
               speedController_M1.getSpeedRPS(),
               speedController_M2.getSpeedRPS());

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