APS de Sistemas Operacionais / Controle 2 FINAL
Dependencies: EthernetInterface HCSR04 PID Servo mbed-rtos mbed
Fork of aps_so_c2_old by
main.cpp
- Committer:
- feupos
- Date:
- 2018-01-14
- Revision:
- 7:a1e648308011
- Parent:
- 6:ee9361616596
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 } }