Richard Hoekstra / Mbed 2 deprecated TLS2-Motorcontroller_v2

De motorcontroller van het TLS2 project.

Dependencies:   mbed PID

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?

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 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 }