![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
De motorcontroller van het TLS2 project.
Control.h@8:648c3963a8e0, 2016-11-22 (annotated)
- Committer:
- RichardHoekstra
- Date:
- Tue Nov 22 22:01:20 2016 +0000
- Revision:
- 8:648c3963a8e0
- Child:
- 9:1bdf5107920f
Transplant succesful. Working on getting the control loop working by creating a new class to handle all the junk and keep stuff tidy like
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RichardHoekstra | 8:648c3963a8e0 | 1 | #include "mbed.h" |
RichardHoekstra | 8:648c3963a8e0 | 2 | #define CURVE_BUFFER_SIZE 100 |
RichardHoekstra | 8:648c3963a8e0 | 3 | class Control { |
RichardHoekstra | 8:648c3963a8e0 | 4 | private: |
RichardHoekstra | 8:648c3963a8e0 | 5 | //----- SETTINGS ----- |
RichardHoekstra | 8:648c3963a8e0 | 6 | //Curve |
RichardHoekstra | 8:648c3963a8e0 | 7 | enum curve_t { OFF=0, CONSTANT_PRESSURE, CONSTANT_FLOW, CONSTANT_SPEED, MODE_SINUS, MODE_ARTERIAL |
RichardHoekstra | 8:648c3963a8e0 | 8 | } curve_mode; |
RichardHoekstra | 8:648c3963a8e0 | 9 | float curve_buffer[CURVE_BUFFER_SIZE]; |
RichardHoekstra | 8:648c3963a8e0 | 10 | float curve_min; //[mmHg] |
RichardHoekstra | 8:648c3963a8e0 | 11 | float curve_max; //[mmHg] |
RichardHoekstra | 8:648c3963a8e0 | 12 | float curve_period; //[ms] |
RichardHoekstra | 8:648c3963a8e0 | 13 | int curve_step; |
RichardHoekstra | 8:648c3963a8e0 | 14 | |
RichardHoekstra | 8:648c3963a8e0 | 15 | //Motor output |
RichardHoekstra | 8:648c3963a8e0 | 16 | float gradient; //pressure/percentagemaxpowerorjustvoltiguess |
RichardHoekstra | 8:648c3963a8e0 | 17 | PwmOut motorOut; |
RichardHoekstra | 8:648c3963a8e0 | 18 | |
RichardHoekstra | 8:648c3963a8e0 | 19 | |
RichardHoekstra | 8:648c3963a8e0 | 20 | //----- PRIVATE FUNCTIONS ----- |
RichardHoekstra | 8:648c3963a8e0 | 21 | void outputToPin(float input){ |
RichardHoekstra | 8:648c3963a8e0 | 22 | float temp = gradient*input; |
RichardHoekstra | 8:648c3963a8e0 | 23 | if(temp > 1){ |
RichardHoekstra | 8:648c3963a8e0 | 24 | motorOut = 1; |
RichardHoekstra | 8:648c3963a8e0 | 25 | } else if(temp < 0){ |
RichardHoekstra | 8:648c3963a8e0 | 26 | motorOut = 0; |
RichardHoekstra | 8:648c3963a8e0 | 27 | } else { |
RichardHoekstra | 8:648c3963a8e0 | 28 | motorOut = temp; |
RichardHoekstra | 8:648c3963a8e0 | 29 | } |
RichardHoekstra | 8:648c3963a8e0 | 30 | } |
RichardHoekstra | 8:648c3963a8e0 | 31 | void run(){ |
RichardHoekstra | 8:648c3963a8e0 | 32 | switch(curve_mode){ |
RichardHoekstra | 8:648c3963a8e0 | 33 | case CONSTANT_PRESSURE: |
RichardHoekstra | 8:648c3963a8e0 | 34 | break; |
RichardHoekstra | 8:648c3963a8e0 | 35 | case CONSTANT_FLOW: |
RichardHoekstra | 8:648c3963a8e0 | 36 | break; |
RichardHoekstra | 8:648c3963a8e0 | 37 | case CONSTANT_SPEED: |
RichardHoekstra | 8:648c3963a8e0 | 38 | break; |
RichardHoekstra | 8:648c3963a8e0 | 39 | case MODE_SINUS: |
RichardHoekstra | 8:648c3963a8e0 | 40 | outputToPin(curve_buffer[curve_step]); |
RichardHoekstra | 8:648c3963a8e0 | 41 | curve_step++; |
RichardHoekstra | 8:648c3963a8e0 | 42 | if(curve_step >= CURVE_BUFFER_SIZE){ |
RichardHoekstra | 8:648c3963a8e0 | 43 | curve_step = 0; |
RichardHoekstra | 8:648c3963a8e0 | 44 | } |
RichardHoekstra | 8:648c3963a8e0 | 45 | break; |
RichardHoekstra | 8:648c3963a8e0 | 46 | case MODE_ARTERIAL: |
RichardHoekstra | 8:648c3963a8e0 | 47 | break; |
RichardHoekstra | 8:648c3963a8e0 | 48 | } |
RichardHoekstra | 8:648c3963a8e0 | 49 | |
RichardHoekstra | 8:648c3963a8e0 | 50 | } |
RichardHoekstra | 8:648c3963a8e0 | 51 | |
RichardHoekstra | 8:648c3963a8e0 | 52 | public: |
RichardHoekstra | 8:648c3963a8e0 | 53 | Control(PinName n_pin) : motorOut(n_pin){ |
RichardHoekstra | 8:648c3963a8e0 | 54 | curve_step = 0; |
RichardHoekstra | 8:648c3963a8e0 | 55 | curve_min = 80; //[mmHg] |
RichardHoekstra | 8:648c3963a8e0 | 56 | curve_max = 120; //[mmHg] |
RichardHoekstra | 8:648c3963a8e0 | 57 | curve_period = 1000; //[ms] |
RichardHoekstra | 8:648c3963a8e0 | 58 | } |
RichardHoekstra | 8:648c3963a8e0 | 59 | void normalize(AnalogIn& sensor){ |
RichardHoekstra | 8:648c3963a8e0 | 60 | //f'(x) = a |
RichardHoekstra | 8:648c3963a8e0 | 61 | //f'(x) = dy/dx |
RichardHoekstra | 8:648c3963a8e0 | 62 | //g(x) = a*x+b |
RichardHoekstra | 8:648c3963a8e0 | 63 | //volt(pressure) = gradient*pressure+offset |
RichardHoekstra | 8:648c3963a8e0 | 64 | //gradient = dVolt/dPressure |
RichardHoekstra | 8:648c3963a8e0 | 65 | float p1 = 0, |
RichardHoekstra | 8:648c3963a8e0 | 66 | p2 = 0; |
RichardHoekstra | 8:648c3963a8e0 | 67 | motorOut = 0.1; |
RichardHoekstra | 8:648c3963a8e0 | 68 | wait(2); |
RichardHoekstra | 8:648c3963a8e0 | 69 | p1 = sensor.read(); |
RichardHoekstra | 8:648c3963a8e0 | 70 | motorOut = 0.2; |
RichardHoekstra | 8:648c3963a8e0 | 71 | wait(2); |
RichardHoekstra | 8:648c3963a8e0 | 72 | p2 = sensor.read(); |
RichardHoekstra | 8:648c3963a8e0 | 73 | gradient = (p2-p1)/((float)0.1); |
RichardHoekstra | 8:648c3963a8e0 | 74 | } |
RichardHoekstra | 8:648c3963a8e0 | 75 | void set_curve(float min, float max, float period){ |
RichardHoekstra | 8:648c3963a8e0 | 76 | curve_min = min; |
RichardHoekstra | 8:648c3963a8e0 | 77 | curve_max = max; |
RichardHoekstra | 8:648c3963a8e0 | 78 | curve_period = period; |
RichardHoekstra | 8:648c3963a8e0 | 79 | } |
RichardHoekstra | 8:648c3963a8e0 | 80 | void set_mode(int mode){ |
RichardHoekstra | 8:648c3963a8e0 | 81 | curve_mode = (curve_t)mode; |
RichardHoekstra | 8:648c3963a8e0 | 82 | } |
RichardHoekstra | 8:648c3963a8e0 | 83 | void rebuild_buffer_sinus(){ |
RichardHoekstra | 8:648c3963a8e0 | 84 | float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude |
RichardHoekstra | 8:648c3963a8e0 | 85 | //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten |
RichardHoekstra | 8:648c3963a8e0 | 86 | float offset = (curve_max+curve_min)/2; |
RichardHoekstra | 8:648c3963a8e0 | 87 | //Genereer een volle periode en zet het in de buffer |
RichardHoekstra | 8:648c3963a8e0 | 88 | float step = 2*3.1415926/CURVE_BUFFER_SIZE; |
RichardHoekstra | 8:648c3963a8e0 | 89 | for(int i=0;i<CURVE_BUFFER_SIZE;i++){ |
RichardHoekstra | 8:648c3963a8e0 | 90 | curve_buffer[i] = offset+amplitude*sin(step*i); |
RichardHoekstra | 8:648c3963a8e0 | 91 | } |
RichardHoekstra | 8:648c3963a8e0 | 92 | } |
RichardHoekstra | 8:648c3963a8e0 | 93 | void rebuild_buffer_arterial(){ |
RichardHoekstra | 8:648c3963a8e0 | 94 | //Coming to a cinema near you... soon! |
RichardHoekstra | 8:648c3963a8e0 | 95 | } |
RichardHoekstra | 8:648c3963a8e0 | 96 | }; |