Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
main.cpp
- Committer:
- RichardHoekstra
- Date:
- 2016-11-22
- Revision:
- 8:648c3963a8e0
- Parent:
- 7:ace2a14eff7d
- Child:
- 9:1bdf5107920f
File content as of revision 8:648c3963a8e0:
#include "mbed.h" #include "PID.h" #include "instructions.h" #include "Control.h" //I2C settings #define SDA D10 #define SCL D11 #define motor_addr 0x91 #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); // ----- 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. 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; 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 successful at updating bool updateVariables(char* arr){ 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; case SET_CONSTANT_PRESSURE: constant_pressure = char2_to_int(arr,1)/100.0; break; case SET_CONSTANT_FLOW: constant_flow = char2_to_int(arr,1)/10.0; break; case SET_CONSTANT_SPEED: constant_speed = (float)char2_to_int(arr,1); 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; // ----- 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 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 of sorts. return false; } //Clear the buffer for the next iteration for(int i=0;i<I2C_BUFFER_SIZE;i++){ arr[i] = 0; } 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 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 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 //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 slave.read(buffer,I2C_BUFFER_SIZE); buffer_changed = true; break; case I2CSlave::WriteAddressed: //Received a request to be written to a specific location slave.read(buffer,I2C_BUFFER_SIZE); buffer_changed = true; break; } //Update the variables if the buffer has changed. if(buffer_changed == true){ updateVariables(buffer); buffer_changed = false; } //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(); } }