Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

main.cpp@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 0:48c10918dabf 1 #include "mbed.h"
RichardHoekstra 3:10c6e7aaf375 2 #include "PID.h"
RichardHoekstra 7:ace2a14eff7d 3 #include "instructions.h"
RichardHoekstra 8:648c3963a8e0 4 #include "Control.h"
RichardHoekstra 8:648c3963a8e0 5
RichardHoekstra 8:648c3963a8e0 6
RichardHoekstra 9:1bdf5107920f 7 enum curve_t { OFF=0, CONSTANT_PRESSURE, CONSTANT_FLOW, CONSTANT_SPEED, MODE_SINUS, MODE_ARTERIAL
RichardHoekstra 9:1bdf5107920f 8 } curve_mode;
RichardHoekstra 9:1bdf5107920f 9
RichardHoekstra 9:1bdf5107920f 10
RichardHoekstra 9:1bdf5107920f 11 Control control(D8);
RichardHoekstra 1:9e6c4011eef6 12 //I2C settings
RichardHoekstra 3:10c6e7aaf375 13 #define SDA D10
RichardHoekstra 3:10c6e7aaf375 14 #define SCL D11
RichardHoekstra 3:10c6e7aaf375 15 #define motor_addr 0x91
RichardHoekstra 3:10c6e7aaf375 16 #define I2C_BUFFER_SIZE 10
RichardHoekstra 3:10c6e7aaf375 17 I2CSlave slave(SDA,SCL);
RichardHoekstra 2:b9449fc96691 18
RichardHoekstra 8:648c3963a8e0 19
RichardHoekstra 4:614517a6e2af 20 //Sensor variables
RichardHoekstra 4:614517a6e2af 21 float sensor_pressure;
RichardHoekstra 9:1bdf5107920f 22 float sensor_flow;
RichardHoekstra 8:648c3963a8e0 23
RichardHoekstra 4:614517a6e2af 24
RichardHoekstra 8:648c3963a8e0 25 // ----- SENSOR CONTROLLER SETTINGS -----
RichardHoekstra 8:648c3963a8e0 26 namespace names {
RichardHoekstra 8:648c3963a8e0 27 enum t_sensors {SENSOR_PRESSURE_1 = 0, SENSOR_PRESSURE_2,
RichardHoekstra 8:648c3963a8e0 28 SENSOR_TEMPERATURE_1, SENSOR_TEMPERATURE_2,
RichardHoekstra 8:648c3963a8e0 29 SENSOR_FLOW} sensorRespond;
RichardHoekstra 8:648c3963a8e0 30 }
RichardHoekstra 8:648c3963a8e0 31 float sensorVal[5] = {0}; //Index volgens t_sensors
RichardHoekstra 8:648c3963a8e0 32
RichardHoekstra 8:648c3963a8e0 33 //Sample time
RichardHoekstra 8:648c3963a8e0 34 int tick_ms_druksensor1 = 10, //100 Hz
RichardHoekstra 8:648c3963a8e0 35 tick_ms_druksensor2 = 10,
RichardHoekstra 8:648c3963a8e0 36 tick_ms_tempsensor1 = 1000, //1 Hz
RichardHoekstra 8:648c3963a8e0 37 tick_ms_tempsensor2 = 1000,
RichardHoekstra 8:648c3963a8e0 38 tick_ms_flowsensor = 10; //100 Hz
RichardHoekstra 8:648c3963a8e0 39 //DEPRECATED
RichardHoekstra 8:648c3963a8e0 40 int druksensor1_mva_elements = 10,
RichardHoekstra 8:648c3963a8e0 41 druksensor2_mva_elements = 10,
RichardHoekstra 8:648c3963a8e0 42 tempsensor1_mva_elements = 10,
RichardHoekstra 8:648c3963a8e0 43 tempsensor2_mva_elements = 10,
RichardHoekstra 8:648c3963a8e0 44 flowsensor_mva_elements = 10;
RichardHoekstra 8:648c3963a8e0 45
RichardHoekstra 8:648c3963a8e0 46 // ----- SENSOR CONTROLLER FUNCTIONS -----
RichardHoekstra 8:648c3963a8e0 47 //DEPRECATED
RichardHoekstra 8:648c3963a8e0 48 float calc_moving_average(float val, int samples = 1){
RichardHoekstra 8:648c3963a8e0 49 if(samples > 1){
RichardHoekstra 8:648c3963a8e0 50 static float sample_arr[100] = {0}; //[0] is the newest
RichardHoekstra 8:648c3963a8e0 51 float moving_average = 0;
RichardHoekstra 8:648c3963a8e0 52 if(samples > 0 && samples <= 100){ //Sanity check
RichardHoekstra 8:648c3963a8e0 53 //Put the new val into the sample_arr and push out the oldest one
RichardHoekstra 8:648c3963a8e0 54 for(int i=samples-1; i>0; i--){
RichardHoekstra 8:648c3963a8e0 55 //[9]<-[8]<-[7]
RichardHoekstra 8:648c3963a8e0 56 sample_arr[i] = sample_arr[i-1];
RichardHoekstra 8:648c3963a8e0 57 }
RichardHoekstra 8:648c3963a8e0 58 sample_arr[0] = val;
RichardHoekstra 8:648c3963a8e0 59 //Calculate the moving average
RichardHoekstra 8:648c3963a8e0 60 for(int i=0; i<samples; i++){
RichardHoekstra 8:648c3963a8e0 61 moving_average += sample_arr[i];
RichardHoekstra 8:648c3963a8e0 62 }
RichardHoekstra 8:648c3963a8e0 63 return moving_average/(float)samples;
RichardHoekstra 8:648c3963a8e0 64 } else {
RichardHoekstra 8:648c3963a8e0 65 return 3.1415926; //Improv error code
RichardHoekstra 8:648c3963a8e0 66 }
RichardHoekstra 8:648c3963a8e0 67 } else {
RichardHoekstra 8:648c3963a8e0 68 return val;
RichardHoekstra 8:648c3963a8e0 69 }
RichardHoekstra 8:648c3963a8e0 70 }
RichardHoekstra 8:648c3963a8e0 71
RichardHoekstra 8:648c3963a8e0 72 //Split an integer into two char
RichardHoekstra 8:648c3963a8e0 73 void int_to_2_char(char* arr, int val, int first_element = 0){
RichardHoekstra 8:648c3963a8e0 74 arr[first_element] = val>>8;
RichardHoekstra 8:648c3963a8e0 75 arr[first_element+1] = val&255;
RichardHoekstra 8:648c3963a8e0 76 }
RichardHoekstra 5:006b0b8374ea 77 //Join two char values into one integer value
RichardHoekstra 4:614517a6e2af 78 int char2_to_int(char* arr, int first_element = 0){
RichardHoekstra 4:614517a6e2af 79 return (arr[first_element]<<8)+arr[first_element+1];
RichardHoekstra 4:614517a6e2af 80 }
RichardHoekstra 4:614517a6e2af 81
RichardHoekstra 8:648c3963a8e0 82 //Returns true if it was successful at updating
RichardHoekstra 4:614517a6e2af 83 bool updateVariables(char* arr){
RichardHoekstra 8:648c3963a8e0 84 command_t command = (command_t)arr[0];
RichardHoekstra 8:648c3963a8e0 85 switch(command){
RichardHoekstra 8:648c3963a8e0 86 // ----- MOTOR CONTROLLER COMMANDS -----
RichardHoekstra 4:614517a6e2af 87 case SET_MODE:
RichardHoekstra 9:1bdf5107920f 88 control.set_mode(char2_to_int(arr,1));
RichardHoekstra 4:614517a6e2af 89 break;
RichardHoekstra 4:614517a6e2af 90 case SET_CONSTANT_PRESSURE:
RichardHoekstra 9:1bdf5107920f 91 control.set_constant_pressure(char2_to_int(arr,1)/100.0);
RichardHoekstra 4:614517a6e2af 92 break;
RichardHoekstra 4:614517a6e2af 93 case SET_CONSTANT_FLOW:
RichardHoekstra 9:1bdf5107920f 94 control.set_constant_flow(char2_to_int(arr,1)/10.0);
RichardHoekstra 4:614517a6e2af 95 break;
RichardHoekstra 4:614517a6e2af 96 case SET_CONSTANT_SPEED:
RichardHoekstra 9:1bdf5107920f 97 control.set_constant_speed((float)char2_to_int(arr,1));
RichardHoekstra 4:614517a6e2af 98 break;
RichardHoekstra 4:614517a6e2af 99 case SET_MIN:
RichardHoekstra 9:1bdf5107920f 100 control.set_min(char2_to_int(arr,1)/100.0);
RichardHoekstra 4:614517a6e2af 101 break;
RichardHoekstra 4:614517a6e2af 102 case SET_MAX:
RichardHoekstra 9:1bdf5107920f 103 control.set_max(char2_to_int(arr,1)/100.0);
RichardHoekstra 4:614517a6e2af 104 break;
RichardHoekstra 4:614517a6e2af 105 case SET_FREQUENCY:
RichardHoekstra 4:614517a6e2af 106 //Note: it receives a frequency but internally converts it to
RichardHoekstra 4:614517a6e2af 107 // a period in ms immediately.
RichardHoekstra 9:1bdf5107920f 108 control.set_period(1000/((char2_to_int(arr,1))/100.0));
RichardHoekstra 4:614517a6e2af 109 break;
RichardHoekstra 8:648c3963a8e0 110 // ----- SENSOR CONTROLLER COMMANDS -----
RichardHoekstra 8:648c3963a8e0 111 case SET_RESPONSE_SENSOR_PRESSURE_1:
RichardHoekstra 8:648c3963a8e0 112 names::sensorRespond = names::SENSOR_PRESSURE_1;
RichardHoekstra 8:648c3963a8e0 113 break;
RichardHoekstra 8:648c3963a8e0 114 case SET_RESPONSE_SENSOR_PRESSURE_2:
RichardHoekstra 8:648c3963a8e0 115 names::sensorRespond = names::SENSOR_PRESSURE_2;
RichardHoekstra 8:648c3963a8e0 116 break;
RichardHoekstra 8:648c3963a8e0 117 case SET_RESPONSE_SENSOR_TEMPERATURE_1:
RichardHoekstra 8:648c3963a8e0 118 names::sensorRespond = names::SENSOR_TEMPERATURE_1;
RichardHoekstra 8:648c3963a8e0 119 break;
RichardHoekstra 8:648c3963a8e0 120 case SET_RESPONSE_SENSOR_TEMPERATURE_2:
RichardHoekstra 8:648c3963a8e0 121 names::sensorRespond = names::SENSOR_TEMPERATURE_2;
RichardHoekstra 8:648c3963a8e0 122 break;
RichardHoekstra 8:648c3963a8e0 123 case SET_RESPONSE_SENSOR_FLOW:
RichardHoekstra 8:648c3963a8e0 124 names::sensorRespond = names::SENSOR_FLOW;
RichardHoekstra 8:648c3963a8e0 125 break;
RichardHoekstra 8:648c3963a8e0 126 case SET_RESPONSE_SENSOR_SPEED:
RichardHoekstra 8:648c3963a8e0 127 //Currently not implemented
RichardHoekstra 8:648c3963a8e0 128 break;
RichardHoekstra 8:648c3963a8e0 129
RichardHoekstra 8:648c3963a8e0 130 //Set sample rates
RichardHoekstra 8:648c3963a8e0 131 case SET_SENSOR_PRESSURE_1_SAMPLE_RATE:
RichardHoekstra 8:648c3963a8e0 132 tick_ms_druksensor1 = 1000/(char2_to_int(arr,1));
RichardHoekstra 4:614517a6e2af 133 break;
RichardHoekstra 8:648c3963a8e0 134 case SET_SENSOR_PRESSURE_2_SAMPLE_RATE:
RichardHoekstra 8:648c3963a8e0 135 tick_ms_druksensor2 = 1000/(char2_to_int(arr,1));
RichardHoekstra 8:648c3963a8e0 136 break;
RichardHoekstra 8:648c3963a8e0 137 case SET_SENSOR_TEMPERATURE_1_SAMPLE_RATE:
RichardHoekstra 8:648c3963a8e0 138 tick_ms_tempsensor1 = 1000/(char2_to_int(arr,1));
RichardHoekstra 8:648c3963a8e0 139 break;
RichardHoekstra 8:648c3963a8e0 140 case SET_SENSOR_TEMPERATURE_2_SAMPLE_RATE:
RichardHoekstra 8:648c3963a8e0 141 tick_ms_tempsensor2 = 1000/(char2_to_int(arr,1));
RichardHoekstra 8:648c3963a8e0 142 break;
RichardHoekstra 8:648c3963a8e0 143 case SET_SENSOR_FLOW_SAMPLE_RATE:
RichardHoekstra 8:648c3963a8e0 144 tick_ms_flowsensor = 1000/(char2_to_int(arr,1));
RichardHoekstra 8:648c3963a8e0 145 break;
RichardHoekstra 8:648c3963a8e0 146
RichardHoekstra 8:648c3963a8e0 147 //Set moving averages
RichardHoekstra 8:648c3963a8e0 148 //DEPRECATED
RichardHoekstra 8:648c3963a8e0 149 case SET_SENSOR_PRESSURE_1_MVA:
RichardHoekstra 8:648c3963a8e0 150 druksensor1_mva_elements = char2_to_int(arr,1);
RichardHoekstra 8:648c3963a8e0 151 break;
RichardHoekstra 8:648c3963a8e0 152 case SET_SENSOR_PRESSURE_2_MVA:
RichardHoekstra 8:648c3963a8e0 153 druksensor2_mva_elements = char2_to_int(arr,1);
RichardHoekstra 8:648c3963a8e0 154 break;
RichardHoekstra 8:648c3963a8e0 155 case SET_SENSOR_TEMPERATURE_1_MVA:
RichardHoekstra 8:648c3963a8e0 156 tempsensor1_mva_elements = char2_to_int(arr,1);
RichardHoekstra 8:648c3963a8e0 157 break;
RichardHoekstra 8:648c3963a8e0 158 case SET_SENSOR_TEMPERATURE_2_MVA:
RichardHoekstra 8:648c3963a8e0 159 tempsensor2_mva_elements = char2_to_int(arr,1);
RichardHoekstra 8:648c3963a8e0 160 break;
RichardHoekstra 8:648c3963a8e0 161 case SET_SENSOR_FLOW_MVA:
RichardHoekstra 8:648c3963a8e0 162 flowsensor_mva_elements = char2_to_int(arr,1);
RichardHoekstra 8:648c3963a8e0 163 break;
RichardHoekstra 4:614517a6e2af 164 default:
RichardHoekstra 4:614517a6e2af 165 //No command was valid
RichardHoekstra 8:648c3963a8e0 166 //Create some kind of error? Maybe, a blinking led of sorts.
RichardHoekstra 4:614517a6e2af 167 return false;
RichardHoekstra 4:614517a6e2af 168 }
RichardHoekstra 8:648c3963a8e0 169 //Clear the buffer for the next iteration
RichardHoekstra 8:648c3963a8e0 170 for(int i=0;i<I2C_BUFFER_SIZE;i++){
RichardHoekstra 8:648c3963a8e0 171 arr[i] = 0;
RichardHoekstra 8:648c3963a8e0 172 }
RichardHoekstra 8:648c3963a8e0 173 return true;
RichardHoekstra 4:614517a6e2af 174 }
RichardHoekstra 4:614517a6e2af 175
RichardHoekstra 1:9e6c4011eef6 176 int main() {
RichardHoekstra 8:648c3963a8e0 177 // ----- SENSOR CONTROLLER STUFF -----
RichardHoekstra 8:648c3963a8e0 178 //Pins
RichardHoekstra 8:648c3963a8e0 179 AnalogIn drukSensor1(A0),
RichardHoekstra 8:648c3963a8e0 180 drukSensor2(A1),
RichardHoekstra 8:648c3963a8e0 181 tempSensor1(A2),
RichardHoekstra 8:648c3963a8e0 182 tempSensor2(A3),
RichardHoekstra 8:648c3963a8e0 183 flowSensor(A4);
RichardHoekstra 8:648c3963a8e0 184 //mbed ondersteund onneindig veel timers
RichardHoekstra 8:648c3963a8e0 185 Timer t_druk1,
RichardHoekstra 8:648c3963a8e0 186 t_druk2,
RichardHoekstra 8:648c3963a8e0 187 t_temp1,
RichardHoekstra 8:648c3963a8e0 188 t_temp2,
RichardHoekstra 8:648c3963a8e0 189 t_flow;
RichardHoekstra 8:648c3963a8e0 190
RichardHoekstra 8:648c3963a8e0 191 //Start the timers
RichardHoekstra 8:648c3963a8e0 192 t_druk1.start(); t_druk2.start(); t_temp1.start(); t_temp2.start(); t_flow.start();
RichardHoekstra 8:648c3963a8e0 193
RichardHoekstra 8:648c3963a8e0 194
RichardHoekstra 1:9e6c4011eef6 195 slave.address(motor_addr); //Set the correct address for this module
RichardHoekstra 1:9e6c4011eef6 196 char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C
RichardHoekstra 4:614517a6e2af 197 bool buffer_changed = false;
RichardHoekstra 3:10c6e7aaf375 198
RichardHoekstra 9:1bdf5107920f 199 //Calibrate the motor controller on the pressure sensor
RichardHoekstra 9:1bdf5107920f 200 control.normalize(drukSensor1);
RichardHoekstra 9:1bdf5107920f 201 control.start();
RichardHoekstra 9:1bdf5107920f 202
RichardHoekstra 1:9e6c4011eef6 203 while(1) {
RichardHoekstra 1:9e6c4011eef6 204 int i = slave.receive();
RichardHoekstra 1:9e6c4011eef6 205 switch (i) {
RichardHoekstra 1:9e6c4011eef6 206 case I2CSlave::ReadAddressed:
RichardHoekstra 1:9e6c4011eef6 207 //Received a request to be read
RichardHoekstra 8:648c3963a8e0 208 //Write the requested data to the buffer
RichardHoekstra 8:648c3963a8e0 209 int_to_2_char(buffer,((int)(sensorVal[names::sensorRespond]*65535)));
RichardHoekstra 8:648c3963a8e0 210 //Write the buffer to the I2C line
RichardHoekstra 8:648c3963a8e0 211 slave.write(buffer,2);
RichardHoekstra 1:9e6c4011eef6 212 break;
RichardHoekstra 1:9e6c4011eef6 213 case I2CSlave::WriteGeneral:
RichardHoekstra 1:9e6c4011eef6 214 //Received a request to be written to
RichardHoekstra 1:9e6c4011eef6 215 slave.read(buffer,I2C_BUFFER_SIZE);
RichardHoekstra 1:9e6c4011eef6 216 buffer_changed = true;
RichardHoekstra 1:9e6c4011eef6 217 break;
RichardHoekstra 1:9e6c4011eef6 218 case I2CSlave::WriteAddressed:
RichardHoekstra 1:9e6c4011eef6 219 //Received a request to be written to a specific location
RichardHoekstra 1:9e6c4011eef6 220 slave.read(buffer,I2C_BUFFER_SIZE);
RichardHoekstra 1:9e6c4011eef6 221 buffer_changed = true;
RichardHoekstra 1:9e6c4011eef6 222 break;
RichardHoekstra 1:9e6c4011eef6 223 }
RichardHoekstra 5:006b0b8374ea 224 //Update the variables if the buffer has changed.
RichardHoekstra 1:9e6c4011eef6 225 if(buffer_changed == true){
RichardHoekstra 4:614517a6e2af 226 updateVariables(buffer);
RichardHoekstra 1:9e6c4011eef6 227 buffer_changed = false;
RichardHoekstra 1:9e6c4011eef6 228 }
RichardHoekstra 8:648c3963a8e0 229 //Check whether the sensors have to be read
RichardHoekstra 8:648c3963a8e0 230 if(t_druk1.read_ms() >= tick_ms_druksensor1){
RichardHoekstra 8:648c3963a8e0 231 //Lees de druksensor uit
RichardHoekstra 8:648c3963a8e0 232 sensorVal[names::SENSOR_PRESSURE_1] = calc_moving_average(drukSensor1.read(), druksensor1_mva_elements);
RichardHoekstra 8:648c3963a8e0 233 t_druk1.reset();
RichardHoekstra 8:648c3963a8e0 234 }
RichardHoekstra 8:648c3963a8e0 235 if(t_druk2.read_ms() >= tick_ms_druksensor2){
RichardHoekstra 8:648c3963a8e0 236 //Lees de druksensor uit
RichardHoekstra 8:648c3963a8e0 237 sensorVal[names::SENSOR_PRESSURE_1] = calc_moving_average(drukSensor2.read(), druksensor1_mva_elements);
RichardHoekstra 8:648c3963a8e0 238 t_druk2.reset();
RichardHoekstra 8:648c3963a8e0 239 }
RichardHoekstra 8:648c3963a8e0 240 if(t_temp1.read_ms() >= tick_ms_tempsensor1){
RichardHoekstra 8:648c3963a8e0 241 //Lees de temperatuursensor uit
RichardHoekstra 8:648c3963a8e0 242 sensorVal[names::SENSOR_TEMPERATURE_1] = calc_moving_average(tempSensor1.read(),tempsensor1_mva_elements);
RichardHoekstra 8:648c3963a8e0 243 t_temp1.reset();
RichardHoekstra 8:648c3963a8e0 244 }
RichardHoekstra 8:648c3963a8e0 245 if(t_temp2.read_ms() >= tick_ms_tempsensor2){
RichardHoekstra 8:648c3963a8e0 246 //Lees de temperatuursensor uit
RichardHoekstra 8:648c3963a8e0 247 sensorVal[names::SENSOR_TEMPERATURE_2] = calc_moving_average(tempSensor2.read(),tempsensor2_mva_elements);
RichardHoekstra 8:648c3963a8e0 248 t_temp2.reset();
RichardHoekstra 8:648c3963a8e0 249 }
RichardHoekstra 8:648c3963a8e0 250 if(t_flow.read_ms() >= tick_ms_flowsensor){
RichardHoekstra 8:648c3963a8e0 251 //Lees de flowsensor uit
RichardHoekstra 8:648c3963a8e0 252 sensorVal[names::SENSOR_FLOW] = calc_moving_average(flowSensor.read(),flowsensor_mva_elements);
RichardHoekstra 8:648c3963a8e0 253 t_flow.reset();
RichardHoekstra 8:648c3963a8e0 254 }
RichardHoekstra 8:648c3963a8e0 255 //Execute the control scheme
RichardHoekstra 8:648c3963a8e0 256 //control_loop();
RichardHoekstra 0:48c10918dabf 257
RichardHoekstra 0:48c10918dabf 258 }
RichardHoekstra 0:48c10918dabf 259 }