Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

main.cpp@7:ace2a14eff7d, 2016-11-20 (annotated)

Committer:
RichardHoekstra
Date:
Sun Nov 20 13:11:08 2016 +0000
Revision:
7:ace2a14eff7d
Parent:
6:c090a1a7e8dd
Child:
8:648c3963a8e0
Moved the large enum to instructions.h

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