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@5:afe2339723f6, 2017-11-18 (annotated)

Committer:
feupos
Date:
Sat Nov 18 20:42:14 2017 +0000
Revision:
5:afe2339723f6
Parent:
4:40990500a7cc
Child:
6:ee9361616596
functional version (ethernet timeout bug)

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