Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
main.cpp@3:10c6e7aaf375, 2016-11-16 (annotated)
- Committer:
- RichardHoekstra
- Date:
- Wed Nov 16 16:51:14 2016 +0000
- Revision:
- 3:10c6e7aaf375
- Parent:
- 2:b9449fc96691
- Child:
- 4:614517a6e2af
Added PID controller from a library instead of my own.
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 | 3:10c6e7aaf375 | 13 | float curve_buffer[CURVE_BUFFER_SIZE]; |
RichardHoekstra | 3:10c6e7aaf375 | 14 | float curve_min=80; //[mmHg] |
RichardHoekstra | 3:10c6e7aaf375 | 15 | float curve_max=120; //[mmHg] |
RichardHoekstra | 3:10c6e7aaf375 | 16 | float curve_period=10; //[ms] |
RichardHoekstra | 3:10c6e7aaf375 | 17 | int curve_step_us=(int)((curve_period*1000)/CURVE_BUFFER_SIZE);//[us] |
RichardHoekstra | 3:10c6e7aaf375 | 18 | Timer t_Curve; |
RichardHoekstra | 3:10c6e7aaf375 | 19 | //PID settings |
RichardHoekstra | 3:10c6e7aaf375 | 20 | Timer t_PID; |
RichardHoekstra | 3:10c6e7aaf375 | 21 | #define PID_PERIOD_S 0.01 |
RichardHoekstra | 3:10c6e7aaf375 | 22 | #define PID_INITIAL_SETPOINT 0 |
RichardHoekstra | 3:10c6e7aaf375 | 23 | //Tuning parameters |
RichardHoekstra | 3:10c6e7aaf375 | 24 | #define Kp 0.1 |
RichardHoekstra | 3:10c6e7aaf375 | 25 | #define Ki 0 |
RichardHoekstra | 3:10c6e7aaf375 | 26 | #define Kc 0 |
RichardHoekstra | 3:10c6e7aaf375 | 27 | //Input is a pressure between 0 and 300 mmHg |
RichardHoekstra | 3:10c6e7aaf375 | 28 | #define PID_MIN_INPUT 0 |
RichardHoekstra | 3:10c6e7aaf375 | 29 | #define PID_MAX_INPUT 300 |
RichardHoekstra | 3:10c6e7aaf375 | 30 | //Output is a PWM duty cycle |
RichardHoekstra | 3:10c6e7aaf375 | 31 | #define PID_MIN_OUTPUT 0 |
RichardHoekstra | 3:10c6e7aaf375 | 32 | #define PID_MAX_OUTPUT 1 |
RichardHoekstra | 3:10c6e7aaf375 | 33 | //PID IO |
RichardHoekstra | 3:10c6e7aaf375 | 34 | #define PID_PIN A0 |
RichardHoekstra | 3:10c6e7aaf375 | 35 | float ProcessValue = 0; |
RichardHoekstra | 2:b9449fc96691 | 36 | |
RichardHoekstra | 2:b9449fc96691 | 37 | //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 | 38 | //heb je meer punten in de buffer nodig. |
RichardHoekstra | 3:10c6e7aaf375 | 39 | void curve_sinus(){ |
RichardHoekstra | 2:b9449fc96691 | 40 | float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude |
RichardHoekstra | 2:b9449fc96691 | 41 | //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten |
RichardHoekstra | 2:b9449fc96691 | 42 | float offset = (curve_max+curve_min)/2; |
RichardHoekstra | 2:b9449fc96691 | 43 | //Genereer een volle periode en zet het in de buffer |
RichardHoekstra | 2:b9449fc96691 | 44 | float step = 2*3.1415926/CURVE_BUFFER_SIZE; |
RichardHoekstra | 2:b9449fc96691 | 45 | for(int i=0;i<CURVE_BUFFER_SIZE;i++){ |
RichardHoekstra | 3:10c6e7aaf375 | 46 | curve_buffer[i] = offset+amplitude*sin(step*i); |
RichardHoekstra | 2:b9449fc96691 | 47 | } |
RichardHoekstra | 2:b9449fc96691 | 48 | } |
RichardHoekstra | 2:b9449fc96691 | 49 | |
RichardHoekstra | 3:10c6e7aaf375 | 50 | /* |
RichardHoekstra | 2:b9449fc96691 | 51 | float curve_arterial(){ |
RichardHoekstra | 3:10c6e7aaf375 | 52 | //Help. |
RichardHoekstra | 2:b9449fc96691 | 53 | } |
RichardHoekstra | 3:10c6e7aaf375 | 54 | */ |
RichardHoekstra | 0:48c10918dabf | 55 | |
RichardHoekstra | 1:9e6c4011eef6 | 56 | int main() { |
RichardHoekstra | 1:9e6c4011eef6 | 57 | slave.address(motor_addr); //Set the correct address for this module |
RichardHoekstra | 3:10c6e7aaf375 | 58 | |
RichardHoekstra | 3:10c6e7aaf375 | 59 | //PID controller setup |
RichardHoekstra | 3:10c6e7aaf375 | 60 | PID controller(Kp, Ki, Kc, PID_PERIOD_S); |
RichardHoekstra | 3:10c6e7aaf375 | 61 | controller.setInputLimits(PID_MIN_INPUT, PID_MAX_INPUT); |
RichardHoekstra | 3:10c6e7aaf375 | 62 | controller.setOutputLimits(PID_MIN_OUTPUT, PID_MAX_OUTPUT); |
RichardHoekstra | 3:10c6e7aaf375 | 63 | controller.setSetPoint(PID_INITIAL_SETPOINT); |
RichardHoekstra | 3:10c6e7aaf375 | 64 | AnalogOut PID_Output(PID_PIN); |
RichardHoekstra | 3:10c6e7aaf375 | 65 | |
RichardHoekstra | 3:10c6e7aaf375 | 66 | t_PID.start(); |
RichardHoekstra | 3:10c6e7aaf375 | 67 | t_Curve.start(); |
RichardHoekstra | 3:10c6e7aaf375 | 68 | |
RichardHoekstra | 3:10c6e7aaf375 | 69 | int PID_last_curve_point = 0; |
RichardHoekstra | 1:9e6c4011eef6 | 70 | char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C |
RichardHoekstra | 1:9e6c4011eef6 | 71 | bool buffer_changed; |
RichardHoekstra | 3:10c6e7aaf375 | 72 | |
RichardHoekstra | 1:9e6c4011eef6 | 73 | while(1) { |
RichardHoekstra | 1:9e6c4011eef6 | 74 | int i = slave.receive(); |
RichardHoekstra | 1:9e6c4011eef6 | 75 | switch (i) { |
RichardHoekstra | 1:9e6c4011eef6 | 76 | case I2CSlave::ReadAddressed: |
RichardHoekstra | 1:9e6c4011eef6 | 77 | //Received a request to be read |
RichardHoekstra | 1:9e6c4011eef6 | 78 | //Irrelevant for now |
RichardHoekstra | 1:9e6c4011eef6 | 79 | break; |
RichardHoekstra | 1:9e6c4011eef6 | 80 | case I2CSlave::WriteGeneral: |
RichardHoekstra | 1:9e6c4011eef6 | 81 | //Received a request to be written to |
RichardHoekstra | 1:9e6c4011eef6 | 82 | slave.read(buffer,I2C_BUFFER_SIZE); |
RichardHoekstra | 1:9e6c4011eef6 | 83 | buffer_changed = true; |
RichardHoekstra | 1:9e6c4011eef6 | 84 | break; |
RichardHoekstra | 1:9e6c4011eef6 | 85 | case I2CSlave::WriteAddressed: |
RichardHoekstra | 1:9e6c4011eef6 | 86 | //Received a request to be written to a specific location |
RichardHoekstra | 1:9e6c4011eef6 | 87 | slave.read(buffer,I2C_BUFFER_SIZE); |
RichardHoekstra | 1:9e6c4011eef6 | 88 | buffer_changed = true; |
RichardHoekstra | 1:9e6c4011eef6 | 89 | break; |
RichardHoekstra | 1:9e6c4011eef6 | 90 | } |
RichardHoekstra | 1:9e6c4011eef6 | 91 | //Do shit depending on the command it received in the first data block |
RichardHoekstra | 1:9e6c4011eef6 | 92 | if(buffer_changed == true){ |
RichardHoekstra | 1:9e6c4011eef6 | 93 | int command = buffer[0]; |
RichardHoekstra | 1:9e6c4011eef6 | 94 | switch(command){ |
RichardHoekstra | 1:9e6c4011eef6 | 95 | case 0: |
RichardHoekstra | 1:9e6c4011eef6 | 96 | break; |
RichardHoekstra | 1:9e6c4011eef6 | 97 | case 1: |
RichardHoekstra | 1:9e6c4011eef6 | 98 | break; |
RichardHoekstra | 1:9e6c4011eef6 | 99 | case 2: |
RichardHoekstra | 1:9e6c4011eef6 | 100 | break; |
RichardHoekstra | 3:10c6e7aaf375 | 101 | default: |
RichardHoekstra | 1:9e6c4011eef6 | 102 | //No command was valid |
RichardHoekstra | 1:9e6c4011eef6 | 103 | //Create some kind of error? Maybe, blinking led. |
RichardHoekstra | 1:9e6c4011eef6 | 104 | break; |
RichardHoekstra | 1:9e6c4011eef6 | 105 | } |
RichardHoekstra | 1:9e6c4011eef6 | 106 | |
RichardHoekstra | 1:9e6c4011eef6 | 107 | } |
RichardHoekstra | 1:9e6c4011eef6 | 108 | //Clear the buffer for the next iteration |
RichardHoekstra | 1:9e6c4011eef6 | 109 | if(buffer_changed == true){ |
RichardHoekstra | 1:9e6c4011eef6 | 110 | for(int i=0;i<I2C_BUFFER_SIZE;i++){ |
RichardHoekstra | 1:9e6c4011eef6 | 111 | buffer[i] = 0; |
RichardHoekstra | 1:9e6c4011eef6 | 112 | } |
RichardHoekstra | 1:9e6c4011eef6 | 113 | buffer_changed = false; |
RichardHoekstra | 1:9e6c4011eef6 | 114 | } |
RichardHoekstra | 3:10c6e7aaf375 | 115 | //Check if the PID scheme has to execute again |
RichardHoekstra | 3:10c6e7aaf375 | 116 | if(t_PID.read() >= PID_PERIOD_S){ |
RichardHoekstra | 3:10c6e7aaf375 | 117 | //Run the PID scheme |
RichardHoekstra | 3:10c6e7aaf375 | 118 | //Update the set point value, the value we want to achieve |
RichardHoekstra | 3:10c6e7aaf375 | 119 | if(t_Curve.read_us() >= curve_step_us){ |
RichardHoekstra | 3:10c6e7aaf375 | 120 | PID_last_curve_point++; |
RichardHoekstra | 3:10c6e7aaf375 | 121 | if(PID_last_curve_point >= CURVE_BUFFER_SIZE){ |
RichardHoekstra | 3:10c6e7aaf375 | 122 | PID_last_curve_point = 0; |
RichardHoekstra | 3:10c6e7aaf375 | 123 | } |
RichardHoekstra | 3:10c6e7aaf375 | 124 | } |
RichardHoekstra | 3:10c6e7aaf375 | 125 | controller.setSetPoint(curve_buffer[PID_last_curve_point]); |
RichardHoekstra | 3:10c6e7aaf375 | 126 | //Update the process value to reflect the current value in the physical word |
RichardHoekstra | 3:10c6e7aaf375 | 127 | controller.setProcessValue(ProcessValue); |
RichardHoekstra | 3:10c6e7aaf375 | 128 | PID_last_curve_point++; |
RichardHoekstra | 3:10c6e7aaf375 | 129 | //Adjust output according to the PID scheme |
RichardHoekstra | 3:10c6e7aaf375 | 130 | PID_Output = controller.compute(); |
RichardHoekstra | 3:10c6e7aaf375 | 131 | } |
RichardHoekstra | 0:48c10918dabf | 132 | |
RichardHoekstra | 0:48c10918dabf | 133 | } |
RichardHoekstra | 0:48c10918dabf | 134 | } |