Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

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?

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