File content as of revision 14:cfe1311a63f0:

#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"

#define AUFLOESUNG 0.25412f
#define FUELSENSOR 1
#define BATTERIE 2
#define SENSOR 3
#define MOTOR 4
#define SPERREN 5
#define F_FUELSENSOR 1
#define F_BATTERIE 2

using namespace std::chrono;

InterruptIn user_button(USER_BUTTON);
InterruptIn sensor(PA_1);

//AnalogIn analogIn_1(PA_1);

DigitalOut  led(LED1);

bool  executeMainTask = false;
Timer user_button_timer, loop_timer;


/* declaration of custom button functions */
void button_fall();
void button_rise();
void sensor_fall();
void led_blinken(int fehler);
/* create analog input object */
AnalogIn adc_vbat(ADC_VBAT);
DigitalIn digitalIn(D5);

AnalogIn analogIn_2(PA_0);

float    movement = 0.0f;
float    batt = 0.0f;
float    weight = 0.0f;
float    rotation = 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_M2(PB_6, PB_7);
/* 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_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
PositionController positioncontroller_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);

/* analog output */
AnalogOut BatteryLED (PA_5);
AnalogOut WeightLED (PA_4);
int situation = FUELSENSOR;
int Fehler = 0;
int sensor_trig = 0;
int timer_1 = 0;

int main()
{

    user_button.fall(&button_fall);
    user_button.rise(&button_rise);
    sensor.fall(&sensor_fall);

    loop_timer.start();

    /* enable hardwaredriver dc motors */
    enable_motors = 1;


    float movementold = 0;
    while (true) {

        batt = adc_vbat.read()* (3.3f/AUFLOESUNG);
        loop_timer.reset();

        /* ------------- start hacking ------------- -------------*/
        weight = analogIn_2.read() * 5;
        movement = 0;
//roundf(analogIn_1.read()*1.2);
        rotation = positioncontroller_M2.getRotation();
        switch (situation) {
            case FUELSENSOR:

                if(weight < 4) {
                    WeightLED = 1;
                    situation = BATTERIE;
                    if (weight < 3.5) {
                        Fehler = F_FUELSENSOR;
                        situation = SPERREN;

                    }
                } else {
                    WeightLED = 0;
                    situation = BATTERIE;
                }

                break;
            case BATTERIE:
                if(batt <3.2) {
                    BatteryLED = 1;
                    situation = SENSOR;
                    if(batt < 3) {
                        //fehler = F_BATTERIE;
                        //situation = FEHLER;
                    }
                } else {
                    BatteryLED = 0;
                    situation = SENSOR;
                }
                break;
            case SENSOR:
                if (sensor_trig == 1) {
                    situation = MOTOR;
                     sensor_trig = 0;
                } else {
                    situation = FUELSENSOR;
                }
                break;
            case MOTOR:
                if(timer_1 > 200) {
                    enable_motors = 1;
                    positioncontroller_M2.setDesiredRotation(1.0f);
                    if (timer_1 ==500) {
                        positioncontroller_M2.setDesiredRotation(0.0f);
                        enable_motors = 0;
                        timer_1 = 0;
                        situation = FUELSENSOR;
                    } else {
                        timer_1 ++;
                    }

                } else {
                    timer_1 ++;
                }
                break;
            case SPERREN:
                if (Fehler == F_FUELSENSOR) {
                    led_blinken(F_FUELSENSOR);
                    if(weight > 4.2) {
                        WeightLED = 0;
                        situation = BATTERIE;
                    }
                } else {
                    led_blinken(F_BATTERIE);
                    if(batt >3.2) {
                        BatteryLED = 0;
                        situation = SENSOR;
                    }
                }
                break;



        }

        led = !led;

        /* do only output via serial what's really necessary (this makes your code slow)*/
        printf("%3.3f\n, %3.3f\n, %3.3f\n, %3d\n,%3d\n ",
               movement,
               weight,
               batt,
               timer_1,
               motor);

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

    }
}

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

void sensor_fall()
{
    if (situation == SENSOR) {
        motor = 1;
    }


}
void led_blinken(int fehler)
{
    static int timer = 0;

    if (fehler == F_FUELSENSOR) {
        if (timer == 50) {
            timer = 0;

            WeightLED = !WeightLED;
        }
    } else {
        if (timer == 50) {

            BatteryLED = !BatteryLED;
            timer = 0;
        }
    }
    timer ++;
}
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;
    }
}