Revision 0:48c10918dabf, committed 2016-11-08

Comitter:

RichardHoekstra

Date:

Tue Nov 08 18:49:07 2016 +0000

Child:

1:9e6c4011eef6

Commit message:

First message;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 08 18:49:07 2016 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+
+/* De motorcontroller heeft de volgende taken:
+    1.
+        1. Vraag de meest recente primaire sensor data op
+        2. Voer het ingestelde curve programma uit
+        3. Voer PID regelschema uit met set point uit de curve
+        4. Herhaal
+    2.
+        1. Reageer op I2C commandos
+        2. Pas curve modus aan
+        3. Pas curve instellingen aan
+        4. Herbereken curve
+*/
+
+int PID(int val, int set_val){
+    //Gains
+    const int   Kp = 1,
+                Ki = 1,
+                Kd = 1;
+    //Error
+    int err = set_val - val;
+    
+    //Proportional
+    int p_err = err*Kp;
+    
+    //Integral
+    static int prev_i_err = 0;
+    int i_err = prev_i_err+err;
+    prev_i_err = i_err;
+    i_err *= Ki;
+    
+    //Derivative
+    static int prev_err = 0;
+    int d_err = Kd*(prev_err - err)/2;
+    prev_err = err;
+    
+    return p_err + i_err + d_err; 
+}
+
+//Curve variabelen
+const int points_in_curve = 100;
+static float curve[points_in_curve];
+static float curve_max = 120;
+static float curve_min = 80;
+static float frequency = 60;
+
+float curve_arterial(){
+    /* 
+        The curve is precalculated to reduce CPU time
+        To adjust the frequency, the curve is simply executed more quickly.
+        To adjust the curve_max and curve_min, the curve...       
+    */
+    //Recalculate the curve
+    for(int i=0;i<points_in_curve;i++){
+        /*  MATLAB
+            t = linspace(0,1,100);
+            t = t.*0.75;
+            y1 = (0<=t).*(0.3>t).*(80 + 40*sin(8*t))
+            y2 = (0.3<=t).*(0.4>t).*(107)
+            y3 = (0.4<=t).*(-35*sqrt(t-0.45)+107)
+            ytot = y1+y2+y3
+        */
+        // C++?
+        /* 
+            Je hebt een minimale en maximale waarde van de curve
+            De curve wordt zo berekend dat van begin tot eind het equivalent van 'full_interval_ms' is
+            en niet onder of boven de minimale en maximale waarde komt
+            De curve moet verder nog lijken op een arteriele drukcruve
+        
+        */
+    }
+}
+
+float 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;
+    //sin(x) heeft een periode van 2Pi, dus om een periode van 1ms te krijgen maken we daar 2*Pi van
+    //Dat keer de periode in miliseconden (1000/frequentie) geeft ons de juiste uitdrukking voor de sinus
+    float period = 2*3.14159*(1/(1000/(frequency));
+    for(int i=0;i<points_in_curve;i++){
+        curve[i] = offset+amplitude*sin(period*x);
+    }
+}
+
+//The array has 100 points to execute
+//If you take 1ms per point, the entire pulse takes 100ms
+//Therefore, the frequency is 10Hz
+//To go from frequency to ms per point
+int point_interval_ms = (1000/set_frequency)/points_in_curve;
+int full_interval_ms = 1000/set_frequency;
+int main() {
+    Timer t_point;
+    Timer t_full;
+    t_point.start();
+    t_full.start();
+    while(1) {
+        if(t_point.read_ms() > point_interval_ms){
+            PID(readfromsensor,curve[t_full.read_ms());
+            t_point.reset();
+        }
+        if(t_full.read_ms() > full_interval_ms){
+            t_full.reset();   
+        }
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Nov 08 18:49:07 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9bcdf88f62b0
\ No newline at end of file