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@38:b760c09b311c, 2017-06-15 (annotated)

Committer:
gunarthon
Date:
Thu Jun 15 23:19:38 2017 +0000
Revision:
38:b760c09b311c
Parent:
29:0b58d21e87d6
Child:
39:e4f5710b2f31
initial commit

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"
Jonathan Austin 0:2757d7abb7d9 5
gunarthon 38:b760c09b311c 6 //definitions
gunarthon 38:b760c09b311c 7 typedef struct {
gunarthon 38:b760c09b311c 8 uint32_t input;
gunarthon 38:b760c09b311c 9 } message_t;
gunarthon 38:b760c09b311c 10
gunarthon 38:b760c09b311c 11 const int heightResolution = 1024;
gunarthon 38:b760c09b311c 12 const double setPoint = 0.5;
gunarthon 38:b760c09b311c 13 const double Kp = 1;
gunarthon 38:b760c09b311c 14 const double Ki = 0.2;
gunarthon 38:b760c09b311c 15 const double Kd = 0.3;
gunarthon 38:b760c09b311c 16
gunarthon 38:b760c09b311c 17 enum {btnRIGHT, btnUP, btnDOWN, btnLEFT, btnSELECT, btnNONE};
Jonathan Austin 0:2757d7abb7d9 18
gunarthon 38:b760c09b311c 19 //Pins
gunarthon 38:b760c09b311c 20 PwmOut outPin(D3);
gunarthon 38:b760c09b311c 21 DigitalOut led2(LED2);
gunarthon 38:b760c09b311c 22 DigitalOut led3(LED3);
gunarthon 38:b760c09b311c 23 AnalogIn buttons(A0);
gunarthon 38:b760c09b311c 24
gunarthon 38:b760c09b311c 25 //Threads
gunarthon 38:b760c09b311c 26 Thread blueThread;
gunarthon 38:b760c09b311c 27 Thread pidThread;
gunarthon 38:b760c09b311c 28 Thread communicationThread;
gunarthon 38:b760c09b311c 29 Thread hmiThread;
gunarthon 38:b760c09b311c 30
gunarthon 38:b760c09b311c 31 //Global variables
gunarthon 38:b760c09b311c 32 TextLCD lcd(D8, D9, D4, D5, D6, D7); // rs, e, d4-d7
gunarthon 38:b760c09b311c 33 Pid* pidController;
gunarthon 38:b760c09b311c 34 MemoryPool<message_t, 16> mpool;
gunarthon 38:b760c09b311c 35 Queue<message_t,16> messageQueue;
gunarthon 38:b760c09b311c 36
gunarthon 38:b760c09b311c 37 //=============================Thread Methodes==================================
gunarthon 38:b760c09b311c 38
gunarthon 38:b760c09b311c 39 void BlueMethode(void)
gunarthon 38:b760c09b311c 40 {
gunarthon 38:b760c09b311c 41 while(true)
gunarthon 38:b760c09b311c 42 {
gunarthon 38:b760c09b311c 43 led3 = !led3;
gunarthon 38:b760c09b311c 44 Thread::wait(500);
Jonathan Austin 0:2757d7abb7d9 45 }
Jonathan Austin 0:2757d7abb7d9 46 }
Jonathan Austin 1:846c97078558 47
gunarthon 38:b760c09b311c 48 //------------------------------------------------------------------------------
gunarthon 38:b760c09b311c 49
gunarthon 38:b760c09b311c 50 void PidMethode(void)
gunarthon 38:b760c09b311c 51 {
gunarthon 38:b760c09b311c 52 while(true)
gunarthon 38:b760c09b311c 53 {
gunarthon 38:b760c09b311c 54 osEvent ev = messageQueue.get(osWaitForever);
gunarthon 38:b760c09b311c 55 if (ev.status == osEventMessage)
gunarthon 38:b760c09b311c 56 {
gunarthon 38:b760c09b311c 57 message_t *message = (message_t*)ev.value.p;
gunarthon 38:b760c09b311c 58
gunarthon 38:b760c09b311c 59 double input = double(message->input) / heightResolution;
gunarthon 38:b760c09b311c 60
gunarthon 38:b760c09b311c 61 double newPwm = pidController->getPwm(input);
gunarthon 38:b760c09b311c 62
gunarthon 38:b760c09b311c 63 outPin.write(newPwm);
gunarthon 38:b760c09b311c 64
gunarthon 38:b760c09b311c 65 mpool.free(message);
gunarthon 38:b760c09b311c 66
gunarthon 38:b760c09b311c 67 lcd.cls();
gunarthon 38:b760c09b311c 68 lcd.printf("set in out");
gunarthon 38:b760c09b311c 69 lcd.locate(0,1);
gunarthon 38:b760c09b311c 70 lcd.printf("%d", int(1000*pidController->getSetPoint()));
gunarthon 38:b760c09b311c 71 lcd.locate(6,1);
gunarthon 38:b760c09b311c 72 lcd.printf("%d", int(1000*input));
gunarthon 38:b760c09b311c 73 lcd.locate(12,1);
gunarthon 38:b760c09b311c 74 lcd.printf("%d", int(1000*newPwm));
gunarthon 38:b760c09b311c 75 }
gunarthon 38:b760c09b311c 76 }
gunarthon 38:b760c09b311c 77 }
gunarthon 38:b760c09b311c 78 //------------------------------------------------------------------------------
gunarthon 38:b760c09b311c 79
gunarthon 38:b760c09b311c 80 void CommunicationMethode(void)
gunarthon 38:b760c09b311c 81 {
gunarthon 38:b760c09b311c 82 uint32_t input = 0;
gunarthon 38:b760c09b311c 83 while(true)
gunarthon 38:b760c09b311c 84 {
gunarthon 38:b760c09b311c 85 input = input>=1000 ? 0 : input+1;
gunarthon 38:b760c09b311c 86 message_t *message = mpool.alloc();
gunarthon 38:b760c09b311c 87 message->input = input;
gunarthon 38:b760c09b311c 88 messageQueue.put(message);
gunarthon 38:b760c09b311c 89 Thread::wait(100);
gunarthon 38:b760c09b311c 90 }
gunarthon 38:b760c09b311c 91 }
gunarthon 38:b760c09b311c 92
gunarthon 38:b760c09b311c 93 //------------------------------------------------------------------------------
gunarthon 38:b760c09b311c 94 //human-machine interface
gunarthon 38:b760c09b311c 95 void hmiMethode(void)
gunarthon 38:b760c09b311c 96 {
gunarthon 38:b760c09b311c 97 while(true)
gunarthon 38:b760c09b311c 98 {
gunarthon 38:b760c09b311c 99 double buttonsValue = buttons.read();
gunarthon 38:b760c09b311c 100
gunarthon 38:b760c09b311c 101 unsigned button = btnNONE;
gunarthon 38:b760c09b311c 102 if(buttonsValue < 0.08) //0.000
gunarthon 38:b760c09b311c 103 button = btnRIGHT;
gunarthon 38:b760c09b311c 104 else if(buttonsValue < 0.28) //0.170
gunarthon 38:b760c09b311c 105 button = btnUP;
gunarthon 38:b760c09b311c 106 else if(buttonsValue < 0.51) //0.397
gunarthon 38:b760c09b311c 107 button = btnDOWN;
gunarthon 38:b760c09b311c 108 else if(buttonsValue < 0.78) //0.621
gunarthon 38:b760c09b311c 109 button = btnLEFT;
gunarthon 38:b760c09b311c 110 else if(buttonsValue < 0.97) //0.936
gunarthon 38:b760c09b311c 111 button = btnSELECT;
gunarthon 38:b760c09b311c 112 else //1.000
gunarthon 38:b760c09b311c 113 button = btnNONE;
gunarthon 38:b760c09b311c 114
gunarthon 38:b760c09b311c 115
gunarthon 38:b760c09b311c 116 double prevSetPoint = pidController->getSetPoint();
gunarthon 38:b760c09b311c 117 double newSetPoint = prevSetPoint;
gunarthon 38:b760c09b311c 118 if(button == btnUP)
gunarthon 38:b760c09b311c 119 newSetPoint += 0.001;
gunarthon 38:b760c09b311c 120 else if(button == btnDOWN)
gunarthon 38:b760c09b311c 121 newSetPoint -= 0.001;
gunarthon 38:b760c09b311c 122 else if(button == btnLEFT)
gunarthon 38:b760c09b311c 123 newSetPoint -= 0.01;
gunarthon 38:b760c09b311c 124 else if(button == btnRIGHT)
gunarthon 38:b760c09b311c 125 newSetPoint += 0.01;
gunarthon 38:b760c09b311c 126 else if(button == btnSELECT)
gunarthon 38:b760c09b311c 127 newSetPoint = 0.5;
gunarthon 38:b760c09b311c 128
gunarthon 38:b760c09b311c 129 pidController->setSetPoint(newSetPoint);
gunarthon 38:b760c09b311c 130
gunarthon 38:b760c09b311c 131 Thread::wait(100);
gunarthon 38:b760c09b311c 132 }
gunarthon 38:b760c09b311c 133 }
gunarthon 38:b760c09b311c 134
gunarthon 38:b760c09b311c 135 //=============================Main Thread======================================
gunarthon 38:b760c09b311c 136 // main() runs in its own thread in the OS
gunarthon 38:b760c09b311c 137
gunarthon 38:b760c09b311c 138 int main() {
gunarthon 38:b760c09b311c 139
gunarthon 38:b760c09b311c 140 //led1.period(4.0f); // 4 second period
gunarthon 38:b760c09b311c 141
gunarthon 38:b760c09b311c 142 lcd.cls();
gunarthon 38:b760c09b311c 143 lcd.printf("HELLO");
gunarthon 38:b760c09b311c 144
gunarthon 38:b760c09b311c 145 pidController = new Pid(Kp, Ki, Kd);
gunarthon 38:b760c09b311c 146 pidController->setSetPoint(setPoint);
gunarthon 38:b760c09b311c 147
gunarthon 38:b760c09b311c 148 blueThread.start(BlueMethode);
gunarthon 38:b760c09b311c 149 pidThread.start(PidMethode);
gunarthon 38:b760c09b311c 150 communicationThread.start(CommunicationMethode);
gunarthon 38:b760c09b311c 151 hmiThread.start(hmiMethode);
gunarthon 38:b760c09b311c 152
gunarthon 38:b760c09b311c 153 while (true) {
gunarthon 38:b760c09b311c 154 led2 = !led2;
gunarthon 38:b760c09b311c 155 Thread::wait(1100);
gunarthon 38:b760c09b311c 156 }
gunarthon 38:b760c09b311c 157 }
gunarthon 38:b760c09b311c 158
gunarthon 38:b760c09b311c 159 //==============================================================================
gunarthon 38:b760c09b311c 160
gunarthon 38:b760c09b311c 161
gunarthon 38:b760c09b311c 162