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@40:19d51f6e6800, 2017-07-02 (annotated)

Committer:
gunarthon
Date:
Sun Jul 02 19:01:07 2017 +0000
Revision:
40:19d51f6e6800
Parent:
39:e4f5710b2f31
Child:
41:3bc2a3885b9d
pid alterations

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