Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
Control.h
- Committer:
- RichardHoekstra
- Date:
- 2016-11-22
- Revision:
- 8:648c3963a8e0
- Child:
- 9:1bdf5107920f
File content as of revision 8:648c3963a8e0:
#include "mbed.h" #define CURVE_BUFFER_SIZE 100 class Control { private: //----- SETTINGS ----- //Curve enum curve_t { OFF=0, CONSTANT_PRESSURE, CONSTANT_FLOW, CONSTANT_SPEED, MODE_SINUS, MODE_ARTERIAL } curve_mode; float curve_buffer[CURVE_BUFFER_SIZE]; float curve_min; //[mmHg] float curve_max; //[mmHg] float curve_period; //[ms] int curve_step; //Motor output float gradient; //pressure/percentagemaxpowerorjustvoltiguess PwmOut motorOut; //----- PRIVATE FUNCTIONS ----- void outputToPin(float input){ float temp = gradient*input; if(temp > 1){ motorOut = 1; } else if(temp < 0){ motorOut = 0; } else { motorOut = temp; } } void run(){ switch(curve_mode){ case CONSTANT_PRESSURE: break; case CONSTANT_FLOW: break; case CONSTANT_SPEED: break; case MODE_SINUS: outputToPin(curve_buffer[curve_step]); curve_step++; if(curve_step >= CURVE_BUFFER_SIZE){ curve_step = 0; } break; case MODE_ARTERIAL: break; } } public: Control(PinName n_pin) : motorOut(n_pin){ curve_step = 0; curve_min = 80; //[mmHg] curve_max = 120; //[mmHg] curve_period = 1000; //[ms] } void normalize(AnalogIn& sensor){ //f'(x) = a //f'(x) = dy/dx //g(x) = a*x+b //volt(pressure) = gradient*pressure+offset //gradient = dVolt/dPressure float p1 = 0, p2 = 0; motorOut = 0.1; wait(2); p1 = sensor.read(); motorOut = 0.2; wait(2); p2 = sensor.read(); gradient = (p2-p1)/((float)0.1); } void set_curve(float min, float max, float period){ curve_min = min; curve_max = max; curve_period = period; } void set_mode(int mode){ curve_mode = (curve_t)mode; } void rebuild_buffer_sinus(){ float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten float offset = (curve_max+curve_min)/2; //Genereer een volle periode en zet het in de buffer float step = 2*3.1415926/CURVE_BUFFER_SIZE; for(int i=0;i<CURVE_BUFFER_SIZE;i++){ curve_buffer[i] = offset+amplitude*sin(step*i); } } void rebuild_buffer_arterial(){ //Coming to a cinema near you... soon! } };