File content as of revision 7:a1e648308011:

#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 BIAS 0.5
#define ETHERNET

#define SAMPLE_RATE 1 //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;
int isRunning = 0;

//Kc, Ti, Td, interval
//PID controller(0.3461, 21.42, 5.26, 0.001);
//PID controller(1.5, 0, 2, 0.001);
//PID controller(-1.1, 0, 1.7, 0.001);//ok
PID controller(-0.9344, 0, 1.733, 0.001);

InterruptIn sw2(SW2);
void sw2Callback()
{
    motorPos=0+BIAS;
    isRunning = !isRunning;
}
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;

#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("",1);
        } else {
            sock.connect("192.168.1.1", 12345);
        }
        Thread::wait(1000);
    }
}

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(5000);
    }
}

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(1000);

    }
}
#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)+2.5;
        
        if (ballDistance > 37.5)
            ballDistance = 35;
        if (ballDistance <0)    
            ballDistance = 2.5;
            
            
        controller.setProcessValue(ballDistance);

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

        //PID CONTROLLER
        //motor.write(motorPos);
        //controller.setProcessValue(ballDistance);
        
        controller.setSetPoint(setpoint);
        if(isRunning)
            motorPos = controller.compute();
       
       #ifdef SERIAL
        //printf("Motor position: %f\n",motorPos);
        #endif           
        motor = 1-motorPos;


        Thread::wait(SAMPLE_RATE);
    }
}


int main()
{
    
    
    Servo myservo(PTA2);
/*
while(1) {    
    for(float p=-90; p<=90; p += 45) {
        myservo.position(180-p+BIAS);
        wait(1);
    }
}
*/
    motor = 0.5;;
    wait(1);
    
    
    statusFlag = IDLE;

#ifdef SERIAL
    printf("BALL AND BEAM\n");
    printf("APS de Sistemas Operacionais / Controle 2\n");
    printf("Alunos: Felipe, Juliana, Rafael\n");
#endif
    // input from 0.0 to 35 cm
    controller.setInputLimits(-1, 50.0);
    //Pwm output from 0.0 to 1.0 (servo)
    controller.setOutputLimits(0, 1);
    //If there's a bias.
    controller.setBias(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
    }




}