Gunar Kroeger / Mbed OS PID_floating_ball

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   TextLCD

Fork of mbed-os-example-mbed5-blinky by mbed-os-examples

main.cpp@41:3bc2a3885b9d, 2017-07-04 (annotated)

Committer:
gunarthon
Date:
Tue Jul 04 16:36:18 2017 +0000
Revision:
41:3bc2a3885b9d
Parent:
40:19d51f6e6800
Child:
42:b69538bba4f9
improved menu navigation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan Austin 0:2757d7abb7d9 1 #include "mbed.h"
gunarthon 38:b760c09b311c 2 #include "Queue.h"
gunarthon 38:b760c09b311c 3 #include "TextLCD.h"
gunarthon 38:b760c09b311c 4 #include "pid.h"
gunarthon 39:e4f5710b2f31 5 #include <Serial.h>
Jonathan Austin 0:2757d7abb7d9 6
gunarthon 38:b760c09b311c 7 //definitions
gunarthon 38:b760c09b311c 8 typedef struct {
gunarthon 40:19d51f6e6800 9 double input;
gunarthon 38:b760c09b311c 10 } message_t;
gunarthon 38:b760c09b311c 11
gunarthon 40:19d51f6e6800 12 const int heightResolution = 1000;
gunarthon 39:e4f5710b2f31 13 const int BAUDRATE = 115200;
gunarthon 40:19d51f6e6800 14 const int cameraFPS = 60;
gunarthon 40:19d51f6e6800 15 const int displayRefreshTime = 200;
gunarthon 38:b760c09b311c 16 const double setPoint = 0.5;
gunarthon 40:19d51f6e6800 17 const double Kp = 0.02;
gunarthon 40:19d51f6e6800 18 const double Ki = 0;
gunarthon 40:19d51f6e6800 19 const double Kd = 0;
gunarthon 41:3bc2a3885b9d 20 const double configStep = 0.001;
gunarthon 41:3bc2a3885b9d 21 const double configFastStep = configStep*10;
gunarthon 38:b760c09b311c 22
gunarthon 40:19d51f6e6800 23 const int simulationSeconds = 10;
gunarthon 40:19d51f6e6800 24
gunarthon 40:19d51f6e6800 25 enum {btnRIGHT, btnUP, btnDOWN, btnLEFT, btnSELECT, btnNONE, btnSIZE};
gunarthon 40:19d51f6e6800 26 enum {menuSETPOINT, menuPID, menuSIMULATE, menuSIZE};
gunarthon 40:19d51f6e6800 27 enum {parameterKP, parameterKI, parameterKD, parameterSIZE};
gunarthon 40:19d51f6e6800 28 enum {simulSTEP, simulRAMP, simulNONE, simulSIZE};
Jonathan Austin 0:2757d7abb7d9 29
gunarthon 38:b760c09b311c 30 //Pins
gunarthon 38:b760c09b311c 31 PwmOut outPin(D3);
gunarthon 40:19d51f6e6800 32 //DigitalOut led2(LED2);
gunarthon 38:b760c09b311c 33 DigitalOut led3(LED3);
gunarthon 38:b760c09b311c 34 AnalogIn buttons(A0);
gunarthon 38:b760c09b311c 35
gunarthon 38:b760c09b311c 36 //Threads
gunarthon 41:3bc2a3885b9d 37 Thread lcdThread;
gunarthon 38:b760c09b311c 38 Thread pidThread;
gunarthon 38:b760c09b311c 39 Thread communicationThread;
gunarthon 38:b760c09b311c 40 Thread hmiThread;
gunarthon 40:19d51f6e6800 41 Thread setPointThread;
gunarthon 38:b760c09b311c 42
gunarthon 38:b760c09b311c 43 //Global variables
gunarthon 38:b760c09b311c 44 TextLCD lcd(D8, D9, D4, D5, D6, D7); // rs, e, d4-d7
gunarthon 38:b760c09b311c 45 Pid* pidController;
gunarthon 38:b760c09b311c 46 MemoryPool<message_t, 16> mpool;
gunarthon 38:b760c09b311c 47 Queue<message_t,16> messageQueue;
gunarthon 38:b760c09b311c 48
gunarthon 40:19d51f6e6800 49 //Menu settings
gunarthon 40:19d51f6e6800 50 int menu = menuSETPOINT;
gunarthon 40:19d51f6e6800 51 int parameter = parameterKP;
gunarthon 40:19d51f6e6800 52 int simul = simulNONE;
gunarthon 41:3bc2a3885b9d 53 bool setPointSimul = false;
gunarthon 40:19d51f6e6800 54
gunarthon 39:e4f5710b2f31 55 void SerialCallback(int);
gunarthon 39:e4f5710b2f31 56
gunarthon 38:b760c09b311c 57 //=============================Thread Methodes==================================
gunarthon 40:19d51f6e6800 58 void setPointMethode(void)
gunarthon 40:19d51f6e6800 59 {
gunarthon 41:3bc2a3885b9d 60 while(1) //toggle every 20 seconds between 2 setPoint values
gunarthon 40:19d51f6e6800 61 {
gunarthon 41:3bc2a3885b9d 62 while(!setPointSimul) Thread::wait(2000);
gunarthon 41:3bc2a3885b9d 63
gunarthon 41:3bc2a3885b9d 64 if(setPointSimul)
gunarthon 41:3bc2a3885b9d 65 {
gunarthon 41:3bc2a3885b9d 66 pidController->setSetPoint(0.2);
gunarthon 41:3bc2a3885b9d 67 Thread::wait(20000);
gunarthon 41:3bc2a3885b9d 68 if(setPointSimul)
gunarthon 41:3bc2a3885b9d 69 {
gunarthon 41:3bc2a3885b9d 70 pidController->setSetPoint(0.8);
gunarthon 41:3bc2a3885b9d 71 Thread::wait(20000);
gunarthon 41:3bc2a3885b9d 72 }
gunarthon 41:3bc2a3885b9d 73 }
gunarthon 40:19d51f6e6800 74 }
gunarthon 40:19d51f6e6800 75 }
gunarthon 41:3bc2a3885b9d 76 void LcdMethode(void)
gunarthon 38:b760c09b311c 77 {
gunarthon 38:b760c09b311c 78 while(true)
gunarthon 38:b760c09b311c 79 {
gunarthon 38:b760c09b311c 80 led3 = !led3;
gunarthon 40:19d51f6e6800 81
gunarthon 40:19d51f6e6800 82 double lastPwm = pidController->getLastPwm();
gunarthon 40:19d51f6e6800 83 double lastInput = pidController->getLastInput();
gunarthon 40:19d51f6e6800 84
gunarthon 40:19d51f6e6800 85 if(menu == menuSETPOINT)
gunarthon 40:19d51f6e6800 86 {
gunarthon 40:19d51f6e6800 87 lcd.cls();
gunarthon 40:19d51f6e6800 88 lcd.printf("set in out");
gunarthon 40:19d51f6e6800 89 lcd.locate(0,1);
gunarthon 40:19d51f6e6800 90 lcd.printf("%d", int(1000*pidController->getSetPoint()));
gunarthon 40:19d51f6e6800 91 lcd.locate(6,1);
gunarthon 40:19d51f6e6800 92 lcd.printf("%d", int(1000*lastInput));
gunarthon 40:19d51f6e6800 93 lcd.locate(12,1);
gunarthon 40:19d51f6e6800 94 lcd.printf("%d", int(1000*lastPwm));
gunarthon 40:19d51f6e6800 95 }
gunarthon 40:19d51f6e6800 96 else if(menu == menuPID)
gunarthon 40:19d51f6e6800 97 {
gunarthon 40:19d51f6e6800 98 lcd.cls();
gunarthon 40:19d51f6e6800 99 lcd.printf(" Kp Ki Kd");
gunarthon 40:19d51f6e6800 100 lcd.locate(6*parameter, 0);
gunarthon 40:19d51f6e6800 101 lcd.printf("*");
gunarthon 40:19d51f6e6800 102 lcd.locate(0,1);
gunarthon 40:19d51f6e6800 103 lcd.printf("%d", int(1000*pidController->getKp()));
gunarthon 40:19d51f6e6800 104 lcd.locate(6,1);
gunarthon 40:19d51f6e6800 105 lcd.printf("%d", int(1000*pidController->getKi()));
gunarthon 40:19d51f6e6800 106 lcd.locate(12,1);
gunarthon 40:19d51f6e6800 107 lcd.printf("%d", int(1000*pidController->getKd()));
gunarthon 40:19d51f6e6800 108 }
gunarthon 40:19d51f6e6800 109 else if(menu == menuSIMULATE)
gunarthon 40:19d51f6e6800 110 {
gunarthon 40:19d51f6e6800 111 lcd.cls();
gunarthon 40:19d51f6e6800 112 if(simul == simulNONE)
gunarthon 41:3bc2a3885b9d 113 {
gunarthon 41:3bc2a3885b9d 114 if(setPointSimul)
gunarthon 41:3bc2a3885b9d 115 lcd.printf("u:STEP * d:RAMP");
gunarthon 41:3bc2a3885b9d 116 else
gunarthon 41:3bc2a3885b9d 117 lcd.printf("u:STEP _ d:RAMP");
gunarthon 41:3bc2a3885b9d 118 }
gunarthon 40:19d51f6e6800 119 else
gunarthon 40:19d51f6e6800 120 lcd.printf("set $in$ out");
gunarthon 40:19d51f6e6800 121 lcd.locate(0,1);
gunarthon 40:19d51f6e6800 122 lcd.printf("%d", int(1000*pidController->getSetPoint()));
gunarthon 40:19d51f6e6800 123 lcd.locate(6,1);
gunarthon 40:19d51f6e6800 124 lcd.printf("%d", int(1000*lastInput));
gunarthon 40:19d51f6e6800 125 lcd.locate(12,1);
gunarthon 40:19d51f6e6800 126 lcd.printf("%d", int(1000*lastPwm));
gunarthon 40:19d51f6e6800 127 }
gunarthon 40:19d51f6e6800 128 Thread::wait(displayRefreshTime);
Jonathan Austin 0:2757d7abb7d9 129 }
Jonathan Austin 0:2757d7abb7d9 130 }
Jonathan Austin 1:846c97078558 131
gunarthon 38:b760c09b311c 132 //------------------------------------------------------------------------------
gunarthon 38:b760c09b311c 133
gunarthon 38:b760c09b311c 134 void PidMethode(void)
gunarthon 38:b760c09b311c 135 {
gunarthon 38:b760c09b311c 136 while(true)
gunarthon 38:b760c09b311c 137 {
gunarthon 38:b760c09b311c 138 osEvent ev = messageQueue.get(osWaitForever);
gunarthon 38:b760c09b311c 139 if (ev.status == osEventMessage)
gunarthon 38:b760c09b311c 140 {
gunarthon 38:b760c09b311c 141 message_t *message = (message_t*)ev.value.p;
gunarthon 38:b760c09b311c 142
gunarthon 40:19d51f6e6800 143 double input = message->input;
gunarthon 38:b760c09b311c 144
gunarthon 38:b760c09b311c 145 double newPwm = pidController->getPwm(input);
gunarthon 38:b760c09b311c 146
gunarthon 38:b760c09b311c 147 outPin.write(newPwm);
gunarthon 38:b760c09b311c 148
gunarthon 38:b760c09b311c 149 mpool.free(message);
gunarthon 38:b760c09b311c 150 }
gunarthon 38:b760c09b311c 151 }
gunarthon 38:b760c09b311c 152 }
gunarthon 38:b760c09b311c 153 //------------------------------------------------------------------------------
gunarthon 38:b760c09b311c 154
gunarthon 38:b760c09b311c 155 void CommunicationMethode(void)
gunarthon 38:b760c09b311c 156 {
gunarthon 39:e4f5710b2f31 157 Serial serial(USBTX, USBRX);
gunarthon 39:e4f5710b2f31 158 serial.baud(BAUDRATE);
gunarthon 39:e4f5710b2f31 159 serial.format(8, SerialBase::None, 1);
gunarthon 40:19d51f6e6800 160 double input = 0;
gunarthon 39:e4f5710b2f31 161
gunarthon 38:b760c09b311c 162 while(true)
gunarthon 38:b760c09b311c 163 {
gunarthon 40:19d51f6e6800 164 input = double(serial.getc()+(serial.getc()<<8)) / heightResolution;
gunarthon 38:b760c09b311c 165 message_t *message = mpool.alloc();
gunarthon 38:b760c09b311c 166 message->input = input;
gunarthon 38:b760c09b311c 167 messageQueue.put(message);
gunarthon 38:b760c09b311c 168 }
gunarthon 38:b760c09b311c 169 }
gunarthon 38:b760c09b311c 170
gunarthon 40:19d51f6e6800 171 unsigned readButtons()
gunarthon 38:b760c09b311c 172 {
gunarthon 38:b760c09b311c 173 double buttonsValue = buttons.read();
gunarthon 38:b760c09b311c 174
gunarthon 38:b760c09b311c 175 unsigned button = btnNONE;
gunarthon 38:b760c09b311c 176 if(buttonsValue < 0.08) //0.000
gunarthon 38:b760c09b311c 177 button = btnRIGHT;
gunarthon 38:b760c09b311c 178 else if(buttonsValue < 0.28) //0.170
gunarthon 38:b760c09b311c 179 button = btnUP;
gunarthon 38:b760c09b311c 180 else if(buttonsValue < 0.51) //0.397
gunarthon 38:b760c09b311c 181 button = btnDOWN;
gunarthon 38:b760c09b311c 182 else if(buttonsValue < 0.78) //0.621
gunarthon 38:b760c09b311c 183 button = btnLEFT;
gunarthon 38:b760c09b311c 184 else if(buttonsValue < 0.97) //0.936
gunarthon 38:b760c09b311c 185 button = btnSELECT;
gunarthon 38:b760c09b311c 186 else //1.000
gunarthon 38:b760c09b311c 187 button = btnNONE;
gunarthon 38:b760c09b311c 188
gunarthon 40:19d51f6e6800 189 return button;
gunarthon 40:19d51f6e6800 190
gunarthon 40:19d51f6e6800 191 }
gunarthon 40:19d51f6e6800 192 //------------------------------------------------------------------------------
gunarthon 40:19d51f6e6800 193 //human-machine interface
gunarthon 40:19d51f6e6800 194 void hmiMethode(void)
gunarthon 40:19d51f6e6800 195 {
gunarthon 40:19d51f6e6800 196
gunarthon 40:19d51f6e6800 197 unsigned button = btnNONE;
gunarthon 41:3bc2a3885b9d 198 bool fastChange = false;
gunarthon 40:19d51f6e6800 199 while(true)
gunarthon 40:19d51f6e6800 200 {
gunarthon 38:b760c09b311c 201
gunarthon 41:3bc2a3885b9d 202 button = readButtons();
gunarthon 41:3bc2a3885b9d 203 while(button == btnNONE)
gunarthon 41:3bc2a3885b9d 204 {
gunarthon 40:19d51f6e6800 205 button = readButtons();
gunarthon 41:3bc2a3885b9d 206 Thread::wait(10);
gunarthon 41:3bc2a3885b9d 207 fastChange = false;
gunarthon 41:3bc2a3885b9d 208 }
gunarthon 40:19d51f6e6800 209
gunarthon 38:b760c09b311c 210 double prevSetPoint = pidController->getSetPoint();
gunarthon 38:b760c09b311c 211 double newSetPoint = prevSetPoint;
gunarthon 40:19d51f6e6800 212
gunarthon 40:19d51f6e6800 213 if(button == btnSELECT)
gunarthon 40:19d51f6e6800 214 {
gunarthon 40:19d51f6e6800 215 menu = (menu+1) % menuSIZE;
gunarthon 40:19d51f6e6800 216 simul = simulNONE;
gunarthon 40:19d51f6e6800 217 }
gunarthon 40:19d51f6e6800 218 else if(menu == menuSETPOINT)
gunarthon 40:19d51f6e6800 219 {
gunarthon 40:19d51f6e6800 220 if(button == btnUP)
gunarthon 41:3bc2a3885b9d 221 newSetPoint += fastChange ? configFastStep : configStep;
gunarthon 40:19d51f6e6800 222 else if(button == btnDOWN)
gunarthon 41:3bc2a3885b9d 223 newSetPoint -= fastChange ? configFastStep : configStep;
gunarthon 40:19d51f6e6800 224 else if(button == btnLEFT)
gunarthon 41:3bc2a3885b9d 225 newSetPoint -= fastChange ? 10*configFastStep : 10*configStep;
gunarthon 40:19d51f6e6800 226 else if(button == btnRIGHT)
gunarthon 41:3bc2a3885b9d 227 newSetPoint += fastChange ? 10*configFastStep : 10*configStep;
gunarthon 40:19d51f6e6800 228
gunarthon 40:19d51f6e6800 229 pidController->setSetPoint(newSetPoint);
gunarthon 40:19d51f6e6800 230 }
gunarthon 40:19d51f6e6800 231 else if(menu == menuPID)
gunarthon 40:19d51f6e6800 232 {
gunarthon 40:19d51f6e6800 233 if(button == btnUP)
gunarthon 40:19d51f6e6800 234 {
gunarthon 40:19d51f6e6800 235 if(parameter == parameterKP)
gunarthon 41:3bc2a3885b9d 236 pidController->addKp(fastChange ? configFastStep : configStep);
gunarthon 40:19d51f6e6800 237 else if(parameter == parameterKI)
gunarthon 41:3bc2a3885b9d 238 pidController->addKi(fastChange ? configFastStep : configStep);
gunarthon 40:19d51f6e6800 239 else if(parameter == parameterKD)
gunarthon 41:3bc2a3885b9d 240 pidController->addKd(fastChange ? configFastStep : configStep);
gunarthon 40:19d51f6e6800 241 }
gunarthon 40:19d51f6e6800 242 else if(button == btnDOWN)
gunarthon 40:19d51f6e6800 243 {
gunarthon 40:19d51f6e6800 244 if(parameter == parameterKP)
gunarthon 41:3bc2a3885b9d 245 pidController->addKp(fastChange ? -configFastStep : -configStep);
gunarthon 40:19d51f6e6800 246 else if(parameter == parameterKI)
gunarthon 41:3bc2a3885b9d 247 pidController->addKi(fastChange ? -configFastStep : -configStep);
gunarthon 40:19d51f6e6800 248 else if(parameter == parameterKD)
gunarthon 41:3bc2a3885b9d 249 pidController->addKd(fastChange ? -configFastStep : -configStep);
gunarthon 40:19d51f6e6800 250 }
gunarthon 40:19d51f6e6800 251 else if(button == btnLEFT)
gunarthon 40:19d51f6e6800 252 parameter = (parameter+1) % parameterSIZE;
gunarthon 40:19d51f6e6800 253 else if(button == btnRIGHT)
gunarthon 40:19d51f6e6800 254 pidController->setParameters(Kp, Ki, Kd);
gunarthon 40:19d51f6e6800 255 }
gunarthon 40:19d51f6e6800 256
gunarthon 40:19d51f6e6800 257 else if(menu == menuSIMULATE)
gunarthon 40:19d51f6e6800 258 {
gunarthon 40:19d51f6e6800 259 if(button == btnUP) //step
gunarthon 40:19d51f6e6800 260 {
gunarthon 40:19d51f6e6800 261 simul = simulSTEP;
gunarthon 40:19d51f6e6800 262 for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
gunarthon 40:19d51f6e6800 263 {
gunarthon 40:19d51f6e6800 264 double input = i < cameraFPS * simulationSeconds / 2 ? 0 : 1;
gunarthon 40:19d51f6e6800 265 message_t *message = mpool.alloc();
gunarthon 40:19d51f6e6800 266 message->input = input;
gunarthon 40:19d51f6e6800 267 messageQueue.put(message);
gunarthon 40:19d51f6e6800 268
gunarthon 40:19d51f6e6800 269 Thread::wait(1000/cameraFPS);
gunarthon 40:19d51f6e6800 270 }
gunarthon 40:19d51f6e6800 271 simul = simulNONE;
gunarthon 40:19d51f6e6800 272 }
gunarthon 40:19d51f6e6800 273 else if(button == btnDOWN) //ramp
gunarthon 40:19d51f6e6800 274 {
gunarthon 40:19d51f6e6800 275 simul = simulRAMP;
gunarthon 40:19d51f6e6800 276 for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
gunarthon 40:19d51f6e6800 277 {
gunarthon 40:19d51f6e6800 278 double input = double(i) / ((cameraFPS * simulationSeconds)-1);
gunarthon 40:19d51f6e6800 279 message_t *message = mpool.alloc();
gunarthon 40:19d51f6e6800 280 message->input = input;
gunarthon 40:19d51f6e6800 281 messageQueue.put(message);
gunarthon 40:19d51f6e6800 282
gunarthon 40:19d51f6e6800 283 Thread::wait(1000/cameraFPS);
gunarthon 40:19d51f6e6800 284 }
gunarthon 40:19d51f6e6800 285 simul = simulNONE;
gunarthon 40:19d51f6e6800 286 }
gunarthon 41:3bc2a3885b9d 287 else if(button == btnLEFT)
gunarthon 41:3bc2a3885b9d 288 setPointSimul = true;
gunarthon 40:19d51f6e6800 289 else if(button == btnRIGHT)
gunarthon 41:3bc2a3885b9d 290 setPointSimul = false;
gunarthon 38:b760c09b311c 291
gunarthon 40:19d51f6e6800 292 }
gunarthon 41:3bc2a3885b9d 293 unsigned repeatCount = 0;
gunarthon 41:3bc2a3885b9d 294 while(button != btnNONE && !fastChange)
gunarthon 40:19d51f6e6800 295 {
gunarthon 41:3bc2a3885b9d 296 repeatCount++;
gunarthon 40:19d51f6e6800 297 button = readButtons();
gunarthon 40:19d51f6e6800 298 Thread::wait(10);
gunarthon 41:3bc2a3885b9d 299 if(repeatCount > 50)
gunarthon 41:3bc2a3885b9d 300 fastChange = true;
gunarthon 41:3bc2a3885b9d 301 }
gunarthon 38:b760c09b311c 302 Thread::wait(100);
gunarthon 38:b760c09b311c 303 }
gunarthon 38:b760c09b311c 304 }
gunarthon 38:b760c09b311c 305
gunarthon 38:b760c09b311c 306 //=============================Main Thread======================================
gunarthon 38:b760c09b311c 307 // main() runs in its own thread in the OS
gunarthon 38:b760c09b311c 308
gunarthon 38:b760c09b311c 309 int main() {
gunarthon 38:b760c09b311c 310
gunarthon 38:b760c09b311c 311 //led1.period(4.0f); // 4 second period
gunarthon 38:b760c09b311c 312
gunarthon 38:b760c09b311c 313 lcd.cls();
gunarthon 38:b760c09b311c 314 lcd.printf("HELLO");
gunarthon 38:b760c09b311c 315
gunarthon 38:b760c09b311c 316 pidController = new Pid(Kp, Ki, Kd);
gunarthon 38:b760c09b311c 317 pidController->setSetPoint(setPoint);
gunarthon 38:b760c09b311c 318
gunarthon 41:3bc2a3885b9d 319 lcdThread.start(LcdMethode);
gunarthon 38:b760c09b311c 320 pidThread.start(PidMethode);
gunarthon 38:b760c09b311c 321 communicationThread.start(CommunicationMethode);
gunarthon 38:b760c09b311c 322 hmiThread.start(hmiMethode);
gunarthon 41:3bc2a3885b9d 323 setPointThread.start(setPointMethode);
gunarthon 38:b760c09b311c 324
gunarthon 38:b760c09b311c 325 while (true) {
gunarthon 38:b760c09b311c 326 Thread::wait(1100);
gunarthon 38:b760c09b311c 327 }
gunarthon 38:b760c09b311c 328 }
gunarthon 38:b760c09b311c 329
gunarthon 38:b760c09b311c 330 //==============================================================================
gunarthon 38:b760c09b311c 331
gunarthon 38:b760c09b311c 332
gunarthon 38:b760c09b311c 333