De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

Files at this revision

API Documentation at this revision

Comitter:
RichardHoekstra
Date:
Tue Nov 22 22:34:18 2016 +0000
Parent:
8:648c3963a8e0
Commit message:
OOP'd this bitch up

Changed in this revision

Control.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 648c3963a8e0 -r 1bdf5107920f Control.h
--- a/Control.h	Tue Nov 22 22:01:20 2016 +0000
+++ b/Control.h	Tue Nov 22 22:34:18 2016 +0000
@@ -11,6 +11,15 @@
     float curve_max;   //[mmHg]
     float curve_period; //[ms]
     int curve_step;
+    int curve_step_ms; //The time it takes to make one step
+    
+    //Constant variables
+    float constant_pressure;
+    float constant_flow;
+    float constant_speed;
+    
+    //Ticker
+    Ticker t;
     
     //Motor output    
     float gradient; //pressure/percentagemaxpowerorjustvoltiguess
@@ -44,10 +53,26 @@
                 }
                 break;
             case MODE_ARTERIAL:
-                break;    
+                break;
+            default:
+                break; 
         }   
         
     }
+    void rebuild_buffer(){
+        if(curve_mode == MODE_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);
+            }
+        } else if(curve_mode == MODE_ARTERIAL){
+           //Coming to a cinema near you... soon!                    
+        }
+    }
     
 public:
     Control(PinName n_pin) : motorOut(n_pin){
@@ -55,7 +80,15 @@
         curve_min = 80;    //[mmHg]
         curve_max = 120;   //[mmHg]
         curve_period = 1000; //[ms]
-    }    
+        motorOut.period_ms(1); //1kHz
+        rebuild_buffer();
+    }   
+    void start(){
+        t.attach(callback(this, &Control::run), (curve_step_ms/1000.0));
+    }
+    void stop(){
+        t.detach();        
+    } 
     void normalize(AnalogIn& sensor){
         //f'(x) = a
         //f'(x) = dy/dx
@@ -73,24 +106,30 @@
         gradient = (p2-p1)/((float)0.1);
     }
     void set_curve(float min, float max, float period){
-        curve_min = min;
+        curve_min = min; //mmHg
+        curve_max = max; //mmHg
+        curve_period = period; //ms
+        curve_step_ms = curve_period/CURVE_BUFFER_SIZE;  
+        rebuild_buffer();
+    }
+    void set_min(float min){
+         curve_min = min;
+         rebuild_buffer();
+    }
+    void set_max(float max){
         curve_max = max;
-        curve_period = period;       
+        rebuild_buffer();
+    }
+    void set_period(float period){
+        curve_period = period; //ms
+        curve_step_ms = curve_period/CURVE_BUFFER_SIZE;  
+        rebuild_buffer();
     }
     void set_mode(int mode){
-        curve_mode = (curve_t)mode;    
+        curve_mode = (curve_t)mode;
+        rebuild_buffer(); 
     }
-    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!         
-    } 
+    void set_constant_pressure(int pressure){   constant_pressure = pressure; }
+    void set_constant_flow(int flow){           constant_flow = flow; }
+    void set_constant_speed(int speed){         constant_speed = speed; }
 };
diff -r 648c3963a8e0 -r 1bdf5107920f main.cpp
--- a/main.cpp	Tue Nov 22 22:01:20 2016 +0000
+++ b/main.cpp	Tue Nov 22 22:34:18 2016 +0000
@@ -4,7 +4,11 @@
 #include "Control.h"
 
 
-
+enum curve_t    { OFF=0, CONSTANT_PRESSURE, CONSTANT_FLOW, CONSTANT_SPEED, MODE_SINUS, MODE_ARTERIAL
+                    } curve_mode;
+                    
+                    
+Control control(D8);
 //I2C settings
     #define SDA D10
     #define SCL D11
@@ -12,26 +16,10 @@
     #define I2C_BUFFER_SIZE 10
     I2CSlave slave(SDA,SCL);
 
-//Curve settings
 
