Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

main.cpp@0:48c10918dabf, 2016-11-08 (annotated)

Committer:
RichardHoekstra
Date:
Tue Nov 08 18:49:07 2016 +0000
Revision:
0:48c10918dabf
Child:
1:9e6c4011eef6
First message;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RichardHoekstra 0:48c10918dabf 1 #include "mbed.h"
RichardHoekstra 0:48c10918dabf 2
RichardHoekstra 0:48c10918dabf 3 /* De motorcontroller heeft de volgende taken:
RichardHoekstra 0:48c10918dabf 4 1.
RichardHoekstra 0:48c10918dabf 5 1. Vraag de meest recente primaire sensor data op
RichardHoekstra 0:48c10918dabf 6 2. Voer het ingestelde curve programma uit
RichardHoekstra 0:48c10918dabf 7 3. Voer PID regelschema uit met set point uit de curve
RichardHoekstra 0:48c10918dabf 8 4. Herhaal
RichardHoekstra 0:48c10918dabf 9 2.
RichardHoekstra 0:48c10918dabf 10 1. Reageer op I2C commandos
RichardHoekstra 0:48c10918dabf 11 2. Pas curve modus aan
RichardHoekstra 0:48c10918dabf 12 3. Pas curve instellingen aan
RichardHoekstra 0:48c10918dabf 13 4. Herbereken curve
RichardHoekstra 0:48c10918dabf 14 */
RichardHoekstra 0:48c10918dabf 15
RichardHoekstra 0:48c10918dabf 16 int PID(int val, int set_val){
RichardHoekstra 0:48c10918dabf 17 //Gains
RichardHoekstra 0:48c10918dabf 18 const int Kp = 1,
RichardHoekstra 0:48c10918dabf 19 Ki = 1,
RichardHoekstra 0:48c10918dabf 20 Kd = 1;
RichardHoekstra 0:48c10918dabf 21 //Error
RichardHoekstra 0:48c10918dabf 22 int err = set_val - val;
RichardHoekstra 0:48c10918dabf 23
RichardHoekstra 0:48c10918dabf 24 //Proportional
RichardHoekstra 0:48c10918dabf 25 int p_err = err*Kp;
RichardHoekstra 0:48c10918dabf 26
RichardHoekstra 0:48c10918dabf 27 //Integral
RichardHoekstra 0:48c10918dabf 28 static int prev_i_err = 0;
RichardHoekstra 0:48c10918dabf 29 int i_err = prev_i_err+err;
RichardHoekstra 0:48c10918dabf 30 prev_i_err = i_err;
RichardHoekstra 0:48c10918dabf 31 i_err *= Ki;
RichardHoekstra 0:48c10918dabf 32
RichardHoekstra 0:48c10918dabf 33 //Derivative
RichardHoekstra 0:48c10918dabf 34 static int prev_err = 0;
RichardHoekstra 0:48c10918dabf 35 int d_err = Kd*(prev_err - err)/2;
RichardHoekstra 0:48c10918dabf 36 prev_err = err;
RichardHoekstra 0:48c10918dabf 37
RichardHoekstra 0:48c10918dabf 38 return p_err + i_err + d_err;
RichardHoekstra 0:48c10918dabf 39 }
RichardHoekstra 0:48c10918dabf 40
RichardHoekstra 0:48c10918dabf 41 //Curve variabelen
RichardHoekstra 0:48c10918dabf 42 const int points_in_curve = 100;
RichardHoekstra 0:48c10918dabf 43 static float curve[points_in_curve];
RichardHoekstra 0:48c10918dabf 44 static float curve_max = 120;
RichardHoekstra 0:48c10918dabf 45 static float curve_min = 80;
RichardHoekstra 0:48c10918dabf 46 static float frequency = 60;
RichardHoekstra 0:48c10918dabf 47
RichardHoekstra 0:48c10918dabf 48 float curve_arterial(){
RichardHoekstra 0:48c10918dabf 49 /*
RichardHoekstra 0:48c10918dabf 50 The curve is precalculated to reduce CPU time
RichardHoekstra 0:48c10918dabf 51 To adjust the frequency, the curve is simply executed more quickly.
RichardHoekstra 0:48c10918dabf 52 To adjust the curve_max and curve_min, the curve...
RichardHoekstra 0:48c10918dabf 53 */
RichardHoekstra 0:48c10918dabf 54 //Recalculate the curve
RichardHoekstra 0:48c10918dabf 55 for(int i=0;i<points_in_curve;i++){
RichardHoekstra 0:48c10918dabf 56 /* MATLAB
RichardHoekstra 0:48c10918dabf 57 t = linspace(0,1,100);
RichardHoekstra 0:48c10918dabf 58 t = t.*0.75;
RichardHoekstra 0:48c10918dabf 59 y1 = (0<=t).*(0.3>t).*(80 + 40*sin(8*t))
RichardHoekstra 0:48c10918dabf 60 y2 = (0.3<=t).*(0.4>t).*(107)
RichardHoekstra 0:48c10918dabf 61 y3 = (0.4<=t).*(-35*sqrt(t-0.45)+107)
RichardHoekstra 0:48c10918dabf 62 ytot = y1+y2+y3
RichardHoekstra 0:48c10918dabf 63 */
RichardHoekstra 0:48c10918dabf 64 // C++?
RichardHoekstra 0:48c10918dabf 65 /*
RichardHoekstra 0:48c10918dabf 66 Je hebt een minimale en maximale waarde van de curve
RichardHoekstra 0:48c10918dabf 67 De curve wordt zo berekend dat van begin tot eind het equivalent van 'full_interval_ms' is
RichardHoekstra 0:48c10918dabf 68 en niet onder of boven de minimale en maximale waarde komt
RichardHoekstra 0:48c10918dabf 69 De curve moet verder nog lijken op een arteriele drukcruve
RichardHoekstra 0:48c10918dabf 70
RichardHoekstra 0:48c10918dabf 71 */
RichardHoekstra 0:48c10918dabf 72 }
RichardHoekstra 0:48c10918dabf 73 }
RichardHoekstra 0:48c10918dabf 74
RichardHoekstra 0:48c10918dabf 75 float curve_sinus(){
RichardHoekstra 0:48c10918dabf 76 float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude
RichardHoekstra 0:48c10918dabf 77 //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten
RichardHoekstra 0:48c10918dabf 78 float offset = (curve_max+curve_min)/2;
RichardHoekstra 0:48c10918dabf 79 //sin(x) heeft een periode van 2Pi, dus om een periode van 1ms te krijgen maken we daar 2*Pi van
RichardHoekstra 0:48c10918dabf 80 //Dat keer de periode in miliseconden (1000/frequentie) geeft ons de juiste uitdrukking voor de sinus
RichardHoekstra 0:48c10918dabf 81 float period = 2*3.14159*(1/(1000/(frequency));
RichardHoekstra 0:48c10918dabf 82 for(int i=0;i<points_in_curve;i++){
RichardHoekstra 0:48c10918dabf 83 curve[i] = offset+amplitude*sin(period*x);
RichardHoekstra 0:48c10918dabf 84 }
RichardHoekstra 0:48c10918dabf 85 }
RichardHoekstra 0:48c10918dabf 86
RichardHoekstra 0:48c10918dabf 87 //The array has 100 points to execute
RichardHoekstra 0:48c10918dabf 88 //If you take 1ms per point, the entire pulse takes 100ms
RichardHoekstra 0:48c10918dabf 89 //Therefore, the frequency is 10Hz
RichardHoekstra 0:48c10918dabf 90 //To go from frequency to ms per point
RichardHoekstra 0:48c10918dabf 91 int point_interval_ms = (1000/set_frequency)/points_in_curve;
RichardHoekstra 0:48c10918dabf 92 int full_interval_ms = 1000/set_frequency;
RichardHoekstra 0:48c10918dabf 93 int main() {
RichardHoekstra 0:48c10918dabf 94 Timer t_point;
RichardHoekstra 0:48c10918dabf 95 Timer t_full;
RichardHoekstra 0:48c10918dabf 96 t_point.start();
RichardHoekstra 0:48c10918dabf 97 t_full.start();
RichardHoekstra 0:48c10918dabf 98 while(1) {
RichardHoekstra 0:48c10918dabf 99 if(t_point.read_ms() > point_interval_ms){
RichardHoekstra 0:48c10918dabf 100 PID(readfromsensor,curve[t_full.read_ms());
RichardHoekstra 0:48c10918dabf 101 t_point.reset();
RichardHoekstra 0:48c10918dabf 102 }
RichardHoekstra 0:48c10918dabf 103 if(t_full.read_ms() > full_interval_ms){
RichardHoekstra 0:48c10918dabf 104 t_full.reset();
RichardHoekstra 0:48c10918dabf 105 }
RichardHoekstra 0:48c10918dabf 106 }
RichardHoekstra 0:48c10918dabf 107 }