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

Committer:
feupos
Date:
Sat Nov 18 18:27:27 2017 +0000
Revision:
3:3d094a31a283
Parent:
2:f3ba67384801
Child:
4:40990500a7cc
working with PID

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 2:f3ba67384801 7
feupos 3:3d094a31a283 8 #define SERIAL
feupos 3:3d094a31a283 9 #define ETHERNET
feupos 0:8d53c4c11953 10
feupos 3:3d094a31a283 11 #define SAMPLE_RATE 10 //sample rate in miliseconds
feupos 3:3d094a31a283 12
feupos 3:3d094a31a283 13 enum status { IDLE, ADJUSTING, STABLE };
feupos 3:3d094a31a283 14
feupos 3:3d094a31a283 15 status statusFlag; //flag to determine behavior
feupos 2:f3ba67384801 16
feupos 2:f3ba67384801 17 DigitalOut ledR(LED1);
feupos 2:f3ba67384801 18 DigitalOut ledG(LED2);
feupos 2:f3ba67384801 19 DigitalOut ledB(LED3);
feupos 2:f3ba67384801 20
feupos 3:3d094a31a283 21 HCSR04 ultrassonicSensor(PTC2, PTC3);
feupos 3:3d094a31a283 22 Servo motor(PTA2);
feupos 3:3d094a31a283 23 float motorPos = 0;
feupos 3:3d094a31a283 24
feupos 3:3d094a31a283 25 //Kc, Ti, Td, interval
feupos 3:3d094a31a283 26 PID controller(1.0, 0.0, 0.0, SAMPLE_RATE);
feupos 0:8d53c4c11953 27
feupos 3:3d094a31a283 28 InterruptIn sw2(SW2);
feupos 3:3d094a31a283 29 void sw2Callback()
feupos 3:3d094a31a283 30 {
feupos 3:3d094a31a283 31 if(motorPos<1)
feupos 3:3d094a31a283 32 motorPos+=0.1;
feupos 3:3d094a31a283 33 }
feupos 3:3d094a31a283 34 InterruptIn sw3(SW3);
feupos 3:3d094a31a283 35 void sw3Callback()
feupos 3:3d094a31a283 36 {
feupos 3:3d094a31a283 37 if(motorPos>0)
feupos 3:3d094a31a283 38 motorPos-=0.1;
feupos 0:8d53c4c11953 39 }
feupos 0:8d53c4c11953 40
feupos 3:3d094a31a283 41 Thread ledSwitchThread;
feupos 3:3d094a31a283 42 Thread serialOutThread;
feupos 3:3d094a31a283 43 Thread controlSystemThread;
feupos 3:3d094a31a283 44
feupos 3:3d094a31a283 45 float distance = 0.0;
feupos 3:3d094a31a283 46 float setpoint = 15.0;
feupos 3:3d094a31a283 47
feupos 3:3d094a31a283 48 void ledSwitch()
feupos 0:8d53c4c11953 49 {
feupos 3:3d094a31a283 50 #ifdef SERIAL
feupos 3:3d094a31a283 51 printf("ledSwitch thread started");
feupos 3:3d094a31a283 52 #endif
feupos 0:8d53c4c11953 53 while (true) {
feupos 3:3d094a31a283 54 switch(statusFlag) {
feupos 3:3d094a31a283 55 case IDLE:
feupos 3:3d094a31a283 56 ledR = 1;
feupos 3:3d094a31a283 57 ledG = 1;
feupos 3:3d094a31a283 58 ledB = !ledB;
feupos 3:3d094a31a283 59 Thread::wait(500);
feupos 3:3d094a31a283 60 break;
feupos 3:3d094a31a283 61 case ADJUSTING:
feupos 2:f3ba67384801 62 ledR = !ledR;
feupos 3:3d094a31a283 63 ledG = 1;
feupos 3:3d094a31a283 64 ledB = 1;
feupos 3:3d094a31a283 65 Thread::wait(200);
feupos 3:3d094a31a283 66 break;
feupos 3:3d094a31a283 67 case STABLE:
feupos 3:3d094a31a283 68 ledR = 1;
feupos 3:3d094a31a283 69 ledG = !ledG;
feupos 3:3d094a31a283 70 ledB = 1;
feupos 2:f3ba67384801 71 Thread::wait(1000);
feupos 2:f3ba67384801 72 break;
feupos 3:3d094a31a283 73 default:
feupos 3:3d094a31a283 74 break;
feupos 2:f3ba67384801 75 }
feupos 3:3d094a31a283 76
feupos 0:8d53c4c11953 77 }
feupos 0:8d53c4c11953 78 }
feupos 0:8d53c4c11953 79
feupos 3:3d094a31a283 80 void serialOut()
feupos 0:8d53c4c11953 81 {
feupos 3:3d094a31a283 82 #ifdef SERIAL
feupos 3:3d094a31a283 83 printf("SerialOut thread started");
feupos 3:3d094a31a283 84 while(true) {
feupos 3:3d094a31a283 85 printf("Ball distance: %fcm\n",distance);
feupos 3:3d094a31a283 86 printf("Setpoint: %fcm\n",setpoint);
feupos 3:3d094a31a283 87 switch(statusFlag) {
feupos 3:3d094a31a283 88 case IDLE:
feupos 3:3d094a31a283 89 printf("System is idle\n");
feupos 3:3d094a31a283 90 break;
feupos 3:3d094a31a283 91 case ADJUSTING:
feupos 3:3d094a31a283 92 printf("System is adjusting\n");
feupos 3:3d094a31a283 93 break;
feupos 3:3d094a31a283 94 case STABLE:
feupos 3:3d094a31a283 95 printf("System is stable\n");
feupos 3:3d094a31a283 96 break;
feupos 3:3d094a31a283 97 default:
feupos 3:3d094a31a283 98 break;
feupos 3:3d094a31a283 99 }
feupos 3:3d094a31a283 100 Thread::wait(500);
feupos 3:3d094a31a283 101 }
feupos 3:3d094a31a283 102 #endif
feupos 3:3d094a31a283 103 }
feupos 2:f3ba67384801 104
feupos 3:3d094a31a283 105 void controlSystem()
feupos 3:3d094a31a283 106 {
feupos 3:3d094a31a283 107 #ifdef SERIAL
feupos 3:3d094a31a283 108 printf("controlSystem thread started");
feupos 3:3d094a31a283 109 #endif
feupos 3:3d094a31a283 110 while(true) {
feupos 3:3d094a31a283 111 distance = ultrassonicSensor.distance(CM);
feupos 3:3d094a31a283 112
feupos 3:3d094a31a283 113 if (distance != setpoint) {
feupos 3:3d094a31a283 114 statusFlag = ADJUSTING;
feupos 3:3d094a31a283 115 } else {
feupos 3:3d094a31a283 116 statusFlag = STABLE;
feupos 3:3d094a31a283 117 }
feupos 3:3d094a31a283 118
feupos 3:3d094a31a283 119 //PID CONTROLLER
feupos 3:3d094a31a283 120 //motor.write(motorPos);
feupos 3:3d094a31a283 121 controller.setProcessValue(distance);
feupos 3:3d094a31a283 122 motor.write(controller.compute());
feupos 3:3d094a31a283 123
feupos 3:3d094a31a283 124
feupos 3:3d094a31a283 125 Thread::wait(SAMPLE_RATE);
feupos 3:3d094a31a283 126 }
feupos 0:8d53c4c11953 127 }
feupos 0:8d53c4c11953 128
feupos 3:3d094a31a283 129
feupos 0:8d53c4c11953 130 int main()
feupos 0:8d53c4c11953 131 {
feupos 3:3d094a31a283 132 statusFlag = IDLE;
feupos 3:3d094a31a283 133
feupos 3:3d094a31a283 134 #ifdef SERIAL
feupos 3:3d094a31a283 135 printf("BALL AND BEAM\n");
feupos 3:3d094a31a283 136 printf("APS de Sistemas Operacionais / Controle 2\n");
feupos 2:f3ba67384801 137 printf("Alunos: Felipe, Juliana, Rafael\n");
feupos 3:3d094a31a283 138 #endif
feupos 3:3d094a31a283 139 //Analog input from 0.0 to 50.0 cm
feupos 3:3d094a31a283 140 controller.setInputLimits(0.0, 50.0);
feupos 3:3d094a31a283 141 //Pwm output from 0.0 to 1.0 (servo)
feupos 3:3d094a31a283 142 controller.setOutputLimits(0.0, 1.0);
feupos 3:3d094a31a283 143 //If there's a bias.
feupos 3:3d094a31a283 144 //controller.setBias(0.3);
feupos 3:3d094a31a283 145 controller.setMode(AUTO_MODE);
feupos 3:3d094a31a283 146 //We want the process variable to be 15cm (default)
feupos 3:3d094a31a283 147 controller.setSetPoint(setpoint);
feupos 3:3d094a31a283 148
feupos 3:3d094a31a283 149 sw2.rise(&sw2Callback);
feupos 3:3d094a31a283 150 sw3.rise(&sw3Callback);
feupos 3:3d094a31a283 151 ledSwitchThread.start(ledSwitch);
feupos 3:3d094a31a283 152 serialOutThread.start(serialOut);
feupos 3:3d094a31a283 153 controlSystemThread.start(controlSystem);
feupos 3:3d094a31a283 154
feupos 3:3d094a31a283 155 while(true) {
feupos 3:3d094a31a283 156 //nothing
feupos 3:3d094a31a283 157 }
feupos 3:3d094a31a283 158
feupos 3:3d094a31a283 159
feupos 3:3d094a31a283 160
feupos 0:8d53c4c11953 161
feupos 0:8d53c4c11953 162 }