Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
main.cpp
- Committer:
- RichardHoekstra
- Date:
- 2016-11-22
- Revision:
- 9:1bdf5107920f
- Parent:
- 8:648c3963a8e0
File content as of revision 9:1bdf5107920f:
#include "mbed.h" #include "PID.h" #include "instructions.h" #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 #define motor_addr 0x91 #define I2C_BUFFER_SIZE 10 I2CSlave slave(SDA,SCL); //Sensor variables float sensor_pressure; float sensor_flow; // ----- 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; } } //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: control.set_mode(char2_to_int(arr,1)); break; case SET_CONSTANT_PRESSURE: control.set_constant_pressure(char2_to_int(arr,1)/100.0); break; case SET_CONSTANT_FLOW: control.set_constant_flow(char2_to_int(arr,1)/10.0); break; case SET_CONSTANT_SPEED: control.set_constant_speed((float)char2_to_int(arr,1)); break; case SET_MIN: control.set_min(char2_to_int(arr,1)/100.0); break; case SET_MAX: 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. control.set_period(1000/((char2_to_int(arr,1))/100.0)); 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; } 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 char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C bool buffer_changed = false; //Calibrate the motor controller on the pressure sensor control.normalize(drukSensor1); control.start(); 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(); } }