File content as of revision 12:b87a1e165fb8:

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

/* PM2_Libary */
#include "EncoderCounter.h"
#include "Servo.h"
#include "SpeedController.h"
#include "FastPWM.h"
#define AUFLOESUNG 0.25412f
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 adc_vbat(ADC_VBAT);
AnalogIn analogIn(PC_2);
float    dist = 0.0f;
float    batt = 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);




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


    while (true) {
        batt = adc_vbat.read()* (3.3f/AUFLOESUNG);
        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( 2.0f);
 //           speedController_M2.setDesiredSpeedRPS(-0.5f);
            /* write output voltage to motor M3 */
            pwmOut_M3.write(0.75);

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

           

            led = 0;
        }

        /* do only output via serial what's really necessary (this makes your code slow)*/
        printf("%3.3f, %3.3f\n, %3d, %3.3f, %3.3f;\r\n",
               dist,
               batt,
               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;
    }
}