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