File content as of revision 5:afe2339723f6:

#include "mbed.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "HCSR04.h"
#include "Servo.h"
#include "PID.h"
#include <string>
#include <iostream>
#include <sstream>

//#define SERIAL
#define ETHERNET

#define SAMPLE_RATE 10 //sample rate in miliseconds

enum status { IDLE, ADJUSTING, STABLE };

status statusFlag; //flag to determine behavior

DigitalOut ledR(LED1);
DigitalOut ledG(LED2);
DigitalOut ledB(LED3);

HCSR04 ultrassonicSensor(PTC2, PTC3);
Servo motor(PTA2);
float motorPos = 0;

//Kc, Ti, Td, interval
PID controller(1.0, 0.0, 0.0, SAMPLE_RATE);

InterruptIn sw2(SW2);
void sw2Callback()
{
    if(motorPos<1)
        motorPos+=(float)0.1;
}
InterruptIn sw3(SW3);
void sw3Callback()
{
    if(motorPos>0)
        motorPos-=(float)0.1;
}

Thread ledSwitchThread;
Thread serialOutThread;
Thread controlSystemThread;

#ifdef ETHERNET
Thread ethernetSendThread;
Thread ethernetReceiveThread;
Thread ethernetKeepAliveThread;
#endif

float ballDistance = 0.0;
float setpoint = 15.0;

#ifdef ETHERNET

EthernetInterface eth;
TCPSocketConnection sock;

void ethernetKeepAlive()
{
#ifdef SERIAL
    printf("ethernetKeepAliveThread started\n");
#endif
    std::stringstream ss;

    while(true) {
        if(sock.is_connected()) {
            sock.send_all(NULL,0);
        } else {
            sock.connect("192.168.1.1", 12345);
        }
        Thread::wait(100);
    }
}

void ethernetSend()
{
#ifdef SERIAL
    printf("ethernetSendThread started\n");
#endif
    std::stringstream ss;

    while(true) {
        if(sock.is_connected()) {
            ss.flush();
            ss << "Ball distance: " << ballDistance << "cm\n";
            ss << "Setpoint: " << setpoint << "cm\n";
            switch(statusFlag) {
                case IDLE:
                    ss << "System is idle\n";
                    break;
                case ADJUSTING:
                    ss << "System is adjusting\n";
                    break;
                case STABLE:
                    ss << "System is stable\n";
                    break;
                default:
                    break;
            }
            sock.send_all((char*)ss.str().data(),ss.str().size());
        } else {
            sock.connect("192.168.1.1", 12345);


        }
        Thread::wait(1000);
    }
}

void ethernetReceive()
{
#ifdef SERIAL
    printf("ethernetReceiveThread started\n");
#endif
    char buffer[10];
    int ret;
    while(true) {
        if(sock.is_connected()) {
            

            ret = sock.receive(buffer, sizeof(buffer)-1);
#ifdef SERIAL
            buffer[ret] = '\0';
            printf("Received %d chars from server:\n%s\n", ret, buffer);
#endif

            switch(ret) {
                default:
                    break;
                case 1:
                    setpoint = (buffer[0]-'0');
                    break;
                case 2:
                    setpoint = (buffer[0]-'0')*10 + buffer[1]-'0';
                    break;
            }


        } else {
            sock.connect("192.168.1.1", 12345);
        }
        //Thread::wait(100);

    }
}
#endif

void ledSwitch()
{
#ifdef SERIAL
    printf("ledSwitch thread started\n");
#endif
    while (true) {
        switch(statusFlag) {
            case IDLE:
                ledR = 1;
                ledG = 1;
                ledB = !ledB;
                Thread::wait(500);
                break;
            case ADJUSTING:
                ledR = !ledR;
                ledG = 1;
                ledB = 1;
                Thread::wait(200);
                break;
            case STABLE:
                ledR = 1;
                ledG = !ledG;
                ledB = 1;
                Thread::wait(1000);
                break;
            default:
                break;
        }

    }
}

void serialOut()
{
#ifdef SERIAL
    printf("SerialOut thread started\n");
    while(true) {
        printf("Ball distance: %fcm\n",ballDistance);
        printf("Setpoint: %fcm\n",setpoint);
        switch(statusFlag) {
            case IDLE:
                printf("System is idle\n");
                break;
            case ADJUSTING:
                printf("System is adjusting\n");
                break;
            case STABLE:
                printf("System is stable\n");
                break;
            default:
                break;
        }
        Thread::wait(500);
    }
#endif
}

void controlSystem()
{
#ifdef SERIAL
    printf("controlSystem thread started\n");
#endif
    while(true) {
        ballDistance = ultrassonicSensor.distance(CM);

        if (ballDistance != setpoint) {
            statusFlag = ADJUSTING;
        } else {
            statusFlag = STABLE;
        }

        //PID CONTROLLER
        //motor.write(motorPos);
        controller.setProcessValue(ballDistance);
        motor.write(controller.compute());


        Thread::wait(SAMPLE_RATE);
    }
}


int main()
{
    statusFlag = IDLE;

#ifdef SERIAL
    printf("BALL AND BEAM\n");
    printf("APS de Sistemas Operacionais / Controle 2\n");
    printf("Alunos: Felipe, Juliana, Rafael\n");
#endif
    //Analog input from 0.0 to 50.0 cm
    controller.setInputLimits(0.0, 50.0);
    //Pwm output from 0.0 to 1.0 (servo)
    controller.setOutputLimits(0.0, 1.0);
    //If there's a bias.
    //controller.setBias(0.3);
    controller.setMode(AUTO_MODE);
    //We want the process variable to be 15cm (default)
    controller.setSetPoint(setpoint);

    sw2.rise(&sw2Callback);
    sw3.rise(&sw3Callback);
    ledSwitchThread.start(ledSwitch);
    #ifdef SERIAL
    serialOutThread.start(serialOut);
    #endif 
    controlSystemThread.start(controlSystem);

#ifdef ETHERNET
    eth.init("192.168.1.2","255.255.255.0","192.168.1.1");
    eth.connect();
    sock.connect("192.168.1.1", 12345);
    sock.set_blocking(0);
#ifdef SERIAL
    printf("IP Address is %s\n", eth.getIPAddress());
#endif
    
    ethernetSendThread.start(ethernetSend);
    ethernetReceiveThread.start(ethernetReceive);
    ethernetKeepAliveThread.start(ethernetKeepAlive);
#endif

    while(true) {
        //nothing
    }




}