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@39:e4f5710b2f31, 2017-06-28 (annotated)

Committer:
gunarthon
Date:
Wed Jun 28 16:36:16 2017 +0000
Revision:
39:e4f5710b2f31
Parent:
38:b760c09b311c
Child:
40:19d51f6e6800
added serial

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