Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

main.cpp@4:614517a6e2af, 2016-11-17 (annotated)

Committer:
RichardHoekstra
Date:
Thu Nov 17 10:29:58 2016 +0000
Revision:
4:614517a6e2af
Parent:
3:10c6e7aaf375
Child:
5:006b0b8374ea
Finished the code to accept and execute commands. Cleaned up the main() function by moving PID and Command understanding/execution to different functions.

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 4:614517a6e2af 67 int char2_to_int(char* arr, int first_element = 0){
RichardHoekstra 4:614517a6e2af 68 return (arr[first_element]<<8)+arr[first_element+1];
RichardHoekstra 4:614517a6e2af 69 }
RichardHoekstra 4:614517a6e2af 70
RichardHoekstra 4:614517a6e2af 71 //Returns true if it was succesfull at updating
RichardHoekstra 4:614517a6e2af 72 bool updateVariables(char* arr){
RichardHoekstra 4:614517a6e2af 73 enum commands_t { SET_MODE = 1, SET_CONSTANT_PRESSURE, SET_CONSTANT_FLOW, SET_CONSTANT_SPEED, SET_MIN,
RichardHoekstra 4:614517a6e2af 74 SET_MAX, SET_FREQUENCY, RECEIVE_PRESSURE, RECEIVE_FLOW
RichardHoekstra 4:614517a6e2af 75 };
RichardHoekstra 4:614517a6e2af 76 commands_t command_codes = (commands_t)arr[0];
RichardHoekstra 4:614517a6e2af 77 switch(command_codes){
RichardHoekstra 4:614517a6e2af 78 case SET_MODE:
RichardHoekstra 4:614517a6e2af 79 curve_mode = (curve_t)char2_to_int(arr,1);
RichardHoekstra 4:614517a6e2af 80 break;
RichardHoekstra 4:614517a6e2af 81 case SET_CONSTANT_PRESSURE:
RichardHoekstra 4:614517a6e2af 82 constant_pressure = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 83 break;
RichardHoekstra 4:614517a6e2af 84 case SET_CONSTANT_FLOW:
RichardHoekstra 4:614517a6e2af 85 constant_flow = char2_to_int(arr,1)/10.0;
RichardHoekstra 4:614517a6e2af 86 break;
RichardHoekstra 4:614517a6e2af 87 case SET_CONSTANT_SPEED:
RichardHoekstra 4:614517a6e2af 88 constant_speed = (float)char2_to_int(arr,1);
RichardHoekstra 4:614517a6e2af 89 break;
RichardHoekstra 4:614517a6e2af 90 case SET_MIN:
RichardHoekstra 4:614517a6e2af 91 curve_min = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 92 break;
RichardHoekstra 4:614517a6e2af 93 case SET_MAX:
RichardHoekstra 4:614517a6e2af 94 curve_max = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 95 break;
RichardHoekstra 4:614517a6e2af 96 case SET_FREQUENCY:
RichardHoekstra 4:614517a6e2af 97 //Note: it receives a frequency but internally converts it to
RichardHoekstra 4:614517a6e2af 98 // a period in ms immediately.
RichardHoekstra 4:614517a6e2af 99 curve_period = 1000/((char2_to_int(arr,1))/100.0);
RichardHoekstra 4:614517a6e2af 100 break;
RichardHoekstra 4:614517a6e2af 101 case RECEIVE_PRESSURE:
RichardHoekstra 4:614517a6e2af 102 sensor_pressure = char2_to_int(arr,1)/100.0;
RichardHoekstra 4:614517a6e2af 103 break;
RichardHoekstra 4:614517a6e2af 104 case RECEIVE_FLOW:
RichardHoekstra 4:614517a6e2af 105 sensor_flow = char2_to_int(arr,1)/10.0;
RichardHoekstra 4:614517a6e2af 106 break;
RichardHoekstra 4:614517a6e2af 107 default:
RichardHoekstra 4:614517a6e2af 108 //No command was valid
RichardHoekstra 4:614517a6e2af 109 //Create some kind of error? Maybe, a blinking led.
RichardHoekstra 4:614517a6e2af 110 return false;
RichardHoekstra 4:614517a6e2af 111 }
RichardHoekstra 4:614517a6e2af 112 return true;
RichardHoekstra 4:614517a6e2af 113 }
RichardHoekstra 4:614517a6e2af 114
RichardHoekstra 4:614517a6e2af 115 void PID_loop(Timer& t_PID, Timer& t_Curve){
RichardHoekstra 4:614517a6e2af 116 static int PID_last_curve_point = 0;
RichardHoekstra 4:614517a6e2af 117 //Check if the PID scheme has to execute again
RichardHoekstra 4:614517a6e2af 118 if(t_PID.read_us() >= PID_PERIOD_US){
RichardHoekstra 4:614517a6e2af 119 bool PID_on = true;
RichardHoekstra 4:614517a6e2af 120 //What mode is the motorcontroller currently in?
RichardHoekstra 4:614517a6e2af 121 switch(curve_mode){
RichardHoekstra 4:614517a6e2af 122 case CONSTANT_PRESSURE:
RichardHoekstra 4:614517a6e2af 123 //Set Point is the value we want to achieve
RichardHoekstra 4:614517a6e2af 124 //Which in this case is constant
RichardHoekstra 4:614517a6e2af 125 //Process Value is the current value of the process
RichardHoekstra 4:614517a6e2af 126 //Which is variable and connected to processes in the physical world
RichardHoekstra 4:614517a6e2af 127 controller.setSetPoint(constant_pressure);
RichardHoekstra 4:614517a6e2af 128 controller.setProcessValue(sensor_pressure);
RichardHoekstra 4:614517a6e2af 129 break;
RichardHoekstra 4:614517a6e2af 130 case CONSTANT_FLOW:
RichardHoekstra 4:614517a6e2af 131 controller.setSetPoint(constant_flow);
RichardHoekstra 4:614517a6e2af 132 controller.setProcessValue(sensor_flow);
RichardHoekstra 4:614517a6e2af 133 break;
RichardHoekstra 4:614517a6e2af 134 case CONSTANT_SPEED:
RichardHoekstra 4:614517a6e2af 135 //There is already a PI control scheme on the Maxon module
RichardHoekstra 4:614517a6e2af 136 //We only have to send a PWM signal and the module will do the rest
RichardHoekstra 4:614517a6e2af 137 //TODO: Clip this back to a value between 0 and 1
RichardHoekstra 4:614517a6e2af 138 PID_Output = constant_speed;
RichardHoekstra 4:614517a6e2af 139 PID_on = false;
RichardHoekstra 4:614517a6e2af 140 break;
RichardHoekstra 4:614517a6e2af 141 case MODE_SINUS:
RichardHoekstra 4:614517a6e2af 142 //Take the current point in the buffer
RichardHoekstra 4:614517a6e2af 143 controller.setSetPoint(curve_buffer[PID_last_curve_point]);
RichardHoekstra 4:614517a6e2af 144 controller.setProcessValue(sensor_pressure);
RichardHoekstra 4:614517a6e2af 145 break;
RichardHoekstra 4:614517a6e2af 146 case MODE_ARTERIAL:
RichardHoekstra 4:614517a6e2af 147 controller.setSetPoint(curve_buffer[PID_last_curve_point]);
RichardHoekstra 4:614517a6e2af 148 controller.setProcessValue(sensor_pressure);
RichardHoekstra 4:614517a6e2af 149 break;
RichardHoekstra 4:614517a6e2af 150 default:
RichardHoekstra 4:614517a6e2af 151 //Curve should be off or the mode is invalid
RichardHoekstra 4:614517a6e2af 152 PID_on = false;
RichardHoekstra 4:614517a6e2af 153 break;
RichardHoekstra 4:614517a6e2af 154 }
RichardHoekstra 4:614517a6e2af 155 if(PID_on == true){
RichardHoekstra 4:614517a6e2af 156 //Adjust output according to the PID scheme
RichardHoekstra 4:614517a6e2af 157 PID_Output = controller.compute();
RichardHoekstra 4:614517a6e2af 158 }
RichardHoekstra 4:614517a6e2af 159 //For the next iteration, check whether it'll have to be a new point on the curve
RichardHoekstra 4:614517a6e2af 160 //Note: putting this at the end, ensures that in the first loop point 0 will get executed
RichardHoekstra 4:614517a6e2af 161 if(t_Curve.read_us() >= curve_step_us){
RichardHoekstra 4:614517a6e2af 162 PID_last_curve_point++;
RichardHoekstra 4:614517a6e2af 163 if(PID_last_curve_point >= CURVE_BUFFER_SIZE){
RichardHoekstra 4:614517a6e2af 164 PID_last_curve_point = 0;
RichardHoekstra 4:614517a6e2af 165 }
RichardHoekstra 4:614517a6e2af 166 //Reset the curve timer
RichardHoekstra 4:614517a6e2af 167 t_Curve.reset();
RichardHoekstra 4:614517a6e2af 168 }
RichardHoekstra 4:614517a6e2af 169 //Reset the PID timer
RichardHoekstra 4:614517a6e2af 170 t_PID.reset();
RichardHoekstra 4:614517a6e2af 171 }
RichardHoekstra 4:614517a6e2af 172
RichardHoekstra 4:614517a6e2af 173
RichardHoekstra 4:614517a6e2af 174 }
RichardHoekstra 4:614517a6e2af 175
RichardHoekstra 1:9e6c4011eef6 176 int main() {
RichardHoekstra 1:9e6c4011eef6 177 slave.address(motor_addr); //Set the correct address for this module
RichardHoekstra 4:614517a6e2af 178
RichardHoekstra 4:614517a6e2af 179 //PID Controller initialization
RichardHoekstra 3:10c6e7aaf375 180 controller.setInputLimits(PID_MIN_INPUT, PID_MAX_INPUT);
RichardHoekstra 3:10c6e7aaf375 181 controller.setOutputLimits(PID_MIN_OUTPUT, PID_MAX_OUTPUT);
RichardHoekstra 3:10c6e7aaf375 182 controller.setSetPoint(PID_INITIAL_SETPOINT);
RichardHoekstra 4:614517a6e2af 183
RichardHoekstra 4:614517a6e2af 184 Timer t_PID;
RichardHoekstra 4:614517a6e2af 185 Timer t_Curve;
RichardHoekstra 3:10c6e7aaf375 186 t_PID.start();
RichardHoekstra 3:10c6e7aaf375 187 t_Curve.start();
RichardHoekstra 3:10c6e7aaf375 188
RichardHoekstra 1:9e6c4011eef6 189 char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C
RichardHoekstra 4:614517a6e2af 190 bool buffer_changed = false;
RichardHoekstra 3:10c6e7aaf375 191
RichardHoekstra 1:9e6c4011eef6 192 while(1) {
RichardHoekstra 1:9e6c4011eef6 193 int i = slave.receive();
RichardHoekstra 1:9e6c4011eef6 194 switch (i) {
RichardHoekstra 1:9e6c4011eef6 195 case I2CSlave::ReadAddressed:
RichardHoekstra 1:9e6c4011eef6 196 //Received a request to be read
RichardHoekstra 1:9e6c4011eef6 197 //Irrelevant for now
RichardHoekstra 1:9e6c4011eef6 198 break;
RichardHoekstra 1:9e6c4011eef6 199 case I2CSlave::WriteGeneral:
RichardHoekstra 1:9e6c4011eef6 200 //Received a request to be written to
RichardHoekstra 1:9e6c4011eef6 201 slave.read(buffer,I2C_BUFFER_SIZE);
RichardHoekstra 1:9e6c4011eef6 202 buffer_changed = true;
RichardHoekstra 1:9e6c4011eef6 203 break;
RichardHoekstra 1:9e6c4011eef6 204 case I2CSlave::WriteAddressed:
RichardHoekstra 1:9e6c4011eef6 205 //Received a request to be written to a specific location
RichardHoekstra 1:9e6c4011eef6 206 slave.read(buffer,I2C_BUFFER_SIZE);
RichardHoekstra 1:9e6c4011eef6 207 buffer_changed = true;
RichardHoekstra 1:9e6c4011eef6 208 break;
RichardHoekstra 1:9e6c4011eef6 209 }
RichardHoekstra 1:9e6c4011eef6 210 //Do shit depending on the command it received in the first data block
RichardHoekstra 1:9e6c4011eef6 211 if(buffer_changed == true){
RichardHoekstra 4:614517a6e2af 212 updateVariables(buffer);
RichardHoekstra 4:614517a6e2af 213 //Clear the buffer for the next iteration
RichardHoekstra 1:9e6c4011eef6 214 for(int i=0;i<I2C_BUFFER_SIZE;i++){
RichardHoekstra 1:9e6c4011eef6 215 buffer[i] = 0;
RichardHoekstra 1:9e6c4011eef6 216 }
RichardHoekstra 1:9e6c4011eef6 217 buffer_changed = false;
RichardHoekstra 1:9e6c4011eef6 218 }
RichardHoekstra 4:614517a6e2af 219 //Execute the PID loop
RichardHoekstra 4:614517a6e2af 220 PID_loop(t_PID,t_Curve);
RichardHoekstra 0:48c10918dabf 221
RichardHoekstra 0:48c10918dabf 222 }
RichardHoekstra 0:48c10918dabf 223 }