Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

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?

UserRevisionLine numberNew 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 };