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@4:40990500a7cc, 2017-11-18 (annotated)

Committer:
feupos
Date:
Sat Nov 18 20:03:58 2017 +0000
Revision:
4:40990500a7cc
Parent:
3:3d094a31a283
Child:
5:afe2339723f6
working ethernet

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