File content as of revision 43:5123f24e0b2c:

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

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

bool useUDP = false;
const int heightResolution = 1000;
const int BAUDRATE = 115200;
const char* host_address = "10.1.1.101";
const int host_port = 7;
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 double configStep = 0.001;
const double configFastStep = configStep*10;

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      led1(LED1);
//DigitalOut      led2(LED2);
DigitalOut      led3(LED3);
AnalogIn        buttons(A0);

//Threads
Thread lcdThread;
Thread pidThread;
Thread serialThread;
Thread hmiThread;
Thread setPointThread;
Thread udpThread;

//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;
bool setPointSimul = false;


// Network interface
EthernetInterface net;

//=============================Thread Methodes==================================
void setPointMethode(void)
{
    while(1) //toggle every 20 seconds between 2 setPoint values
    {
        while(!setPointSimul) Thread::wait(2000);
            
        if(setPointSimul)
        {
            pidController->setSetPoint(0.2);
            Thread::wait(20000);
            if(setPointSimul)
            {
                pidController->setSetPoint(0.8);
                Thread::wait(20000);  
            }
        } 
    }
}
void LcdMethode(void)
{
    while(true)
    {
        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)
            {
                if(setPointSimul)
                    lcd.printf("u:STEP * d:RAMP");
                else
                    lcd.printf("u:STEP _ d: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 SerialMethode(void)
{
    Serial serial(USBTX, USBRX);
    serial.baud(BAUDRATE);
    serial.format(8, SerialBase::None, 1);
    double input = 0;
    
    while(true)
    {
        input = double(serial.getc()+(serial.getc()<<8)) / heightResolution;
        message_t *message = mpool.alloc();
        message->input = input;
        messageQueue.put(message);
    }   
}
//------------------------------------------------------------------------------

void UdpMethode(void)
{
    UDPSocket socket(&net);
    socket.bind(host_port);
    SocketAddress socketAddress;
    socket.set_blocking(true);
    uint16_t packet = 0;
    double input = 0;
    while(true)
    {
        //socket.sendto(host_address, host_port, (const void*)input, sizeof(input));
        //input += 1;
        socket.recvfrom(&socketAddress, &packet, sizeof(packet));
        
        input = double(packet) / heightResolution;
        message_t *message = mpool.alloc();
        message->input = input;
        messageQueue.put(message);
        Thread::wait(300);
    }   
}

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

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;
    bool fastChange = false;
    while(true)
    {
        
        button = readButtons();
        while(button == btnNONE)
        {
            button = readButtons();
            Thread::wait(10);
            fastChange = false;
        }
               
        double prevSetPoint = pidController->getSetPoint();
        double newSetPoint = prevSetPoint;
        
        if(button == btnSELECT)
        {
            menu = (menu+1) % menuSIZE;
            simul = simulNONE;
        }
        else if(menu == menuSETPOINT)
        {
            if(button == btnUP)
                newSetPoint += fastChange ? configFastStep : configStep;
            else if(button == btnDOWN)
                newSetPoint -= fastChange ? configFastStep : configStep;
            else if(button == btnLEFT)
                newSetPoint -= fastChange ? 10*configFastStep : 10*configStep;
            else if(button == btnRIGHT)
                newSetPoint += fastChange ? 10*configFastStep : 10*configStep;
                
            pidController->setSetPoint(newSetPoint);
        }
        else if(menu == menuPID)
        {
            if(button == btnUP)
            {
                if(parameter == parameterKP)
                    pidController->addKp(fastChange ? configFastStep : configStep);
                else if(parameter == parameterKI)
                    pidController->addKi(fastChange ? configFastStep : configStep);
                else if(parameter == parameterKD)
                    pidController->addKd(fastChange ? configFastStep : configStep);
            }
            else if(button == btnDOWN)
            {
                if(parameter == parameterKP)
                    pidController->addKp(fastChange ? -configFastStep : -configStep);
                else if(parameter == parameterKI)
                    pidController->addKi(fastChange ? -configFastStep : -configStep);
                else if(parameter == parameterKD)
                    pidController->addKd(fastChange ? -configFastStep : -configStep);
            }
            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)
                setPointSimul = true;
            else if(button == btnRIGHT)
                setPointSimul = false;
            
        }
        unsigned repeatCount = 0;
        while(button != btnNONE && !fastChange)
        {
            repeatCount++;
            button = readButtons();
            Thread::wait(10);
            if(repeatCount > 50)
                fastChange = true;
        }
        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");
    
    Thread::wait(2000);
    if(readButtons() == btnDOWN)
        useUDP = true;
    
    if(useUDP)
    {
        lcd.cls();
        lcd.printf("Connecting...");
        net.connect();

        // Show the network address
        const char *ip = net.get_ip_address();
        lcd.cls();
        lcd.printf("%s", ip ? ip : "No IP");
        if(!ip)
            while(1); //terminate program
            
        Thread::wait(5000);
        udpThread.start(UdpMethode);
    }
    else
        serialThread.start(SerialMethode);
    
    pidController = new Pid(Kp, Ki, Kd);
    pidController->setSetPoint(setPoint);
    
    lcdThread.start(LcdMethode);
    pidThread.start(PidMethode);
    hmiThread.start(hmiMethode);
    setPointThread.start(setPointMethode);
    
    while (true) {
        Thread::wait(1000);
        led3 != led3;
    }
}

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