Gunar Kroeger
Receives a measured height of a ping-pong ball from a PC, and uses it to control the PWM of a fan to keep the height as set with the keypad. Information is shown on the LCD
Fork of
mbed-os-example-mbed5-blinky
by 
mbed-os-examples
Revision 40:19d51f6e6800, committed 2017-07-02
- Comitter:
- gunarthon
- Date:
- Sun Jul 02 19:01:07 2017 +0000
- Parent:
- 39:e4f5710b2f31
- Child:
- 41:3bc2a3885b9d
- Commit message:
- pid alterations
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| pid.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Wed Jun 28 16:36:16 2017 +0000
+++ b/main.cpp Sun Jul 02 19:01:07 2017 +0000
@@ -6,21 +6,28 @@
//definitions
typedef struct {
- uint32_t input;
+ double input;
} message_t;
-const int heightResolution = 1024;
+const int heightResolution = 1000;
const int BAUDRATE = 115200;
+const int cameraFPS = 60;
+const int displayRefreshTime = 200;
const double setPoint = 0.5;
-const double Kp = 1;
-const double Ki = 0.2;
-const double Kd = 0.3;
+const double Kp = 0.02;
+const double Ki = 0;
+const double Kd = 0;
-enum {btnRIGHT, btnUP, btnDOWN, btnLEFT, btnSELECT, btnNONE};
+const int simulationSeconds = 10;
+
+enum {btnRIGHT, btnUP, btnDOWN, btnLEFT, btnSELECT, btnNONE, btnSIZE};
+enum {menuSETPOINT, menuPID, menuSIMULATE, menuSIZE};
+enum {parameterKP, parameterKI, parameterKD, parameterSIZE};
+enum {simulSTEP, simulRAMP, simulNONE, simulSIZE};
//Pins
PwmOut outPin(D3);
-DigitalOut led2(LED2);
+//DigitalOut led2(LED2);
DigitalOut led3(LED3);
AnalogIn buttons(A0);
@@ -29,6 +36,7 @@
Thread pidThread;
Thread communicationThread;
Thread hmiThread;
+Thread setPointThread;
//Global variables
TextLCD lcd(D8, D9, D4, D5, D6, D7); // rs, e, d4-d7
@@ -36,16 +44,72 @@
MemoryPool<message_t, 16> mpool;
Queue<message_t,16> messageQueue;
+//Menu settings
+int menu = menuSETPOINT;
+int parameter = parameterKP;
+int simul = simulNONE;
+
void SerialCallback(int);
//=============================Thread Methodes==================================
-
+void setPointMethode(void)
+{
+ while(1)
+ {
+ pidController->setSetPoint(0.2);
+ Thread::wait(20000);
+ pidController->setSetPoint(0.8);
+ Thread::wait(20000);
+ }
+}
void BlueMethode(void)
{
while(true)
{
led3 = !led3;
- Thread::wait(500);
+
+ double lastPwm = pidController->getLastPwm();
+ double lastInput = pidController->getLastInput();
+
+ if(menu == menuSETPOINT)
+ {
+ lcd.cls();
+ lcd.printf("set in out");
+ lcd.locate(0,1);
+ lcd.printf("%d", int(1000*pidController->getSetPoint()));
+ lcd.locate(6,1);
+ lcd.printf("%d", int(1000*lastInput));
+ lcd.locate(12,1);
+ lcd.printf("%d", int(1000*lastPwm));
+ }
+ else if(menu == menuPID)
+ {
+ lcd.cls();
+ lcd.printf(" Kp Ki Kd");
+ lcd.locate(6*parameter, 0);
+ lcd.printf("*");
+ lcd.locate(0,1);
+ lcd.printf("%d", int(1000*pidController->getKp()));
+ lcd.locate(6,1);
+ lcd.printf("%d", int(1000*pidController->getKi()));
+ lcd.locate(12,1);
+ lcd.printf("%d", int(1000*pidController->getKd()));
+ }
+ else if(menu == menuSIMULATE)
+ {
+ lcd.cls();
+ if(simul == simulNONE)
+ lcd.printf("u:STEP dwn:RAMP");
+ else
+ lcd.printf("set $in$ out");
+ lcd.locate(0,1);
+ lcd.printf("%d", int(1000*pidController->getSetPoint()));
+ lcd.locate(6,1);
+ lcd.printf("%d", int(1000*lastInput));
+ lcd.locate(12,1);
+ lcd.printf("%d", int(1000*lastPwm));
+ }
+ Thread::wait(displayRefreshTime);
}
}
@@ -60,22 +124,13 @@
{
message_t *message = (message_t*)ev.value.p;
- double input = double(message->input) / heightResolution;
+ double input = message->input;
double newPwm = pidController->getPwm(input);
outPin.write(newPwm);
mpool.free(message);
-
- lcd.cls();
- lcd.printf("set in out");
- lcd.locate(0,1);
- lcd.printf("%d", int(1000*pidController->getSetPoint()));
- lcd.locate(6,1);
- lcd.printf("%d", int(1000*input));
- lcd.locate(12,1);
- lcd.printf("%d", int(1000*newPwm));
}
}
}
@@ -86,7 +141,7 @@
Serial serial(USBTX, USBRX);
serial.baud(BAUDRATE);
serial.format(8, SerialBase::None, 1);
- uint16_t input = 0;
+ double input = 0;
//event_callback_t functionpointer;
//functionpointer.attach(&SerialCallback);
@@ -94,7 +149,7 @@
while(true)
{
//serial.read(&input, 1, functionpointer);
- input = serial.getc()+(serial.getc()<<8);
+ input = double(serial.getc()+(serial.getc()<<8)) / heightResolution;
//lcd.cls();
//lcd.printf("%d", int(input));
message_t *message = mpool.alloc();
@@ -108,12 +163,8 @@
//ut =
}*/
-//------------------------------------------------------------------------------
-//human-machine interface
-void hmiMethode(void)
+unsigned readButtons()
{
- while(true)
- {
double buttonsValue = buttons.read();
unsigned button = btnNONE;
@@ -130,22 +181,115 @@
else //1.000
button = btnNONE;
+ return button;
+
+}
+//------------------------------------------------------------------------------
+//human-machine interface
+void hmiMethode(void)
+{
+
+ unsigned button = btnNONE;
+ while(true)
+ {
+ //while(button == btnNONE)
+ //{
+ button = readButtons();
+ // Thread::wait(10);
+ //}
+
double prevSetPoint = pidController->getSetPoint();
double newSetPoint = prevSetPoint;
- if(button == btnUP)
- newSetPoint += 0.001;
- else if(button == btnDOWN)
- newSetPoint -= 0.001;
- else if(button == btnLEFT)
- newSetPoint -= 0.01;
- else if(button == btnRIGHT)
- newSetPoint += 0.01;
- else if(button == btnSELECT)
- newSetPoint = 0.5;
+
+ if(button == btnSELECT)
+ {
+ menu = (menu+1) % menuSIZE;
+ simul = simulNONE;
+ }
+ else if(menu == menuSETPOINT)
+ {
+ if(button == btnUP)
+ newSetPoint += 0.001;
+ else if(button == btnDOWN)
+ newSetPoint -= 0.001;
+ else if(button == btnLEFT)
+ newSetPoint -= 0.01;
+ else if(button == btnRIGHT)
+ newSetPoint += 0.01;
+
+ pidController->setSetPoint(newSetPoint);
+ }
+ else if(menu == menuPID)
+ {
+ if(button == btnUP)
+ {
+ if(parameter == parameterKP)
+ pidController->addKp(0.001);
+ else if(parameter == parameterKI)
+ pidController->addKi(0.001);
+ else if(parameter == parameterKD)
+ pidController->addKd(0.001);
+ }
+ else if(button == btnDOWN)
+ {
+ if(parameter == parameterKP)
+ pidController->addKp(-0.001);
+ else if(parameter == parameterKI)
+ pidController->addKi(-0.001);
+ else if(parameter == parameterKD)
+ pidController->addKd(-0.001);
+ }
+ else if(button == btnLEFT)
+ parameter = (parameter+1) % parameterSIZE;
+ else if(button == btnRIGHT)
+ pidController->setParameters(Kp, Ki, Kd);
+ }
+
+ else if(menu == menuSIMULATE)
+ {
+ if(button == btnUP) //step
+ {
+ simul = simulSTEP;
+ for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
+ {
+ double input = i < cameraFPS * simulationSeconds / 2 ? 0 : 1;
+ message_t *message = mpool.alloc();
+ message->input = input;
+ messageQueue.put(message);
+
+ Thread::wait(1000/cameraFPS);
+ }
+ simul = simulNONE;
+ }
+ else if(button == btnDOWN) //ramp
+ {
+ simul = simulRAMP;
+ for(unsigned i = 0; i < cameraFPS * simulationSeconds; i++)
+ {
+ double input = double(i) / ((cameraFPS * simulationSeconds)-1);
+ message_t *message = mpool.alloc();
+ message->input = input;
+ messageQueue.put(message);
+
+ Thread::wait(1000/cameraFPS);
+ }
+ simul = simulNONE;
+ }
+ else if(button == btnLEFT) //ramp
+ {
+ setPointThread.start(setPointMethode);
+ }
+ else if(button == btnRIGHT)
+ setPointThread.terminate();
- pidController->setSetPoint(newSetPoint);
+ }
+ /*while(button != btnNONE)
+ {
+ button = readButtons();
+ Thread::wait(10);
+ }*/
Thread::wait(100);
}
}
@@ -169,7 +313,6 @@
hmiThread.start(hmiMethode);
while (true) {
- led2 = !led2;
Thread::wait(1100);
}
}
--- a/pid.h Wed Jun 28 16:36:16 2017 +0000
+++ b/pid.h Sun Jul 02 19:01:07 2017 +0000
@@ -1,6 +1,10 @@
#ifndef _PID_H_
#define _PID_H_
+#include "mbed.h"
+
+
+DigitalOut led2(LED2);
//http://coder-tronics.com/pid-tutorial-c-code-example-pt2/
class Pid
@@ -12,6 +16,9 @@
double integrator, derivator;
double maxInteg, minInteg;
double maxPwm, minPwm;
+ double lastInput;
+ double lastPwm;
+ double middlePoint;
public:
Pid(double Kp, double Ki, double Kd)
@@ -19,30 +26,40 @@
this->Kp = Kp;
this->Ki = Ki;
this->Kd = Kd;
-
+ this->middlePoint = 0.08;
integrator = 0;
derivator = 0;
- maxInteg = 3;
- minInteg = -3;
+ maxInteg = 1.0;
+ minInteg = -1.0;
maxPwm = 1;
minPwm = 0.05;
+ lastInput = 0.5;
+ lastPwm = 0.5;
}
-
- //desired value
- void setSetPoint(double setPoint)
+ void addKp(double value) {Kp += value;}
+ void addKi(double value) {Ki += value;}
+ void addKd(double value) {Kd += value;}
+ double getKp() {return Kp;}
+ double getKi() {return Ki;}
+ double getKd() {return Kd;}
+ void setParameters(double Kp, double Ki, double Kd)
{
- this->setPoint = setPoint;
+ this->Kp = Kp;
+ this->Ki = Ki;
+ this->Kd = Kd;
}
- double getSetPoint()
- {
- return setPoint;
- }
+ //desired value
+ void setSetPoint(double setPoint) {this->setPoint = setPoint;}
+ double getSetPoint() {return setPoint;}
+ double getLastInput() {return lastInput;}
+ double getLastPwm() {return lastPwm;}
//values from 0-1
double getPwm(double input)
{
+ lastInput = input;
double errorValue = setPoint - input;
//P term
@@ -51,20 +68,25 @@
//I term
integrator += errorValue;
- integrator = integrator>maxInteg? maxInteg : integrator;
- integrator = integrator<minInteg? minInteg : integrator;
+ double I = (Ki/60.0) * integrator;
- double I = Ki * integrator;
+ if(I > maxInteg || I < minInteg)
+ led2 = 1;
+ else led2 = 0;
+
+ I = I>maxInteg? maxInteg : I;
+ I = I<minInteg? minInteg : I;
//D term
double D = Kd * (derivator - errorValue);
derivator = errorValue;
//output
- double pwm = P + I + D;
+ double pwm = P + I + D + middlePoint;
pwm = pwm>maxPwm? maxPwm : pwm;
pwm = pwm<minPwm? minPwm : pwm;
+ lastPwm = pwm;
return pwm;
}