File content as of revision 3:3d094a31a283:

#include "mbed.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "HCSR04.h"
#include "Servo.h"
#include "PID.h"

#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+=0.1;
}
InterruptIn sw3(SW3);
void sw3Callback()
{
    if(motorPos>0)
        motorPos-=0.1;
}

Thread ledSwitchThread;
Thread serialOutThread;
Thread controlSystemThread;

float distance = 0.0;
float setpoint = 15.0;

void ledSwitch()
{
#ifdef SERIAL
    printf("ledSwitch thread started");
#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");
    while(true) {
        printf("Ball distance: %fcm\n",distance);
        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");
#endif
    while(true) {
        distance = ultrassonicSensor.distance(CM);

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

        //PID CONTROLLER
        //motor.write(motorPos);
        controller.setProcessValue(distance);
        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);
    serialOutThread.start(serialOut);
    controlSystemThread.start(controlSystem);

    while(true) {
        //nothing
    }




}