Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

main.cpp@5:006b0b8374ea, 2016-11-17 (annotated)

Committer:
RichardHoekstra
Date:
Thu Nov 17 10:32:45 2016 +0000
Revision:
5:006b0b8374ea
Parent:
4:614517a6e2af
Child:
6:c090a1a7e8dd
Small comment changes. Minus some profanity

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 3:10c6e7aaf375 3
RichardHoekstra 1:9e6c4011eef6 4 //I2C settings
RichardHoekstra 3:10c6e7aaf375 5 #define SDA D10
RichardHoekstra 3:10c6e7aaf375 6 #define SCL D11
RichardHoekstra 3:10c6e7aaf375 7 #define motor_addr 0x91
RichardHoekstra 3:10c6e7aaf375 8 #define I2C_BUFFER_SIZE 10
RichardHoekstra 3:10c6e7aaf375 9 I2CSlave slave(SDA,SCL);
RichardHoekstra 2:b9449fc96691 10
RichardHoekstra 2:b9449fc96691 11 //Curve settings
RichardHoekstra 3:10c6e7aaf375 12 #define CURVE_BUFFER_SIZE 100
RichardHoekstra 4:614517a6e2af 13 enum curve_t { OFF=0, CONSTANT_PRESSURE, CONSTANT_FLOW, CONSTANT_SPEED, MODE_SINUS, MODE_ARTERIAL
RichardHoekstra 4:614517a6e2af 14 } curve_mode;
RichardHoekstra 3:10c6e7aaf375 15 float curve_buffer[CURVE_BUFFER_SIZE];
RichardHoekstra 3:10c6e7aaf375 16 float curve_min=80; //[mmHg]
RichardHoekstra 3:10c6e7aaf375 17 float curve_max=120; //[mmHg]
RichardHoekstra 3:10c6e7aaf375 18 float curve_period=10; //[ms]
RichardHoekstra 3:10c6e7aaf375 19 int curve_step_us=(int)((curve_period*1000)/CURVE_BUFFER_SIZE);//[us]
RichardHoekstra 4:614517a6e2af 20 //Constant variables
RichardHoekstra 4:614517a6e2af 21 float constant_pressure;
RichardHoekstra 4:614517a6e2af 22 float constant_flow;
RichardHoekstra 4:614517a6e2af 23 float constant_speed;
RichardHoekstra 4:614517a6e2af 24 //Sensor variables
RichardHoekstra 4:614517a6e2af 25 float sensor_pressure;
RichardHoekstra 4:614517a6e2af 26 float sensor_flow;
RichardHoekstra 4:614517a6e2af 27
RichardHoekstra 3:10c6e7aaf375 28 //PID settings
RichardHoekstra 4:614517a6e2af 29 #define PID_PERIOD_US 1000
RichardHoekstra 3:10c6e7aaf375 30 #define PID_INITIAL_SETPOINT 0
RichardHoekstra 3:10c6e7aaf375 31 //Tuning parameters
RichardHoekstra 3:10c6e7aaf375 32 #define Kp 0.1
RichardHoekstra 3:10c6e7aaf375 33 #define Ki 0
RichardHoekstra 3:10c6e7aaf375 34 #define Kc 0
RichardHoekstra 3:10c6e7aaf375 35 //Input is a pressure between 0 and 300 mmHg
RichardHoekstra 3:10c6e7aaf375 36 #define PID_MIN_INPUT 0
RichardHoekstra 3:10c6e7aaf375 37 #define PID_MAX_INPUT 300
RichardHoekstra 3:10c6e7aaf375 38 //Output is a PWM duty cycle
RichardHoekstra 3:10c6e7aaf375 39 #define PID_MIN_OUTPUT 0
RichardHoekstra 3:10c6e7aaf375 40 #define PID_MAX_OUTPUT 1
RichardHoekstra 3:10c6e7aaf375 41 //PID IO
RichardHoekstra 3:10c6e7aaf375 42 #define PID_PIN A0
RichardHoekstra 3:10c6e7aaf375 43 float ProcessValue = 0;
RichardHoekstra 4:614517a6e2af 44 //PID Controller
RichardHoekstra 4:614517a6e2af 45 PID controller(Kp, Ki, Kc, PID_PERIOD_US);
RichardHoekstra 4:614517a6e2af 46 AnalogOut PID_Output(PID_PIN);
RichardHoekstra 2:b9449fc96691 47
RichardHoekstra 2:b9449fc96691 48 //Note: om de frequentie aan te passen speel je de buffer sneller af. Hierbij neemt nauwkeurigheid wel af. Om dit te verminderen
RichardHoekstra 2:b9449fc96691 49 //heb je meer punten in de buffer nodig.
RichardHoekstra 3:10c6e7aaf375 50 void curve_sinus(){
RichardHoekstra 2:b9449fc96691 51 float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude
RichardHoekstra 2:b9449fc96691 52 //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten
RichardHoekstra 2:b9449fc96691 53 float offset = (curve_max+curve_min)/2;
RichardHoekstra 2:b9449fc96691 54 //Genereer een volle periode en zet het in de buffer
RichardHoekstra 2:b9449fc96691 55 float step = 2*3.1415926/CURVE_BUFFER_SIZE;
RichardHoekstra 2:b9449fc96691 56 for(int i=0;i<CURVE_BUFFER_SIZE;i++){
RichardHoekstra 3:10c6e7aaf375 57 curve_buffer[i] = offset+amplitude*sin(step*i);
RichardHoekstra 2:b9449fc96691 58 }
RichardHoekstra 2:b9449fc96691 59 }
RichardHoekstra 2:b9449fc96691 60
RichardHoekstra 3:10c6e7aaf375 61 /*
RichardHoekstra 2:b9449fc96691 62 float curve_arterial(){
RichardHoekstra 3:10c6e7aaf375 63 //Help.
RichardHoekstra 2:b9449fc96691 64 }
RichardHoekstra 3:10c6e7aaf375 65 */
RichardHoekstra 0:48c10918dabf 66
RichardHoekstra 5:006b0b8374ea 67 //Join two char values into one integer value
RichardHoekstra 4:614517a6e2af 68 int char2_to_int(char* arr, int first_element = 0){
RichardHoekstra 4:614517a6e2af 69 return (arr[first_element]<<8)+arr[first_element+1];
RichardHoekstra 4:614517a6e2af 70 }
RichardHoekstra 4:614517a6e2af 71
RichardHoekstra 5:006b0b8374ea 72 //Returns true if it was succesful at updating
RichardHoekstra 4:614517a6e2af 73 bool updateVariables(char* arr){
RichardHoekstra 4:614517a6e2af 74 enum commands_t { SET_MODE = 1, SET_CONSTANT_PRESSURE, SET_CONSTANT_FLOW, SET_CONSTANT_SPEED, SET_MIN,
RichardHoekstra 4:614517a6e2af 75 SET_MAX, SET_FREQUENCY, RECEIVE_PRESSURE, RECEIVE_FLOW
RichardHoekstra 4:614517a6e2af 76 };
RichardHoekstra 4:614517a6e2af 77 commands_t command_codes = (commands_t)arr[0];
RichardHoekstra 4:614517a6e2af 78 switch(command_codes){
RichardHoekstra 4:614517a6e2af 79 case SET_MODE:
RichardHoekstra 4:614517a6e2af 80 curve_mode = (curve_t)char2_to_int(arr,1);
RichardHoekstra 4:614517a6e2af 81 break;
RichardHoekstra 4:614517a6e2af 82 case SET_CONSTANT_PRESSURE:
RichardHoekstra 4:614517a6e2af 83 constant_pressure = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 84 break;
RichardHoekstra 4:614517a6e2af 85 case SET_CONSTANT_FLOW:
RichardHoekstra 4:614517a6e2af 86 constant_flow = char2_to_int(arr,1)/10.0;
RichardHoekstra 4:614517a6e2af 87 break;
RichardHoekstra 4:614517a6e2af 88 case SET_CONSTANT_SPEED:
RichardHoekstra 4:614517a6e2af 89 constant_speed = (float)char2_to_int(arr,1);
RichardHoekstra 4:614517a6e2af 90 break;
RichardHoekstra 4:614517a6e2af 91 case SET_MIN:
RichardHoekstra 4:614517a6e2af 92 curve_min = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 93 break;
RichardHoekstra 4:614517a6e2af 94 case SET_MAX:
RichardHoekstra 4:614517a6e2af 95 curve_max = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 96 break;
RichardHoekstra 4:614517a6e2af 97 case SET_FREQUENCY:
RichardHoekstra 4:614517a6e2af 98 //Note: it receives a frequency but internally converts it to
RichardHoekstra 4:614517a6e2af 99 // a period in ms immediately.
RichardHoekstra 4:614517a6e2af 100 curve_period = 1000/((char2_to_int(arr,1))/100.0);
RichardHoekstra 4:614517a6e2af 101 break;
RichardHoekstra 4:614517a6e2af 102 case RECEIVE_PRESSURE:
RichardHoekstra 4:614517a6e2af 103 sensor_pressure = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 104 break;
RichardHoekstra 4:614517a6e2af 105 case RECEIVE_FLOW:
RichardHoekstra 4:614517a6e2af 106 sensor_flow = char2_to_int(arr,1)/10.0;
RichardHoekstra 4:614517a6e2af 107 break;
RichardHoekstra 4:614517a6e2af 108 default:
RichardHoekstra 4:614517a6e2af 109 //No command was valid
RichardHoekstra 4:614517a6e2af 110 //Create some kind of error? Maybe, a blinking led.
RichardHoekstra 4:614517a6e2af 111 return false;
RichardHoekstra 4:614517a6e2af 112 }
RichardHoekstra 4:614517a6e2af 113 return true;
RichardHoekstra 4:614517a6e2af 114 }
RichardHoekstra 4:614517a6e2af 115
RichardHoekstra 4:614517a6e2af 116 void PID_loop(Timer& t_PID, Timer& t_Curve){
RichardHoekstra 4:614517a6e2af 117 static int PID_last_curve_point = 0;
RichardHoekstra 4:614517a6e2af 118 //Check if the PID scheme has to execute again
RichardHoekstra 4:614517a6e2af 119 if(t_PID.read_us() >= PID_PERIOD_US){
RichardHoekstra 4:614517a6e2af 120 bool PID_on = true;
RichardHoekstra 4:614517a6e2af 121 //What mode is the motorcontroller currently in?
RichardHoekstra 4:614517a6e2af 122 switch(curve_mode){
RichardHoekstra 4:614517a6e2af 123 case CONSTANT_PRESSURE:
RichardHoekstra 4:614517a6e2af 124 //Set Point is the value we want to achieve
RichardHoekstra 4:614517a6e2af 125 //Which in this case is constant
RichardHoekstra 4:614517a6e2af 126 //Process Value is the current value of the process
RichardHoekstra 4:614517a6e2af 127 //Which is variable and connected to processes in the physical world
RichardHoekstra 4:614517a6e2af 128 controller.setSetPoint(constant_pressure);
RichardHoekstra 4:614517a6e2af 129 controller.setProcessValue(sensor_pressure);
RichardHoekstra 4:614517a6e2af 130 break;
RichardHoekstra 4:614517a6e2af 131 case CONSTANT_FLOW:
RichardHoekstra 4:614517a6e2af 132 controller.setSetPoint(constant_flow);
RichardHoekstra 4:614517a6e2af 133 controller.setProcessValue(sensor_flow);
RichardHoekstra 4:614517a6e2af 134 break;
RichardHoekstra 4:614517a6e2af 135 case CONSTANT_SPEED:
RichardHoekstra 4:614517a6e2af 136 //There is already a PI control scheme on the Maxon module
RichardHoekstra 4:614517a6e2af 137 //We only have to send a PWM signal and the module will do the rest
RichardHoekstra 4:614517a6e2af 138 //TODO: Clip this back to a value between 0 and 1
RichardHoekstra 4:614517a6e2af 139 PID_Output = constant_speed;
RichardHoekstra 4:614517a6e2af 140 PID_on = false;
RichardHoekstra 4:614517a6e2af 141 break;
RichardHoekstra 4:614517a6e2af 142 case MODE_SINUS:
RichardHoekstra 4:614517a6e2af 143 //Take the current point in the buffer
RichardHoekstra 4:614517a6e2af 144 controller.setSetPoint(curve_buffer[PID_last_curve_point]);
RichardHoekstra 4:614517a6e2af 145 controller.setProcessValue(sensor_pressure);
RichardHoekstra 4:614517a6e2af 146 break;
RichardHoekstra 4:614517a6e2af 147 case MODE_ARTERIAL:
RichardHoekstra 4:614517a6e2af 148 controller.setSetPoint(curve_buffer[PID_last_curve_point]);
RichardHoekstra 4:614517a6e2af 149 controller.setProcessValue(sensor_pressure);
RichardHoekstra 4:614517a6e2af 150 break;
RichardHoekstra 4:614517a6e2af 151 default:
RichardHoekstra 4:614517a6e2af 152 //Curve should be off or the mode is invalid
RichardHoekstra 4:614517a6e2af 153 PID_on = false;
RichardHoekstra 4:614517a6e2af 154 break;
RichardHoekstra 4:614517a6e2af 155 }
RichardHoekstra 4:614517a6e2af 156 if(PID_on == true){
RichardHoekstra 4:614517a6e2af 157 //Adjust output according to the PID scheme
RichardHoekstra 4:614517a6e2af 158 PID_Output = controller.compute();
RichardHoekstra 4:614517a6e2af 159 }
RichardHoekstra 4:614517a6e2af 160 //For the next iteration, check whether it'll have to be a new point on the curve
RichardHoekstra 4:614517a6e2af 161 //Note: putting this at the end, ensures that in the first loop point 0 will get executed
RichardHoekstra 4:614517a6e2af 162 if(t_Curve.read_us() >= curve_step_us){
RichardHoekstra 4:614517a6e2af 163 PID_last_curve_point++;
RichardHoekstra 4:614517a6e2af 164 if(PID_last_curve_point >= CURVE_BUFFER_SIZE){
RichardHoekstra 4:614517a6e2af 165 PID_last_curve_point = 0;
RichardHoekstra 4:614517a6e2af 166 }
RichardHoekstra 4:614517a6e2af 167 //Reset the curve timer
RichardHoekstra 4:614517a6e2af 168 t_Curve.reset();
RichardHoekstra 4:614517a6e2af 169 }
RichardHoekstra 4:614517a6e2af 170 //Reset the PID timer
RichardHoekstra 4:614517a6e2af 171 t_PID.reset();
RichardHoekstra 4:614517a6e2af 172 }
RichardHoekstra 4:614517a6e2af 173
RichardHoekstra 4:614517a6e2af 174
RichardHoekstra 4:614517a6e2af 175 }
RichardHoekstra 4:614517a6e2af 176
RichardHoekstra 1:9e6c4011eef6 177 int main() {
RichardHoekstra 1:9e6c4011eef6 178 slave.address(motor_addr); //Set the correct address for this module
RichardHoekstra 4:614517a6e2af 179
RichardHoekstra 4:614517a6e2af 180 //PID Controller initialization
RichardHoekstra 3:10c6e7aaf375 181 controller.setInputLimits(PID_MIN_INPUT, PID_MAX_INPUT);
RichardHoekstra 3:10c6e7aaf375 182 controller.setOutputLimits(PID_MIN_OUTPUT, PID_MAX_OUTPUT);
RichardHoekstra 3:10c6e7aaf375 183 controller.setSetPoint(PID_INITIAL_SETPOINT);
RichardHoekstra 4:614517a6e2af 184
RichardHoekstra 4:614517a6e2af 185 Timer t_PID;
RichardHoekstra 4:614517a6e2af 186 Timer t_Curve;
RichardHoekstra 3:10c6e7aaf375 187 t_PID.start();
RichardHoekstra 3:10c6e7aaf375 188 t_Curve.start();
RichardHoekstra 3:10c6e7aaf375 189
RichardHoekstra 1:9e6c4011eef6 190 char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C
RichardHoekstra 4:614517a6e2af 191 bool buffer_changed = false;
RichardHoekstra 3:10c6e7aaf375 192
RichardHoekstra 1:9e6c4011eef6 193 while(1) {
RichardHoekstra 1:9e6c4011eef6 194 int i = slave.receive();
RichardHoekstra 1:9e6c4011eef6 195 switch (i) {
RichardHoekstra 1:9e6c4011eef6 196 case I2CSlave::ReadAddressed:
RichardHoekstra 1:9e6c4011eef6 197 //Received a request to be read
RichardHoekstra 1:9e6c4011eef6 198 //Irrelevant for now
RichardHoekstra 1:9e6c4011eef6 199 break;
RichardHoekstra 1:9e6c4011eef6 200 case I2CSlave::WriteGeneral:
RichardHoekstra 1:9e6c4011eef6 201 //Received a request to be written to
RichardHoekstra 1:9e6c4011eef6 202 slave.read(buffer,I2C_BUFFER_SIZE);
RichardHoekstra 1:9e6c4011eef6 203 buffer_changed = true;
RichardHoekstra 1:9e6c4011eef6 204 break;
RichardHoekstra 1:9e6c4011eef6 205 case I2CSlave::WriteAddressed:
RichardHoekstra 1:9e6c4011eef6 206 //Received a request to be written to a specific location
RichardHoekstra 1:9e6c4011eef6 207 slave.read(buffer,I2C_BUFFER_SIZE);
RichardHoekstra 1:9e6c4011eef6 208 buffer_changed = true;
RichardHoekstra 1:9e6c4011eef6 209 break;
RichardHoekstra 1:9e6c4011eef6 210 }
RichardHoekstra 5:006b0b8374ea 211 //Update the variables if the buffer has changed.
RichardHoekstra 1:9e6c4011eef6 212 if(buffer_changed == true){
RichardHoekstra 4:614517a6e2af 213 updateVariables(buffer);
RichardHoekstra 4:614517a6e2af 214 //Clear the buffer for the next iteration
RichardHoekstra 1:9e6c4011eef6 215 for(int i=0;i<I2C_BUFFER_SIZE;i++){
RichardHoekstra 1:9e6c4011eef6 216 buffer[i] = 0;
RichardHoekstra 1:9e6c4011eef6 217 }
RichardHoekstra 1:9e6c4011eef6 218 buffer_changed = false;
RichardHoekstra 1:9e6c4011eef6 219 }
RichardHoekstra 4:614517a6e2af 220 //Execute the PID loop
RichardHoekstra 4:614517a6e2af 221 PID_loop(t_PID,t_Curve);
RichardHoekstra 0:48c10918dabf 222
RichardHoekstra 0:48c10918dabf 223 }
RichardHoekstra 0:48c10918dabf 224 }