Felipe dos Santos Neves / Mbed 2 deprecated aps_so_c2

APS de Sistemas Operacionais / Controle 2 FINAL

Dependencies:   EthernetInterface HCSR04 PID Servo mbed-rtos mbed

Fork of aps_so_c2_old by Felipe dos Santos Neves

main.cpp@7:a1e648308011, 2018-01-14 (annotated)

Committer:
feupos
Date:
Sun Jan 14 00:38:21 2018 +0000
Revision:
7:a1e648308011
Parent:
6:ee9361616596 
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
feupos 0:8d53c4c11953 1 #include "mbed.h"
feupos 0:8d53c4c11953 2 #include "rtos.h"
feupos 2:f3ba67384801 3 #include "EthernetInterface.h"
feupos 3:3d094a31a283 4 #include "HCSR04.h"
feupos 3:3d094a31a283 5 #include "Servo.h"
feupos 3:3d094a31a283 6 #include "PID.h"
feupos 4:40990500a7cc 7 #include <string>
feupos 4:40990500a7cc 8 #include <iostream>
feupos 4:40990500a7cc 9 #include <sstream>
feupos 2:f3ba67384801 10
feupos 5:afe2339723f6 11 //#define SERIAL
feupos 6:ee9361616596 12 #define BIAS 0.5
feupos 3:3d094a31a283 13 #define ETHERNET
feupos 0:8d53c4c11953 14
feupos 6:ee9361616596 15 #define SAMPLE_RATE 1 //sample rate in miliseconds
feupos 3:3d094a31a283 16
feupos 3:3d094a31a283 17 enum status { IDLE, ADJUSTING, STABLE };
feupos 3:3d094a31a283 18
feupos 3:3d094a31a283 19 status statusFlag; //flag to determine behavior
feupos 2:f3ba67384801 20
feupos 2:f3ba67384801 21 DigitalOut ledR(LED1);
feupos 2:f3ba67384801 22 DigitalOut ledG(LED2);
feupos 2:f3ba67384801 23 DigitalOut ledB(LED3);
feupos 2:f3ba67384801 24
feupos 3:3d094a31a283 25 HCSR04 ultrassonicSensor(PTC2, PTC3);
feupos 3:3d094a31a283 26 Servo motor(PTA2);
feupos 3:3d094a31a283 27 float motorPos = 0;
feupos 6:ee9361616596 28 int isRunning = 0;
feupos 3:3d094a31a283 29
feupos 3:3d094a31a283 30 //Kc, Ti, Td, interval
feupos 6:ee9361616596 31 //PID controller(0.3461, 21.42, 5.26, 0.001);
feupos 6:ee9361616596 32 //PID controller(1.5, 0, 2, 0.001);
feupos 6:ee9361616596 33 //PID controller(-1.1, 0, 1.7, 0.001);//ok
feupos 6:ee9361616596 34 PID controller(-0.9344, 0, 1.733, 0.001);
feupos 0:8d53c4c11953 35
feupos 3:3d094a31a283 36 InterruptIn sw2(SW2);
feupos 3:3d094a31a283 37 void sw2Callback()
feupos 3:3d094a31a283 38 {
feupos 6:ee9361616596 39 motorPos=0+BIAS;
feupos 6:ee9361616596 40 isRunning = !isRunning;
feupos 3:3d094a31a283 41 }
feupos 3:3d094a31a283 42 InterruptIn sw3(SW3);
feupos 3:3d094a31a283 43 void sw3Callback()
feupos 3:3d094a31a283 44 {
feupos 6:ee9361616596 45 /*if(motorPos>0)
feupos 6:ee9361616596 46 motorPos-=(float)0.1;*/
feupos 0:8d53c4c11953 47 }
feupos 0:8d53c4c11953 48
feupos 3:3d094a31a283 49 Thread ledSwitchThread;
feupos 3:3d094a31a283 50 Thread serialOutThread;
feupos 3:3d094a31a283 51 Thread controlSystemThread;
feupos 3:3d094a31a283 52
feupos 4:40990500a7cc 53 #ifdef ETHERNET
feupos 4:40990500a7cc 54 Thread ethernetSendThread;
feupos 4:40990500a7cc 55 Thread ethernetReceiveThread;
feupos 5:afe2339723f6 56 Thread ethernetKeepAliveThread;
feupos 4:40990500a7cc 57 #endif
feupos 4:40990500a7cc 58
feupos 4:40990500a7cc 59 float ballDistance = 0.0;
feupos 6:ee9361616596 60 float setpoint = 15;
feupos 3:3d094a31a283 61
feupos 4:40990500a7cc 62 #ifdef ETHERNET
feupos 4:40990500a7cc 63
feupos 4:40990500a7cc 64 EthernetInterface eth;
feupos 4:40990500a7cc 65 TCPSocketConnection sock;
feupos 4:40990500a7cc 66
feupos 5:afe2339723f6 67 void ethernetKeepAlive()
feupos 4:40990500a7cc 68 {
feupos 4:40990500a7cc 69 #ifdef SERIAL
feupos 5:afe2339723f6 70 printf("ethernetKeepAliveThread started\n");
feupos 4:40990500a7cc 71 #endif
feupos 4:40990500a7cc 72 std::stringstream ss;
feupos 4:40990500a7cc 73
feupos 5:afe2339723f6 74 while(true) {
feupos 5:afe2339723f6 75 if(sock.is_connected()) {
feupos 6:ee9361616596 76 sock.send_all("",1);
feupos 5:afe2339723f6 77 } else {
feupos 5:afe2339723f6 78 sock.connect("192.168.1.1", 12345);
feupos 5:afe2339723f6 79 }
feupos 6:ee9361616596 80 Thread::wait(1000);
feupos 5:afe2339723f6 81 }
feupos 5:afe2339723f6 82 }
feupos 5:afe2339723f6 83
feupos 5:afe2339723f6 84 void ethernetSend()
feupos 5:afe2339723f6 85 {
feupos 4:40990500a7cc 86 #ifdef SERIAL
feupos 5:afe2339723f6 87 printf("ethernetSendThread started\n");
feupos 4:40990500a7cc 88 #endif
feupos 5:afe2339723f6 89 std::stringstream ss;
feupos 4:40990500a7cc 90
feupos 4:40990500a7cc 91 while(true) {
feupos 4:40990500a7cc 92 if(sock.is_connected()) {
feupos 4:40990500a7cc 93 ss.flush();
feupos 4:40990500a7cc 94 ss << "Ball distance: " << ballDistance << "cm\n";
feupos 4:40990500a7cc 95 ss << "Setpoint: " << setpoint << "cm\n";
feupos 4:40990500a7cc 96 switch(statusFlag) {
feupos 4:40990500a7cc 97 case IDLE:
feupos 4:40990500a7cc 98 ss << "System is idle\n";
feupos 4:40990500a7cc 99 break;
feupos 4:40990500a7cc 100 case ADJUSTING:
feupos 4:40990500a7cc 101 ss << "System is adjusting\n";
feupos 4:40990500a7cc 102 break;
feupos 4:40990500a7cc 103 case STABLE:
feupos 4:40990500a7cc 104 ss << "System is stable\n";
feupos 4:40990500a7cc 105 break;
feupos 4:40990500a7cc 106 default:
feupos 4:40990500a7cc 107 break;
feupos 4:40990500a7cc 108 }
feupos 4:40990500a7cc 109 sock.send_all((char*)ss.str().data(),ss.str().size());
feupos 4:40990500a7cc 110 } else {
feupos 4:40990500a7cc 111 sock.connect("192.168.1.1", 12345);
feupos 4:40990500a7cc 112
feupos 4:40990500a7cc 113
feupos 4:40990500a7cc 114 }
feupos 6:ee9361616596 115 Thread::wait(5000);
feupos 4:40990500a7cc 116 }
feupos 4:40990500a7cc 117 }
feupos 4:40990500a7cc 118
feupos 4:40990500a7cc 119 void ethernetReceive()
feupos 4:40990500a7cc 120 {
feupos 5:afe2339723f6 121 #ifdef SERIAL
feupos 5:afe2339723f6 122 printf("ethernetReceiveThread started\n");
feupos 5:afe2339723f6 123 #endif
feupos 4:40990500a7cc 124 char buffer[10];
feupos 4:40990500a7cc 125 int ret;
feupos 4:40990500a7cc 126 while(true) {
feupos 4:40990500a7cc 127 if(sock.is_connected()) {
feupos 4:40990500a7cc 128
feupos 4:40990500a7cc 129
feupos 4:40990500a7cc 130 ret = sock.receive(buffer, sizeof(buffer)-1);
feupos 4:40990500a7cc 131 #ifdef SERIAL
feupos 4:40990500a7cc 132 buffer[ret] = '\0';
feupos 4:40990500a7cc 133 printf("Received %d chars from server:\n%s\n", ret, buffer);
feupos 4:40990500a7cc 134 #endif
feupos 4:40990500a7cc 135
feupos 4:40990500a7cc 136 switch(ret) {
feupos 4:40990500a7cc 137 default:
feupos 4:40990500a7cc 138 break;
feupos 4:40990500a7cc 139 case 1:
feupos 5:afe2339723f6 140 setpoint = (buffer[0]-'0');
feupos 4:40990500a7cc 141 break;
feupos 4:40990500a7cc 142 case 2:
feupos 4:40990500a7cc 143 setpoint = (buffer[0]-'0')*10 + buffer[1]-'0';
feupos 4:40990500a7cc 144 break;
feupos 4:40990500a7cc 145 }
feupos 4:40990500a7cc 146
feupos 4:40990500a7cc 147
feupos 4:40990500a7cc 148 } else {
feupos 4:40990500a7cc 149 sock.connect("192.168.1.1", 12345);
feupos 4:40990500a7cc 150 }
feupos 6:ee9361616596 151 Thread::wait(1000);
feupos 4:40990500a7cc 152
feupos 4:40990500a7cc 153 }
feupos 4:40990500a7cc 154 }
feupos 4:40990500a7cc 155 #endif
feupos 4:40990500a7cc 156
feupos 3:3d094a31a283 157 void ledSwitch()
feupos 0:8d53c4c11953 158 {
feupos 3:3d094a31a283 159 #ifdef SERIAL
feupos 4:40990500a7cc 160 printf("ledSwitch thread started\n");
feupos 3:3d094a31a283 161 #endif
feupos 0:8d53c4c11953 162 while (true) {
feupos 3:3d094a31a283 163 switch(statusFlag) {
feupos 3:3d094a31a283 164 case IDLE:
feupos 3:3d094a31a283 165 ledR = 1;
feupos 3:3d094a31a283 166 ledG = 1;
feupos 3:3d094a31a283 167 ledB = !ledB;
feupos 3:3d094a31a283 168 Thread::wait(500);
feupos 3:3d094a31a283 169 break;
feupos 3:3d094a31a283 170 case ADJUSTING:
feupos 2:f3ba67384801 171 ledR = !ledR;
feupos 3:3d094a31a283 172 ledG = 1;
feupos 3:3d094a31a283 173 ledB = 1;
feupos 3:3d094a31a283 174 Thread::wait(200);
feupos 3:3d094a31a283 175 break;
feupos 3:3d094a31a283 176 case STABLE:
feupos 3:3d094a31a283 177 ledR = 1;
feupos 3:3d094a31a283 178 ledG = !ledG;
feupos 3:3d094a31a283 179 ledB = 1;
feupos 2:f3ba67384801 180 Thread::wait(1000);
feupos 2:f3ba67384801 181 break;
feupos 3:3d094a31a283 182 default:
feupos 3:3d094a31a283 183 break;
feupos 2:f3ba67384801 184 }
feupos 3:3d094a31a283 185
feupos 0:8d53c4c11953 186 }
feupos 0:8d53c4c11953 187 }
feupos 0:8d53c4c11953 188
feupos 3:3d094a31a283 189 void serialOut()
feupos 0:8d53c4c11953 190 {
feupos 3:3d094a31a283 191 #ifdef SERIAL
feupos 4:40990500a7cc 192 printf("SerialOut thread started\n");
feupos 3:3d094a31a283 193 while(true) {
feupos 4:40990500a7cc 194 printf("Ball distance: %fcm\n",ballDistance);
feupos 3:3d094a31a283 195 printf("Setpoint: %fcm\n",setpoint);
feupos 3:3d094a31a283 196 switch(statusFlag) {
feupos 3:3d094a31a283 197 case IDLE:
feupos 3:3d094a31a283 198 printf("System is idle\n");
feupos 3:3d094a31a283 199 break;
feupos 3:3d094a31a283 200 case ADJUSTING:
feupos 3:3d094a31a283 201 printf("System is adjusting\n");
feupos 3:3d094a31a283 202 break;
feupos 3:3d094a31a283 203 case STABLE:
feupos 3:3d094a31a283 204 printf("System is stable\n");
feupos 3:3d094a31a283 205 break;
feupos 3:3d094a31a283 206 default:
feupos 3:3d094a31a283 207 break;
feupos 3:3d094a31a283 208 }
feupos 3:3d094a31a283 209 Thread::wait(500);
feupos 3:3d094a31a283 210 }
feupos 3:3d094a31a283 211 #endif
feupos 3:3d094a31a283 212 }
feupos 2:f3ba67384801 213
feupos 3:3d094a31a283 214 void controlSystem()
feupos 3:3d094a31a283 215 {
feupos 3:3d094a31a283 216 #ifdef SERIAL
feupos 4:40990500a7cc 217 printf("controlSystem thread started\n");
feupos 3:3d094a31a283 218 #endif
feupos 3:3d094a31a283 219 while(true) {
feupos 6:ee9361616596 220 ballDistance = ultrassonicSensor.distance(CM)+2.5;
feupos 6:ee9361616596 221
feupos 6:ee9361616596 222 if (ballDistance > 37.5)
feupos 6:ee9361616596 223 ballDistance = 35;
feupos 6:ee9361616596 224 if (ballDistance <0)
feupos 6:ee9361616596 225 ballDistance = 2.5;
feupos 6:ee9361616596 226
feupos 6:ee9361616596 227
feupos 6:ee9361616596 228 controller.setProcessValue(ballDistance);
feupos 3:3d094a31a283 229
feupos 4:40990500a7cc 230 if (ballDistance != setpoint) {
feupos 3:3d094a31a283 231 statusFlag = ADJUSTING;
feupos 6:ee9361616596 232 } else if (ballDistance == setpoint){
feupos 3:3d094a31a283 233 statusFlag = STABLE;
feupos 6:ee9361616596 234 } else if (!isRunning){
feupos 6:ee9361616596 235 statusFlag = IDLE;
feupos 3:3d094a31a283 236 }
feupos 3:3d094a31a283 237
feupos 3:3d094a31a283 238 //PID CONTROLLER
feupos 3:3d094a31a283 239 //motor.write(motorPos);
feupos 6:ee9361616596 240 //controller.setProcessValue(ballDistance);
feupos 6:ee9361616596 241
feupos 6:ee9361616596 242 controller.setSetPoint(setpoint);
feupos 6:ee9361616596 243 if(isRunning)
feupos 6:ee9361616596 244 motorPos = controller.compute();
feupos 6:ee9361616596 245
feupos 6:ee9361616596 246 #ifdef SERIAL
feupos 6:ee9361616596 247 //printf("Motor position: %f\n",motorPos);
feupos 6:ee9361616596 248 #endif
feupos 6:ee9361616596 249 motor = 1-motorPos;
feupos 4:40990500a7cc 250
feupos 3:3d094a31a283 251
feupos 3:3d094a31a283 252 Thread::wait(SAMPLE_RATE);
feupos 3:3d094a31a283 253 }
feupos 0:8d53c4c11953 254 }
feupos 0:8d53c4c11953 255
feupos 3:3d094a31a283 256
feupos 0:8d53c4c11953 257 int main()
feupos 0:8d53c4c11953 258 {
feupos 6:ee9361616596 259
feupos 6:ee9361616596 260
feupos 6:ee9361616596 261 Servo myservo(PTA2);
feupos 6:ee9361616596 262 /*
feupos 6:ee9361616596 263 while(1) {
feupos 6:ee9361616596 264 for(float p=-90; p<=90; p += 45) {
feupos 6:ee9361616596 265 myservo.position(180-p+BIAS);
feupos 6:ee9361616596 266 wait(1);
feupos 6:ee9361616596 267 }
feupos 6:ee9361616596 268 }
feupos 6:ee9361616596 269 */
feupos 6:ee9361616596 270 motor = 0.5;;
feupos 6:ee9361616596 271 wait(1);
feupos 6:ee9361616596 272
feupos 6:ee9361616596 273
feupos 3:3d094a31a283 274 statusFlag = IDLE;
feupos 3:3d094a31a283 275
feupos 3:3d094a31a283 276 #ifdef SERIAL
feupos 3:3d094a31a283 277 printf("BALL AND BEAM\n");
feupos 3:3d094a31a283 278 printf("APS de Sistemas Operacionais / Controle 2\n");
feupos 2:f3ba67384801 279 printf("Alunos: Felipe, Juliana, Rafael\n");
feupos 3:3d094a31a283 280 #endif
feupos 6:ee9361616596 281 // input from 0.0 to 35 cm
feupos 6:ee9361616596 282 controller.setInputLimits(-1, 50.0);
feupos 3:3d094a31a283 283 //Pwm output from 0.0 to 1.0 (servo)
feupos 6:ee9361616596 284 controller.setOutputLimits(0, 1);
feupos 3:3d094a31a283 285 //If there's a bias.
feupos 6:ee9361616596 286 controller.setBias(BIAS);
feupos 3:3d094a31a283 287 //controller.setBias(0.3);
feupos 3:3d094a31a283 288 controller.setMode(AUTO_MODE);
feupos 3:3d094a31a283 289 //We want the process variable to be 15cm (default)
feupos 3:3d094a31a283 290 controller.setSetPoint(setpoint);
feupos 3:3d094a31a283 291
feupos 3:3d094a31a283 292 sw2.rise(&sw2Callback);
feupos 3:3d094a31a283 293 sw3.rise(&sw3Callback);
feupos 3:3d094a31a283 294 ledSwitchThread.start(ledSwitch);
feupos 5:afe2339723f6 295 #ifdef SERIAL
feupos 3:3d094a31a283 296 serialOutThread.start(serialOut);
feupos 5:afe2339723f6 297 #endif
feupos 3:3d094a31a283 298 controlSystemThread.start(controlSystem);
feupos 3:3d094a31a283 299
feupos 4:40990500a7cc 300 #ifdef ETHERNET
feupos 5:afe2339723f6 301 eth.init("192.168.1.2","255.255.255.0","192.168.1.1");
feupos 5:afe2339723f6 302 eth.connect();
feupos 5:afe2339723f6 303 sock.connect("192.168.1.1", 12345);
feupos 5:afe2339723f6 304 sock.set_blocking(0);
feupos 5:afe2339723f6 305 #ifdef SERIAL
feupos 5:afe2339723f6 306 printf("IP Address is %s\n", eth.getIPAddress());
feupos 5:afe2339723f6 307 #endif
feupos 5:afe2339723f6 308
feupos 4:40990500a7cc 309 ethernetSendThread.start(ethernetSend);
feupos 4:40990500a7cc 310 ethernetReceiveThread.start(ethernetReceive);
feupos 5:afe2339723f6 311 ethernetKeepAliveThread.start(ethernetKeepAlive);
feupos 4:40990500a7cc 312 #endif
feupos 4:40990500a7cc 313
feupos 3:3d094a31a283 314 while(true) {
feupos 3:3d094a31a283 315 //nothing
feupos 3:3d094a31a283 316 }
feupos 3:3d094a31a283 317
feupos 3:3d094a31a283 318
feupos 3:3d094a31a283 319
feupos 0:8d53c4c11953 320
feupos 0:8d53c4c11953 321 }