Richard Hoekstra
/
TLS2-Motorcontroller_v2
De motorcontroller van het TLS2 project.
Diff: main.cpp
- Revision:
- 3:10c6e7aaf375
- Parent:
- 2:b9449fc96691
- Child:
- 4:614517a6e2af
--- a/main.cpp Wed Nov 16 15:49:30 2016 +0000 +++ b/main.cpp Wed Nov 16 16:51:14 2016 +0000 @@ -1,41 +1,75 @@ #include "mbed.h" -#define motor_addr 0x91 +#include "PID.h" + //I2C settings -#define SDA D14 -#define SCL D15 -#define I2C_BUFFER_SIZE 10 -I2CSlave slave(SDA,SCL); - -#define CURVE_BUFFER_SIZE 100 + #define SDA D10 + #define SCL D11 + #define motor_addr 0x91 + #define I2C_BUFFER_SIZE 10 + I2CSlave slave(SDA,SCL); //Curve settings -float curve_buffer[CURVE_BUFFER_SIZE]; -float curve_min=80; //[mmHg] -float curve_max=120; //[mmHg] -float curve_period=10; //[ms] + #define CURVE_BUFFER_SIZE 100 + float curve_buffer[CURVE_BUFFER_SIZE]; + float curve_min=80; //[mmHg] + float curve_max=120; //[mmHg] + float curve_period=10; //[ms] + int curve_step_us=(int)((curve_period*1000)/CURVE_BUFFER_SIZE);//[us] + Timer t_Curve; +//PID settings + Timer t_PID; + #define PID_PERIOD_S 0.01 + #define PID_INITIAL_SETPOINT 0 + //Tuning parameters + #define Kp 0.1 + #define Ki 0 + #define Kc 0 + //Input is a pressure between 0 and 300 mmHg + #define PID_MIN_INPUT 0 + #define PID_MAX_INPUT 300 + //Output is a PWM duty cycle + #define PID_MIN_OUTPUT 0 + #define PID_MAX_OUTPUT 1 + //PID IO + #define PID_PIN A0 + float ProcessValue = 0; //Note: om de frequentie aan te passen speel je de buffer sneller af. Hierbij neemt nauwkeurigheid wel af. Om dit te verminderen //heb je meer punten in de buffer nodig. -float curve_sinus(){ +void curve_sinus(){ float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten float offset = (curve_max+curve_min)/2; //Genereer een volle periode en zet het in de buffer float step = 2*3.1415926/CURVE_BUFFER_SIZE; for(int i=0;i<CURVE_BUFFER_SIZE;i++){ - curve_buffer[i] = offset+amplitude*sin(step*x); + curve_buffer[i] = offset+amplitude*sin(step*i); } } +/* float curve_arterial(){ - //Help. + //Help. } - +*/ int main() { slave.address(motor_addr); //Set the correct address for this module + + //PID controller setup + PID controller(Kp, Ki, Kc, PID_PERIOD_S); + controller.setInputLimits(PID_MIN_INPUT, PID_MAX_INPUT); + controller.setOutputLimits(PID_MIN_OUTPUT, PID_MAX_OUTPUT); + controller.setSetPoint(PID_INITIAL_SETPOINT); + AnalogOut PID_Output(PID_PIN); + + t_PID.start(); + t_Curve.start(); + + int PID_last_curve_point = 0; char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C bool buffer_changed; + while(1) { int i = slave.receive(); switch (i) { @@ -64,7 +98,7 @@ break; case 2: break; - case default: + default: //No command was valid //Create some kind of error? Maybe, blinking led. break; @@ -78,6 +112,23 @@ } buffer_changed = false; } + //Check if the PID scheme has to execute again + if(t_PID.read() >= PID_PERIOD_S){ + //Run the PID scheme + //Update the set point value, the value we want to achieve + if(t_Curve.read_us() >= curve_step_us){ + PID_last_curve_point++; + if(PID_last_curve_point >= CURVE_BUFFER_SIZE){ + PID_last_curve_point = 0; + } + } + controller.setSetPoint(curve_buffer[PID_last_curve_point]); + //Update the process value to reflect the current value in the physical word + controller.setProcessValue(ProcessValue); + PID_last_curve_point++; + //Adjust output according to the PID scheme + PID_Output = controller.compute(); + } } }