Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
main.cpp
- Committer:
- RichardHoekstra
- Date:
- 2016-11-17
- Revision:
- 4:614517a6e2af
- Parent:
- 3:10c6e7aaf375
- Child:
- 5:006b0b8374ea
File content as of revision 4:614517a6e2af:
#include "mbed.h" #include "PID.h" //I2C settings #define SDA D10 #define SCL D11 #define motor_addr 0x91 #define I2C_BUFFER_SIZE 10 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]; 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; //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); //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. } */ int char2_to_int(char* arr, int first_element = 0){ return (arr[first_element]<<8)+arr[first_element+1]; } //Returns true if it was succesfull at updating bool updateVariables(char* arr){ enum commands_t { SET_MODE = 1, SET_CONSTANT_PRESSURE, SET_CONSTANT_FLOW, SET_CONSTANT_SPEED, SET_MIN, SET_MAX, SET_FREQUENCY, RECEIVE_PRESSURE, RECEIVE_FLOW }; commands_t command_codes = (commands_t)arr[0]; switch(command_codes){ 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; break; case SET_MAX: curve_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); break; case RECEIVE_PRESSURE: sensor_pressure = char2_to_int(arr,1)/100.0; break; case RECEIVE_FLOW: sensor_flow = char2_to_int(arr,1)/10.0; break; default: //No command was valid //Create some kind of error? Maybe, a blinking led. return false; } 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(); } } int main() { 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(); char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C bool buffer_changed = false; while(1) { int i = slave.receive(); switch (i) { case I2CSlave::ReadAddressed: //Received a request to be read //Irrelevant for now 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; } //Do shit depending on the command it received in the first data block 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); } }