Diff: main.cpp

Revision:

3:10c6e7aaf375

Parent:

2:b9449fc96691

Child:

4:614517a6e2af

diff -r b9449fc96691 -r 10c6e7aaf375 main.cpp
--- 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();
+        } 
         
     }
 }