Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
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?
User | Revision | Line number | New 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 | } |