Wessel Klumperman / Mbed 2 deprecated hoi_gpio

PID Controller

Dependencies:   HIDScope mbed

main.cpp@0:d566a0281325, 2018-10-22 (annotated)

Committer:
Wimboo
Date:
Mon Oct 22 08:33:23 2018 +0000
Revision:
0:d566a0281325
Lijkt wel oke te werken;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Wimboo 0:d566a0281325 1 #include "mbed.h"
Wimboo 0:d566a0281325 2 #include "HIDScope.h"
Wimboo 0:d566a0281325 3
Wimboo 0:d566a0281325 4 DigitalOut gpo(D0);
Wimboo 0:d566a0281325 5 DigitalOut led(LED_RED);
Wimboo 0:d566a0281325 6 HIDScope scope(2);
Wimboo 0:d566a0281325 7
Wimboo 0:d566a0281325 8 const float Kp = 2;
Wimboo 0:d566a0281325 9 const float Ki = 0.2;
Wimboo 0:d566a0281325 10 const float Kd = 0;
Wimboo 0:d566a0281325 11 const float Timestep = 0.001;
Wimboo 0:d566a0281325 12 float G = 15; //input
Wimboo 0:d566a0281325 13 float Output = 0 ; //Starting value
Wimboo 0:d566a0281325 14 float P = 0 ; // Starting value
Wimboo 0:d566a0281325 15 float e1 = 0 ; //Starting value
Wimboo 0:d566a0281325 16 float e2 = 0 ; // Starting value
Wimboo 0:d566a0281325 17 float e3;
Wimboo 0:d566a0281325 18 float Output_Last; // Remember previous position
Wimboo 0:d566a0281325 19 float Y; // Value that is outputted to motor control
Wimboo 0:d566a0281325 20 float P_Last = 0; // Starting position
Wimboo 0:d566a0281325 21 const float Max_Speed = 400; //Max speed of the motor
Wimboo 0:d566a0281325 22 int main()
Wimboo 0:d566a0281325 23 {
Wimboo 0:d566a0281325 24 while (true) {
Wimboo 0:d566a0281325 25 P_Last = P;
Wimboo 0:d566a0281325 26 e1 = e2;
Wimboo 0:d566a0281325 27 e2 = e3;
Wimboo 0:d566a0281325 28 e3 = G - P;
Wimboo 0:d566a0281325 29 Output_Last = Output;
Wimboo 0:d566a0281325 30 Output = Kp * (e3 - e2) + Output_Last +Ki * e3 + Kd * (e3 - 2*e2 + e1);
Wimboo 0:d566a0281325 31 Y = Output;
Wimboo 0:d566a0281325 32 if (Output >= 1){
Wimboo 0:d566a0281325 33 Y = 1;
Wimboo 0:d566a0281325 34 }
Wimboo 0:d566a0281325 35 else if (Output <= -1){
Wimboo 0:d566a0281325 36 Y = -1;
Wimboo 0:d566a0281325 37 }
Wimboo 0:d566a0281325 38 P = P_Last + Y * Timestep * Max_Speed;
Wimboo 0:d566a0281325 39 wait(0.1f);
Wimboo 0:d566a0281325 40 scope.set(0,Output);
Wimboo 0:d566a0281325 41 scope.set(1,P);
Wimboo 0:d566a0281325 42 scope.send();
Wimboo 0:d566a0281325 43 }
Wimboo 0:d566a0281325 44
Wimboo 0:d566a0281325 45 }