Revision 8:648c3963a8e0, committed 2016-11-22
- Comitter:
- RichardHoekstra
- Date:
- Tue Nov 22 22:01:20 2016 +0000
- Parent:
- 7:ace2a14eff7d
- Child:
- 9:1bdf5107920f
- Commit message:
- Transplant succesful. Working on getting the control loop working by creating a new class to handle all the junk and keep stuff tidy like
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Control.h Tue Nov 22 22:01:20 2016 +0000
@@ -0,0 +1,96 @@
+#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!
+ }
+};
--- a/main.cpp Sun Nov 20 13:11:08 2016 +0000
+++ b/main.cpp Tue Nov 22 22:01:20 2016 +0000
@@ -1,6 +1,9 @@
#include "mbed.h"
#include "PID.h"
#include "instructions.h"
+#include "Control.h"
+
+
//I2C settings
#define SDA D10
@@ -10,7 +13,7 @@
I2CSlave slave(SDA,SCL);
//Curve settings
- #define CURVE_BUFFER_SIZE 100
+
enum curve_t { OFF=0, CONSTANT_PRESSURE, CONSTANT_FLOW, CONSTANT_SPEED, MODE_SINUS, MODE_ARTERIAL
} curve_mode;
float curve_buffer[CURVE_BUFFER_SIZE];
@@ -24,27 +27,60 @@
float constant_speed;
//Sensor variables
float sensor_pressure;
- float sensor_flow;
+ float SENSOR_FLOW;
+
+// ----- MOTOR CONTROLLER SETTINGS -----
+ float rel_pressure_volt; //The relationship between increase in voltage and increase in pressure
+ PwmOut motorOut(D8);
+
-//PID settings
- #define PID_PERIOD_US 1000
- #define PID_INITIAL_SETPOINT 0
- //Tuning parameters
- #define Kp 0.1
- #define Ki 0
- #define Kc 0
- //Input is a pressure between 0 and 300 mmHg
- #define PID_MIN_INPUT 0
- #define PID_MAX_INPUT 300
- //Output is a PWM duty cycle
- #define PID_MIN_OUTPUT 0
- #define PID_MAX_OUTPUT 1
- //PID IO
- #define PID_PIN A0
- float ProcessValue = 0;
- //PID Controller
- PID controller(Kp, Ki, Kc, PID_PERIOD_US);
- AnalogOut PID_Output(PID_PIN);
+// ----- SENSOR CONTROLLER SETTINGS -----
+namespace names {
+ enum t_sensors {SENSOR_PRESSURE_1 = 0, SENSOR_PRESSURE_2,
+ SENSOR_TEMPERATURE_1, SENSOR_TEMPERATURE_2,
+ SENSOR_FLOW} sensorRespond;
+}
+float sensorVal[5] = {0}; //Index volgens t_sensors
+
+//Sample time
+int tick_ms_druksensor1 = 10, //100 Hz
+ tick_ms_druksensor2 = 10,
+ tick_ms_tempsensor1 = 1000, //1 Hz
+ tick_ms_tempsensor2 = 1000,
+ tick_ms_flowsensor = 10; //100 Hz
+//DEPRECATED
+int druksensor1_mva_elements = 10,
+ druksensor2_mva_elements = 10,
+ tempsensor1_mva_elements = 10,
+ tempsensor2_mva_elements = 10,
+ flowsensor_mva_elements = 10;
+
+// ----- SENSOR CONTROLLER FUNCTIONS -----
+//DEPRECATED
+float calc_moving_average(float val, int samples = 1){
+ if(samples > 1){
+ static float sample_arr[100] = {0}; //[0] is the newest
+ float moving_average = 0;
+ if(samples > 0 && samples <= 100){ //Sanity check
+ //Put the new val into the sample_arr and push out the oldest one
+ for(int i=samples-1; i>0; i--){
+ //[9]<-[8]<-[7]
+ sample_arr[i] = sample_arr[i-1];
+ }
+ sample_arr[0] = val;
+ //Calculate the moving average
+ for(int i=0; i<samples; i++){
+ moving_average += sample_arr[i];
+ }
+ return moving_average/(float)samples;
+ } else {
+ return 3.1415926; //Improv error code
+ }
+ } else {
+ return val;
+ }
+}
+
//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.
@@ -64,16 +100,21 @@
//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;
+ arr[first_element+1] = val&255;
+}
//Join two char values into one integer value
int char2_to_int(char* arr, int first_element = 0){
return (arr[first_element]<<8)+arr[first_element+1];
}
-//Returns true if it was succesful at updating
+//Returns true if it was successful at updating
bool updateVariables(char* arr){
- command_t command_codes = (command_t)arr[0];
- switch(command_codes){
+ command_t command = (command_t)arr[0];
+ switch(command){
+ // ----- MOTOR CONTROLLER COMMANDS -----
case SET_MODE:
curve_mode = (curve_t)char2_to_int(arr,1);
break;
@@ -88,110 +129,138 @@
break;
case SET_MIN:
curve_min = char2_to_int(arr,1)/100.0;
+ curve_sinus();
break;
case SET_MAX:
curve_max = char2_to_int(arr,1)/100.0;
+ curve_sinus();
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();
break;
- case RECEIVE_PRESSURE:
- sensor_pressure = char2_to_int(arr,1)/100.0;
+ // ----- SENSOR CONTROLLER COMMANDS -----
+ case SET_RESPONSE_SENSOR_PRESSURE_1:
+ names::sensorRespond = names::SENSOR_PRESSURE_1;
+ break;
+ case SET_RESPONSE_SENSOR_PRESSURE_2:
+ names::sensorRespond = names::SENSOR_PRESSURE_2;
+ break;
+ case SET_RESPONSE_SENSOR_TEMPERATURE_1:
+ names::sensorRespond = names::SENSOR_TEMPERATURE_1;
+ break;
+ case SET_RESPONSE_SENSOR_TEMPERATURE_2:
+ names::sensorRespond = names::SENSOR_TEMPERATURE_2;
+ break;
+ case SET_RESPONSE_SENSOR_FLOW:
+ names::sensorRespond = names::SENSOR_FLOW;
+ break;
+ case SET_RESPONSE_SENSOR_SPEED:
+ //Currently not implemented
+ break;
+
+ //Set sample rates
+ case SET_SENSOR_PRESSURE_1_SAMPLE_RATE:
+ tick_ms_druksensor1 = 1000/(char2_to_int(arr,1));
break;
- case RECEIVE_FLOW:
- sensor_flow = char2_to_int(arr,1)/10.0;
- break;
+ case SET_SENSOR_PRESSURE_2_SAMPLE_RATE:
+ tick_ms_druksensor2 = 1000/(char2_to_int(arr,1));
+ break;
+ case SET_SENSOR_TEMPERATURE_1_SAMPLE_RATE:
+ tick_ms_tempsensor1 = 1000/(char2_to_int(arr,1));
+ break;
+ case SET_SENSOR_TEMPERATURE_2_SAMPLE_RATE:
+ tick_ms_tempsensor2 = 1000/(char2_to_int(arr,1));
+ break;
+ case SET_SENSOR_FLOW_SAMPLE_RATE:
+ tick_ms_flowsensor = 1000/(char2_to_int(arr,1));
+ break;
+
+ //Set moving averages
+ //DEPRECATED
+ case SET_SENSOR_PRESSURE_1_MVA:
+ druksensor1_mva_elements = char2_to_int(arr,1);
+ break;
+ case SET_SENSOR_PRESSURE_2_MVA:
+ druksensor2_mva_elements = char2_to_int(arr,1);
+ break;
+ case SET_SENSOR_TEMPERATURE_1_MVA:
+ tempsensor1_mva_elements = char2_to_int(arr,1);
+ break;
+ case SET_SENSOR_TEMPERATURE_2_MVA:
+ tempsensor2_mva_elements = char2_to_int(arr,1);
+ break;
+ case SET_SENSOR_FLOW_MVA:
+ flowsensor_mva_elements = char2_to_int(arr,1);
+ break;
default:
//No command was valid
- //Create some kind of error? Maybe, a blinking led.
+ //Create some kind of error? Maybe, a blinking led of sorts.
return false;
}
- return true;
+ //Clear the buffer for the next iteration
+ for(int i=0;i<I2C_BUFFER_SIZE;i++){
+ arr[i] = 0;
+ }
+ return true;
}
-void PID_loop(Timer& t_PID, Timer& t_Curve){
- static int PID_last_curve_point = 0;
- //Check if the PID scheme has to execute again
- if(t_PID.read_us() >= PID_PERIOD_US){
- bool PID_on = true;
- //What mode is the motorcontroller currently in?
- switch(curve_mode){
- case CONSTANT_PRESSURE:
- //Set Point is the value we want to achieve
- //Which in this case is constant
- //Process Value is the current value of the process
- //Which is variable and connected to processes in the physical world
- controller.setSetPoint(constant_pressure);
- controller.setProcessValue(sensor_pressure);
- break;
- case CONSTANT_FLOW:
- controller.setSetPoint(constant_flow);
- controller.setProcessValue(sensor_flow);
- break;
- case CONSTANT_SPEED:
- //There is already a PI control scheme on the Maxon module
- //We only have to send a PWM signal and the module will do the rest
- //TODO: Clip this back to a value between 0 and 1
- PID_Output = constant_speed;
- PID_on = false;
- break;
- case MODE_SINUS:
- //Take the current point in the buffer
- controller.setSetPoint(curve_buffer[PID_last_curve_point]);
- controller.setProcessValue(sensor_pressure);
- break;
- case MODE_ARTERIAL:
- controller.setSetPoint(curve_buffer[PID_last_curve_point]);
- controller.setProcessValue(sensor_pressure);
- break;
- default:
- //Curve should be off or the mode is invalid
- PID_on = false;
- break;
- }
- if(PID_on == true){
- //Adjust output according to the PID scheme
- PID_Output = controller.compute();
- }
- //For the next iteration, check whether it'll have to be a new point on the curve
- //Note: putting this at the end, ensures that in the first loop point 0 will get executed
- if(t_Curve.read_us() >= curve_step_us){
- PID_last_curve_point++;
- if(PID_last_curve_point >= CURVE_BUFFER_SIZE){
- PID_last_curve_point = 0;
- }
- //Reset the curve timer
- t_Curve.reset();
- }
- //Reset the PID timer
- t_PID.reset();
- }
+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
+ AnalogIn drukSensor1(A0),
+ drukSensor2(A1),
+ tempSensor1(A2),
+ tempSensor2(A3),
+ flowSensor(A4);
+
+ //mbed ondersteund onneindig veel timers
+ Timer t_druk1,
+ t_druk2,
+ t_temp1,
+ t_temp2,
+ t_flow;
+
+ //Start the timers
+ t_druk1.start(); t_druk2.start(); t_temp1.start(); t_temp2.start(); t_flow.start();
+
+
slave.address(motor_addr); //Set the correct address for this module
-
- //PID Controller initialization
- controller.setInputLimits(PID_MIN_INPUT, PID_MAX_INPUT);
- controller.setOutputLimits(PID_MIN_OUTPUT, PID_MAX_OUTPUT);
- controller.setSetPoint(PID_INITIAL_SETPOINT);
-
- Timer t_PID;
- Timer t_Curve;
- t_PID.start();
- t_Curve.start();
-
+ 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);
while(1) {
int i = slave.receive();
switch (i) {
case I2CSlave::ReadAddressed:
//Received a request to be read
- //Irrelevant for now
+ //Write the requested data to the buffer
+ int_to_2_char(buffer,((int)(sensorVal[names::sensorRespond]*65535)));
+ //Write the buffer to the I2C line
+ slave.write(buffer,2);
break;
case I2CSlave::WriteGeneral:
//Received a request to be written to
@@ -207,14 +276,36 @@
//Update the variables if the buffer has changed.
if(buffer_changed == true){
updateVariables(buffer);
- //Clear the buffer for the next iteration
- for(int i=0;i<I2C_BUFFER_SIZE;i++){
- buffer[i] = 0;
- }
buffer_changed = false;
}
- //Execute the PID loop
- PID_loop(t_PID,t_Curve);
+ //Check whether the sensors have to be read
+ if(t_druk1.read_ms() >= tick_ms_druksensor1){
+ //Lees de druksensor uit
+ sensorVal[names::SENSOR_PRESSURE_1] = calc_moving_average(drukSensor1.read(), druksensor1_mva_elements);
+ t_druk1.reset();
+ }
+ if(t_druk2.read_ms() >= tick_ms_druksensor2){
+ //Lees de druksensor uit
+ sensorVal[names::SENSOR_PRESSURE_1] = calc_moving_average(drukSensor2.read(), druksensor1_mva_elements);
+ t_druk2.reset();
+ }
+ if(t_temp1.read_ms() >= tick_ms_tempsensor1){
+ //Lees de temperatuursensor uit
+ sensorVal[names::SENSOR_TEMPERATURE_1] = calc_moving_average(tempSensor1.read(),tempsensor1_mva_elements);
+ t_temp1.reset();
+ }
+ if(t_temp2.read_ms() >= tick_ms_tempsensor2){
+ //Lees de temperatuursensor uit
+ sensorVal[names::SENSOR_TEMPERATURE_2] = calc_moving_average(tempSensor2.read(),tempsensor2_mva_elements);
+ t_temp2.reset();
+ }
+ if(t_flow.read_ms() >= tick_ms_flowsensor){
+ //Lees de flowsensor uit
+ sensorVal[names::SENSOR_FLOW] = calc_moving_average(flowSensor.read(),flowsensor_mva_elements);
+ t_flow.reset();
+ }
+ //Execute the control scheme
+ //control_loop();
}
}
Richard Hoekstra
