Revision 1:9e6c4011eef6, committed 2016-11-16

Comitter:

RichardHoekstra

Date:

Wed Nov 16 15:30:21 2016 +0000

Parent:

0:48c10918dabf

Child:

2:b9449fc96691

Commit message:

Motorcontroller I2C is ready and the switch statement is ready to work with codes.

Changed in this revision

--- a/main.cpp	Tue Nov 08 18:49:07 2016 +0000
+++ b/main.cpp	Wed Nov 16 15:30:21 2016 +0000
@@ -1,107 +1,59 @@
 #include "mbed.h"
+#define motor_addr 0x91
+//I2C settings
+#define SDA D14
+#define SCL D15
+#define I2C_BUFFER_SIZE 10
+I2CSlave slave(SDA,SCL);
 
-/* 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
+int main() {
+    slave.address(motor_addr); //Set the correct address for this module
+    char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C
+    bool buffer_changed;
+    while(1) {
+        int i = slave.receive();        
+        switch (i) {
+            case I2CSlave::ReadAddressed:
+                //Received a request to be read
+                //Irrelevant for now
+                break;
+            case I2CSlave::WriteGeneral:
+                //Received a request to be written to
+                slave.read(buffer,I2C_BUFFER_SIZE);
+                buffer_changed = true;
+                break;
+            case I2CSlave::WriteAddressed:
+                //Received a request to be written to a specific location
+                slave.read(buffer,I2C_BUFFER_SIZE);
+                buffer_changed = true;
+                break;
+        }
+        //Do shit depending on the command it received in the first data block
+        if(buffer_changed == true){
+            int command = buffer[0];
+            switch(command){
+                case 0: 
+                    break;
+                case 1:
+                    break;
+                case 2:
+                    break;  
+                case default:
+                    //No command was valid
+                    //Create some kind of error? Maybe, blinking led.
+                    break;  
+            } 
+                
+        }
+        //Clear the buffer for the next iteration
+        if(buffer_changed == true){
+            for(int i=0;i<I2C_BUFFER_SIZE;i++){
+                buffer[i] = 0;    
+            }
+            buffer_changed = false;
+        }
         
-        */
     }
 }
-
-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();   
-        }
-    }
-}