Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
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?
User | Revision | Line number | New 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 | } |