File content as of revision 40:19d51f6e6800:

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

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

const int heightResolution = 1000;
const int BAUDRATE = 115200;
const int cameraFPS = 60;
const int displayRefreshTime = 200;
const double setPoint = 0.5;
const double Kp = 0.02;
const double Ki = 0;
const double Kd = 0;

const int simulationSeconds = 10;

enum {btnRIGHT, btnUP, btnDOWN, btnLEFT, btnSELECT, btnNONE, btnSIZE};
enum {menuSETPOINT, menuPID, menuSIMULATE, menuSIZE};
enum {parameterKP, parameterKI, parameterKD, parameterSIZE};
enum {simulSTEP, simulRAMP, simulNONE, simulSIZE};

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

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

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

//Menu settings
int menu = menuSETPOINT;
int parameter = parameterKP;
int simul = simulNONE;

void SerialCallback(int);

//=============================Thread Methodes==================================
void setPointMethode(void)
{
    while(1)
    {
        pidController->setSetPoint(0.2);
        Thread::wait(20000);   
        pidController->setSetPoint(0.8);
        Thread::wait(20000);   
    }
}
void BlueMethode(void)
{
    while(true)
    {
        led3 = !led3;
        
        double lastPwm = pidController->getLastPwm();
        double lastInput = pidController->getLastInput();
        
        if(menu == menuSETPOINT)
        {
            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*lastInput));
            lcd.locate(12,1);
            lcd.printf("%d", int(1000*lastPwm));
        }
        else if(menu == menuPID)
        {
            lcd.cls();
            lcd.printf(" Kp    Ki    Kd");
            lcd.locate(6*parameter, 0);
            lcd.printf("*");
            lcd.locate(0,1);
            lcd.printf("%d", int(1000*pidController->getKp()));
            lcd.locate(6,1);
            lcd.printf("%d", int(1000*pidController->getKi()));
            lcd.locate(12,1);
            lcd.printf("%d", int(1000*pidController->getKd()));
        }
        else if(menu == menuSIMULATE)
        {
            lcd.cls();
            if(simul == simulNONE)
                lcd.printf("u:STEP dwn:RAMP");
            else
                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*lastInput));
            lcd.locate(12,1);
            lcd.printf("%d", int(1000*lastPwm));
        }
        Thread::wait(displayRefreshTime);
    }
}

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

void PidMethode(void)
{
    while(true)
    {
        osEvent ev = messageQueue.get(osWaitForever); 
        if (ev.status == osEventMessage)
        {
            message_t *message = (message_t*)ev.value.p;
            
            double input = message->input;
            
            double newPwm = pidController->getPwm(input);
            
            outPin.write(newPwm);
            
            mpool.free(message);
        }
    }   
}
//------------------------------------------------------------------------------

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

unsigned readButtons()
{
        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;
            
        return button;
    
}
//------------------------------------------------------------------------------
//human-machine interface
void hmiMethode(void)
{
    
    unsigned button = btnNONE;
    while(true)
    {
        
        //while(button == btnNONE)
        //{
            button = readButtons();
          //  Thread::wait(10);
        //}
               
        double prevSetPoint = pidController->getSetPoint();
        double newSetPoint = prevSetPoint;
        
        if(button == btnSELECT)
        {
            menu = (menu+1) % menuSIZE;
            simul = simulNONE;
        }
        else if(menu == menuSETPOINT)
        {
            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;
                
            pidController->setSetPoint(newSetPoint);
        }
        else if(menu == menuPID)
        {
            if(button == btnUP)
            {
                if(parameter == parameterKP)
                    pidController->addKp(0.001);
                else if(parameter == parameterKI)
                    pidController->addKi(0.001);
                else if(parameter == parameterKD)
                    pidController->addKd(0.001);
            }
            else if(button == btnDOWN)
            {
                if(parameter == parameterKP)
                    pidController->addKp(-0.001);
                else if(parameter == parameterKI)
                    pidController->addKi(-0.001);
                else if(parameter == parameterKD)
                    pidController->addKd(-0.001);
            }
            else if(button == btnLEFT)
                parameter = (parameter+1) % parameterSIZE;
            else if(button == btnRIGHT)
                pidController->setParameters(Kp, Ki, Kd);
        }
        
        else if(menu == menuSIMULATE)
        {
            if(button == btnUP) //step
            {
                simul = simulSTEP;
                for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
                {
                    double input =  i < cameraFPS * simulationSeconds / 2 ? 0 : 1;
                    message_t *message = mpool.alloc();
                    message->input = input;
                    messageQueue.put(message);
                    
                    Thread::wait(1000/cameraFPS);    
                }
                simul = simulNONE;
            }
            else if(button == btnDOWN) //ramp
            {
                simul = simulRAMP;
                for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
                {
                    double input =  double(i) / ((cameraFPS * simulationSeconds)-1);
                    message_t *message = mpool.alloc();
                    message->input = input;
                    messageQueue.put(message);
                    
                    Thread::wait(1000/cameraFPS);    
                }
                simul = simulNONE;
            }
            else if(button == btnLEFT) //ramp
            {
                setPointThread.start(setPointMethode);
            }
            else if(button == btnRIGHT)
                setPointThread.terminate();
            
        }
        
        /*while(button != btnNONE)
        {
            button = readButtons();
            Thread::wait(10);
        }*/
        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) {
        Thread::wait(1100);
    }
}

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