File content as of revision 39:e4f5710b2f31:

#include "mbed.h"
#include "Queue.h"
#include "TextLCD.h"
#include "pid.h"
#include <Serial.h>

//definitions
typedef struct {
    uint32_t input;
} message_t;

const int heightResolution = 1024;
const int BAUDRATE = 115200;
const double setPoint = 0.5;
const double Kp = 1;
const double Ki = 0.2;
const double Kd = 0.3;

enum {btnRIGHT, btnUP, btnDOWN, btnLEFT, btnSELECT, btnNONE};

//Pins
PwmOut          outPin(D3);
DigitalOut      led2(LED2);
DigitalOut      led3(LED3);
AnalogIn        buttons(A0);

//Threads
Thread blueThread;
Thread pidThread;
Thread communicationThread;
Thread hmiThread;

//Global variables
TextLCD lcd(D8, D9, D4, D5, D6, D7); // rs, e, d4-d7
Pid* pidController;
MemoryPool<message_t, 16> mpool;
Queue<message_t,16> messageQueue;

void SerialCallback(int);

//=============================Thread Methodes==================================

void BlueMethode(void)
{
    while(true)
    {
        led3 = !led3;
        Thread::wait(500);
    }
}

//------------------------------------------------------------------------------

void PidMethode(void)
{
    while(true)
    {
        osEvent ev = messageQueue.get(osWaitForever); 
        if (ev.status == osEventMessage)
        {
            message_t *message = (message_t*)ev.value.p;
            
            double input = double(message->input) / heightResolution;
            
            double newPwm = pidController->getPwm(input);
            
            outPin.write(newPwm);
            
            mpool.free(message);
            
            lcd.cls();
            lcd.printf("set   in    out");
            lcd.locate(0,1);
            lcd.printf("%d", int(1000*pidController->getSetPoint()));
            lcd.locate(6,1);
            lcd.printf("%d", int(1000*input));
            lcd.locate(12,1);
            lcd.printf("%d", int(1000*newPwm));
        }
    }   
}
//------------------------------------------------------------------------------

void CommunicationMethode(void)
{
    Serial serial(USBTX, USBRX);
    serial.baud(BAUDRATE);
    serial.format(8, SerialBase::None, 1);
    uint16_t input = 0;
    
    //event_callback_t functionpointer;
    //functionpointer.attach(&SerialCallback);
    
    while(true)
    {
        //serial.read(&input, 1, functionpointer);
        input = serial.getc()+(serial.getc()<<8);
        //lcd.cls();
        //lcd.printf("%d", int(input));
        message_t *message = mpool.alloc();
        message->input = input;
        messageQueue.put(message);
    }   
}
/*
void SerialCallback(int)
{
   //ut = 
}*/

//------------------------------------------------------------------------------
//human-machine interface
void hmiMethode(void)
{
    while(true)
    {
        double buttonsValue = buttons.read();
        
        unsigned button = btnNONE;
        if(buttonsValue < 0.08) //0.000
            button = btnRIGHT;
        else if(buttonsValue < 0.28) //0.170
            button = btnUP;
        else if(buttonsValue < 0.51) //0.397
            button = btnDOWN;
        else if(buttonsValue < 0.78) //0.621
            button = btnLEFT;
        else if(buttonsValue < 0.97) //0.936
            button = btnSELECT;
        else                         //1.000
            button = btnNONE;
            
        
        double prevSetPoint = pidController->getSetPoint();
        double newSetPoint = prevSetPoint;
        if(button == btnUP)
            newSetPoint += 0.001;
        else if(button == btnDOWN)
            newSetPoint -= 0.001;
        else if(button == btnLEFT)
            newSetPoint -= 0.01;
        else if(button == btnRIGHT)
            newSetPoint += 0.01;
        else if(button == btnSELECT)
            newSetPoint = 0.5;
            
        pidController->setSetPoint(newSetPoint);
        
        Thread::wait(100);
    }   
}

//=============================Main Thread======================================
// main() runs in its own thread in the OS

int main() {
    
    //led1.period(4.0f);      // 4 second period
    
    lcd.cls();
    lcd.printf("HELLO");
    
    pidController = new Pid(Kp, Ki, Kd);
    pidController->setSetPoint(setPoint);
    
    blueThread.start(BlueMethode);
    pidThread.start(PidMethode);
    communicationThread.start(CommunicationMethode);
    hmiThread.start(hmiMethode);
    
    while (true) {
        led2 = !led2;
        Thread::wait(1100);
    }
}

//==============================================================================