APS de Sistemas Operacionais / Controle 2 FINAL
Dependencies: EthernetInterface HCSR04 PID Servo mbed-rtos mbed
Fork of aps_so_c2_old by
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?
User | Revision | Line number | New 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 | } |