Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
Control.h@9:1bdf5107920f, 2016-11-22 (annotated)
- Committer:
- RichardHoekstra
- Date:
- Tue Nov 22 22:34:18 2016 +0000
- Revision:
- 9:1bdf5107920f
- Parent:
- 8:648c3963a8e0
OOP'd this bitch up
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 | 9:1bdf5107920f | 14 | int curve_step_ms; //The time it takes to make one step |
RichardHoekstra | 9:1bdf5107920f | 15 | |
RichardHoekstra | 9:1bdf5107920f | 16 | //Constant variables |
RichardHoekstra | 9:1bdf5107920f | 17 | float constant_pressure; |
RichardHoekstra | 9:1bdf5107920f | 18 | float constant_flow; |
RichardHoekstra | 9:1bdf5107920f | 19 | float constant_speed; |
RichardHoekstra | 9:1bdf5107920f | 20 | |
RichardHoekstra | 9:1bdf5107920f | 21 | //Ticker |
RichardHoekstra | 9:1bdf5107920f | 22 | Ticker t; |
RichardHoekstra | 8:648c3963a8e0 | 23 | |
RichardHoekstra | 8:648c3963a8e0 | 24 | //Motor output |
RichardHoekstra | 8:648c3963a8e0 | 25 | float gradient; //pressure/percentagemaxpowerorjustvoltiguess |
RichardHoekstra | 8:648c3963a8e0 | 26 | PwmOut motorOut; |
RichardHoekstra | 8:648c3963a8e0 | 27 | |
RichardHoekstra | 8:648c3963a8e0 | 28 | |
RichardHoekstra | 8:648c3963a8e0 | 29 | //----- PRIVATE FUNCTIONS ----- |
RichardHoekstra | 8:648c3963a8e0 | 30 | void outputToPin(float input){ |
RichardHoekstra | 8:648c3963a8e0 | 31 | float temp = gradient*input; |
RichardHoekstra | 8:648c3963a8e0 | 32 | if(temp > 1){ |
RichardHoekstra | 8:648c3963a8e0 | 33 | motorOut = 1; |
RichardHoekstra | 8:648c3963a8e0 | 34 | } else if(temp < 0){ |
RichardHoekstra | 8:648c3963a8e0 | 35 | motorOut = 0; |
RichardHoekstra | 8:648c3963a8e0 | 36 | } else { |
RichardHoekstra | 8:648c3963a8e0 | 37 | motorOut = temp; |
RichardHoekstra | 8:648c3963a8e0 | 38 | } |
RichardHoekstra | 8:648c3963a8e0 | 39 | } |
RichardHoekstra | 8:648c3963a8e0 | 40 | void run(){ |
RichardHoekstra | 8:648c3963a8e0 | 41 | switch(curve_mode){ |
RichardHoekstra | 8:648c3963a8e0 | 42 | case CONSTANT_PRESSURE: |
RichardHoekstra | 8:648c3963a8e0 | 43 | break; |
RichardHoekstra | 8:648c3963a8e0 | 44 | case CONSTANT_FLOW: |
RichardHoekstra | 8:648c3963a8e0 | 45 | break; |
RichardHoekstra | 8:648c3963a8e0 | 46 | case CONSTANT_SPEED: |
RichardHoekstra | 8:648c3963a8e0 | 47 | break; |
RichardHoekstra | 8:648c3963a8e0 | 48 | case MODE_SINUS: |
RichardHoekstra | 8:648c3963a8e0 | 49 | outputToPin(curve_buffer[curve_step]); |
RichardHoekstra | 8:648c3963a8e0 | 50 | curve_step++; |
RichardHoekstra | 8:648c3963a8e0 | 51 | if(curve_step >= CURVE_BUFFER_SIZE){ |
RichardHoekstra | 8:648c3963a8e0 | 52 | curve_step = 0; |
RichardHoekstra | 8:648c3963a8e0 | 53 | } |
RichardHoekstra | 8:648c3963a8e0 | 54 | break; |
RichardHoekstra | 8:648c3963a8e0 | 55 | case MODE_ARTERIAL: |
RichardHoekstra | 9:1bdf5107920f | 56 | break; |
RichardHoekstra | 9:1bdf5107920f | 57 | default: |
RichardHoekstra | 9:1bdf5107920f | 58 | break; |
RichardHoekstra | 8:648c3963a8e0 | 59 | } |
RichardHoekstra | 8:648c3963a8e0 | 60 | |
RichardHoekstra | 8:648c3963a8e0 | 61 | } |
RichardHoekstra | 9:1bdf5107920f | 62 | void rebuild_buffer(){ |
RichardHoekstra | 9:1bdf5107920f | 63 | if(curve_mode == MODE_SINUS){ |
RichardHoekstra | 9:1bdf5107920f | 64 | float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude |
RichardHoekstra | 9:1bdf5107920f | 65 | //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten |
RichardHoekstra | 9:1bdf5107920f | 66 | float offset = (curve_max+curve_min)/2; |
RichardHoekstra | 9:1bdf5107920f | 67 | //Genereer een volle periode en zet het in de buffer |
RichardHoekstra | 9:1bdf5107920f | 68 | float step = 2*3.1415926/CURVE_BUFFER_SIZE; |
RichardHoekstra | 9:1bdf5107920f | 69 | for(int i=0;i<CURVE_BUFFER_SIZE;i++){ |
RichardHoekstra | 9:1bdf5107920f | 70 | curve_buffer[i] = offset+amplitude*sin(step*i); |
RichardHoekstra | 9:1bdf5107920f | 71 | } |
RichardHoekstra | 9:1bdf5107920f | 72 | } else if(curve_mode == MODE_ARTERIAL){ |
RichardHoekstra | 9:1bdf5107920f | 73 | //Coming to a cinema near you... soon! |
RichardHoekstra | 9:1bdf5107920f | 74 | } |
RichardHoekstra | 9:1bdf5107920f | 75 | } |
RichardHoekstra | 8:648c3963a8e0 | 76 | |
RichardHoekstra | 8:648c3963a8e0 | 77 | public: |
RichardHoekstra | 8:648c3963a8e0 | 78 | Control(PinName n_pin) : motorOut(n_pin){ |
RichardHoekstra | 8:648c3963a8e0 | 79 | curve_step = 0; |
RichardHoekstra | 8:648c3963a8e0 | 80 | curve_min = 80; //[mmHg] |
RichardHoekstra | 8:648c3963a8e0 | 81 | curve_max = 120; //[mmHg] |
RichardHoekstra | 8:648c3963a8e0 | 82 | curve_period = 1000; //[ms] |
RichardHoekstra | 9:1bdf5107920f | 83 | motorOut.period_ms(1); //1kHz |
RichardHoekstra | 9:1bdf5107920f | 84 | rebuild_buffer(); |
RichardHoekstra | 9:1bdf5107920f | 85 | } |
RichardHoekstra | 9:1bdf5107920f | 86 | void start(){ |
RichardHoekstra | 9:1bdf5107920f | 87 | t.attach(callback(this, &Control::run), (curve_step_ms/1000.0)); |
RichardHoekstra | 9:1bdf5107920f | 88 | } |
RichardHoekstra | 9:1bdf5107920f | 89 | void stop(){ |
RichardHoekstra | 9:1bdf5107920f | 90 | t.detach(); |
RichardHoekstra | 9:1bdf5107920f | 91 | } |
RichardHoekstra | 8:648c3963a8e0 | 92 | void normalize(AnalogIn& sensor){ |
RichardHoekstra | 8:648c3963a8e0 | 93 | //f'(x) = a |
RichardHoekstra | 8:648c3963a8e0 | 94 | //f'(x) = dy/dx |
RichardHoekstra | 8:648c3963a8e0 | 95 | //g(x) = a*x+b |
RichardHoekstra | 8:648c3963a8e0 | 96 | //volt(pressure) = gradient*pressure+offset |
RichardHoekstra | 8:648c3963a8e0 | 97 | //gradient = dVolt/dPressure |
RichardHoekstra | 8:648c3963a8e0 | 98 | float p1 = 0, |
RichardHoekstra | 8:648c3963a8e0 | 99 | p2 = 0; |
RichardHoekstra | 8:648c3963a8e0 | 100 | motorOut = 0.1; |
RichardHoekstra | 8:648c3963a8e0 | 101 | wait(2); |
RichardHoekstra | 8:648c3963a8e0 | 102 | p1 = sensor.read(); |
RichardHoekstra | 8:648c3963a8e0 | 103 | motorOut = 0.2; |
RichardHoekstra | 8:648c3963a8e0 | 104 | wait(2); |
RichardHoekstra | 8:648c3963a8e0 | 105 | p2 = sensor.read(); |
RichardHoekstra | 8:648c3963a8e0 | 106 | gradient = (p2-p1)/((float)0.1); |
RichardHoekstra | 8:648c3963a8e0 | 107 | } |
RichardHoekstra | 8:648c3963a8e0 | 108 | void set_curve(float min, float max, float period){ |
RichardHoekstra | 9:1bdf5107920f | 109 | curve_min = min; //mmHg |
RichardHoekstra | 9:1bdf5107920f | 110 | curve_max = max; //mmHg |
RichardHoekstra | 9:1bdf5107920f | 111 | curve_period = period; //ms |
RichardHoekstra | 9:1bdf5107920f | 112 | curve_step_ms = curve_period/CURVE_BUFFER_SIZE; |
RichardHoekstra | 9:1bdf5107920f | 113 | rebuild_buffer(); |
RichardHoekstra | 9:1bdf5107920f | 114 | } |
RichardHoekstra | 9:1bdf5107920f | 115 | void set_min(float min){ |
RichardHoekstra | 9:1bdf5107920f | 116 | curve_min = min; |
RichardHoekstra | 9:1bdf5107920f | 117 | rebuild_buffer(); |
RichardHoekstra | 9:1bdf5107920f | 118 | } |
RichardHoekstra | 9:1bdf5107920f | 119 | void set_max(float max){ |
RichardHoekstra | 8:648c3963a8e0 | 120 | curve_max = max; |
RichardHoekstra | 9:1bdf5107920f | 121 | rebuild_buffer(); |
RichardHoekstra | 9:1bdf5107920f | 122 | } |
RichardHoekstra | 9:1bdf5107920f | 123 | void set_period(float period){ |
RichardHoekstra | 9:1bdf5107920f | 124 | curve_period = period; //ms |
RichardHoekstra | 9:1bdf5107920f | 125 | curve_step_ms = curve_period/CURVE_BUFFER_SIZE; |
RichardHoekstra | 9:1bdf5107920f | 126 | rebuild_buffer(); |
RichardHoekstra | 8:648c3963a8e0 | 127 | } |
RichardHoekstra | 8:648c3963a8e0 | 128 | void set_mode(int mode){ |
RichardHoekstra | 9:1bdf5107920f | 129 | curve_mode = (curve_t)mode; |
RichardHoekstra | 9:1bdf5107920f | 130 | rebuild_buffer(); |
RichardHoekstra | 8:648c3963a8e0 | 131 | } |
RichardHoekstra | 9:1bdf5107920f | 132 | void set_constant_pressure(int pressure){ constant_pressure = pressure; } |
RichardHoekstra | 9:1bdf5107920f | 133 | void set_constant_flow(int flow){ constant_flow = flow; } |
RichardHoekstra | 9:1bdf5107920f | 134 | void set_constant_speed(int speed){ constant_speed = speed; } |
RichardHoekstra | 8:648c3963a8e0 | 135 | }; |