-    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=80;    //[mmHg]
-    float curve_max=120;   //[mmHg]
-    float curve_period=10; //[ms]
-    int curve_step_us=(int)((curve_period*1000)/CURVE_BUFFER_SIZE);//[us]
-//Constant variables
-    float constant_pressure;
-    float constant_flow;
-    float constant_speed;
 //Sensor variables
     float sensor_pressure;
-    float SENSOR_FLOW;
-
-// ----- MOTOR CONTROLLER SETTINGS -----
-    float rel_pressure_volt; //The relationship between increase in voltage and increase in pressure
-    PwmOut motorOut(D8);
+    float sensor_flow;
     
     
 // ----- SENSOR CONTROLLER SETTINGS -----
@@ -81,25 +69,6 @@
     }
 }
     
-
-//Note: om de frequentie aan te passen speel je de buffer sneller af. Hierbij neemt nauwkeurigheid wel af. Om dit te verminderen
-//heb je meer punten in de buffer nodig.
-void curve_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);
-    }
-}
-
-/*
-float curve_arterial(){
-    //Help.
-}
-*/
 //Split an integer into two char
 void int_to_2_char(char* arr, int val, int first_element = 0){
     arr[first_element] = val>>8;
@@ -116,30 +85,27 @@
     switch(command){
     // ----- MOTOR CONTROLLER COMMANDS -----
         case SET_MODE:
-            curve_mode = (curve_t)char2_to_int(arr,1);
+            control.set_mode(char2_to_int(arr,1));
             break;
         case SET_CONSTANT_PRESSURE:
-            constant_pressure = char2_to_int(arr,1)/100.0;
+            control.set_constant_pressure(char2_to_int(arr,1)/100.0);
             break;
         case SET_CONSTANT_FLOW:
-            constant_flow = char2_to_int(arr,1)/10.0;
+            control.set_constant_flow(char2_to_int(arr,1)/10.0);
             break;
         case SET_CONSTANT_SPEED:
-            constant_speed = (float)char2_to_int(arr,1);
+            control.set_constant_speed((float)char2_to_int(arr,1));
             break;
         case SET_MIN:
-            curve_min = char2_to_int(arr,1)/100.0;
-            curve_sinus();
+            control.set_min(char2_to_int(arr,1)/100.0);
             break;
         case SET_MAX:
-            curve_max = char2_to_int(arr,1)/100.0;
-            curve_sinus();
+            control.set_max(char2_to_int(arr,1)/100.0);
             break;
         case SET_FREQUENCY:
             //Note: it receives a frequency but internally converts it to
             //      a period in ms immediately.
-            curve_period = 1000/((char2_to_int(arr,1))/100.0);
-            curve_sinus();
+            control.set_period(1000/((char2_to_int(arr,1))/100.0));
             break;
     // ----- SENSOR CONTROLLER COMMANDS -----
         case SET_RESPONSE_SENSOR_PRESSURE_1:
@@ -207,24 +173,6 @@
     return true;   
 }
 
-void control_init(AnalogIn& sensor){
-    float press1;
-    float press2;
-    //Set the motor at 10% power
-    motorOut = 0.1;
-    //Wait several seconds for everything to settle
-    wait(2);
-    press1 = sensor.read(); //DANGEROUS: Not moving averaged
-    //Set the motor at 20% power
-    motorOut = 0.2;
-    //Wait several seconds for everything to settle
-    wait(2);
-    press2 = sensor.read();
-    
-    //TODO: Make this better
-    rel_pressure_volt = press1/press2;
-}
-
 int main() {
     // ----- SENSOR CONTROLLER STUFF -----
     //Pins
@@ -233,7 +181,6 @@
                 tempSensor1(A2),
                 tempSensor2(A3),
                 flowSensor(A4);
-
     //mbed ondersteund onneindig veel timers
     Timer   t_druk1,
             t_druk2,
@@ -246,12 +193,13 @@
     
     
     slave.address(motor_addr); //Set the correct address for this module
-    motorOut.period_ms(1); //1KHz
     char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C
     bool buffer_changed = false;
     
-    //Calibrate the motor controller
-    control_init(drukSensor1);
+    //Calibrate the motor controller on the pressure sensor
+    control.normalize(drukSensor1);
+    control.start();
+    
     while(1) {
         int i = slave.receive();        
         switch (i) {