File content as of revision 15:1ebd2ee7b1c8:

#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"
/*Defines initialisieren*/
#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);
/*Initialisierung outputs*/
DigitalOut  led(LED1);
AnalogOut BatteryLED (PA_5);
AnalogOut WeightLED (PA_4);
/*Initialisierung inputs*/
AnalogIn adc_vbat(ADC_VBAT);
AnalogIn analogIn_2(PA_0);
DigitalIn digitalIn(D5);
//AnalogIn analogIn_1(PA_1);


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


/*Variabeln initialisieren*/
float    movement = 0.0f;
float    batt = 0.0f;
float    weight = 0.0f;
float    rotation = 0.0f;
int situation = FUELSENSOR;
int Fehler = 0;
int sensor_trig = 0;
int timer_1 = 0;
/*Initialisierung motor ¦ entnommen aus PM2_example_PES_board*/
/* 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 */


/*Hauptprogramm*/
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();
       weight = analogIn_2.read() * 5;
        /* ------------- start hacking ------------- -------------*/
 
        movement = 0;
        rotation = positioncontroller_M2.getRotation();
        switch (situation) {
            case FUELSENSOR:

                if(weight < 4) {//Ist Der Füllstand Unter 25%?
                    WeightLED = 1;//Anzeige LED
                    situation = BATTERIE;//Weiter zu Batteriestand
                    if (weight < 3.5) {//Ist der Füllstand unter 10%
                        Fehler = F_FUELSENSOR;//Weitergabe Feler
                        situation = SPERREN;//Gerät sperren

                    }
                } else {
                    WeightLED = 0;//LED AUS
                    situation = BATTERIE;//Weiter zu Batteriestand
                }

                break;
            case BATTERIE:          //Der Batteriestand
                if(batt <3.2) {     //Ist der Batteriesand Unter 25%
                    BatteryLED = 1;//Anzeige LED
                    situation = SENSOR;//Weiter zu Sensor
                    if(batt < 3) {//Ist der Batteriestand unter 10%
                        fehler = F_BATTERIE;//Weitergabe Fehler
                        situation = FEHLER;//Gerät sperren
                    }
                } else {
                    BatteryLED = 0;//LED AUS
                    situation = SENSOR;//Weiter zu Sensor
                }
                break;
            case SENSOR:                //Sensor
                if (sensor_trig == 1) {//Wurde der Bewegungsensor Betätigt
                    situation = MOTOR;//Weiter zu Motor
                    sensor_trig = 0;
                } else {
                    situation = FUELSENSOR;//Weiter zu Füllsensor
                }
                break;
            case MOTOR:                 //Motor
                if(timer_1 > 200) {     //Ist genug zeit vergangen das jemand seine Hand wegnemmen konte(hier 2 sek)
                    enable_motors = 1;  //motor ein
                    positioncontroller_M2.setDesiredRotation(1.0f);
                    if (timer_1 ==500) {//Hat der motor lange genug gedreht?
                        positioncontroller_M2.setDesiredRotation(0.0f);
                        enable_motors = 0;//Motor aus
                        timer_1 = 0;
                        situation = FUELSENSOR;//weiter zu Füllsensor
                    } else {
                        timer_1 ++;     //warten
                    }

                } else {
                    timer_1 ++;             //warten
                }
                break;
            /*Gerät Sperren*/
            case SPERREN:
                if (Fehler == F_FUELSENSOR) { //Liegt der Fehler beim Füllsensor?
                    led_blinken(F_FUELSENSOR);//Anzeige LED Blinken
                    if(weight > 4.2) {         //Liegt der Füllstand über 30%
                        WeightLED = 0;         //LED aus
                        situation = BATTERIE;   //Weiter Batteriestand
                    }
                } else {
                    led_blinken(F_BATTERIE);//Anzeige LED Blinken
                    if(batt >3.2) {
                        BatteryLED = 0;     //LED aus
                        situation = SENSOR;//Weiter zu 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);

        /* ------------- 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 != MOTOR && situation != SPERREN) {
        sensor_trig = 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;
    }
}