Diff: accgyro.cpp

Revision:

4:99891561a38b

Parent:

2:7d574b1ab3cd

--- a/accgyro.cpp	Wed May 02 07:49:13 2018 +0000
+++ b/accgyro.cpp	Fri May 25 09:00:15 2018 +0000
@@ -1,45 +1,109 @@
-#include "LSM6DSLSensor.h"
+#include "LSM6DSLSensor.h" 
 #include "accgyro.h"
 #include "mbed.h"
-#include "rtos.h"
+#include "behaviour.h"
 
-//Serial pc(USBTX, USBRX);
-DigitalOut led1(LED1);
+int i;
 int32_t ACCGYRO_gyro_axes[3];
 int32_t ACCGYRO_acc_axes[3];
+int32_t gyro_x[5];
+int32_t GYRO_Xf = 0;
+int32_t GYRO_Yf = 0;
+int32_t GYRO_Zf = 0;
+int32_t GYRO_Xfold;
+int32_t GYRO_Yfold;
+int32_t GYRO_Zfold;
+float GYRO_X_D;
+float GYRO_Y_D;
+float GYRO_Z_D;
+float GYRO_X_Dold;
+float GYRO_Y_Dold;
+float GYRO_Z_Dold;
+float GYRO_X_Time;
+float GYRO_Y_Time;
+float GYRO_Z_Time;
+
 static DevI2C devI2c(PB_11,PB_10);
 static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11);
+
+Timer t;
+Ticker getdata;
 EventQueue ACCGYRO_queue;
+extern EventQueue BEHAVIOUR_queue;
+
 //Mutex ACCGYRO_mutex;
-
 int32_t ACCGYRO_get_gyro_x()
 {
     int32_t temp;
-    //ACCGYRO_mutex.lock();
     temp = ACCGYRO_gyro_axes[0];
-    //ACCGYRO_mutex.unlock();
+    return temp;
+}
+
+int32_t ACCGYRO_get_gyro_y()
+{
+    int32_t temp;
+    temp = ACCGYRO_gyro_axes[1];
     return temp;
 }
 
+int32_t ACCGYRO_get_gyro_z()
+{
+    int32_t temp;
+    temp = ACCGYRO_gyro_axes[2];
+    return temp;
+}
+/*
+void GYRO_get()
+{
+    
+    for (i=0; i<5; i++) 
+    {
+        gyro_x[i] = ACCGYRO_gyro_axes[0];
+    }
+    printf("%61d\n", &i);
+}
+*/
+
 void ACCGYRO_task()
 {
-    led1 = !led1;
-    //ACCGYRO_mutex.lock();
-    acc_gyro.get_x_axes(ACCGYRO_gyro_axes);
-    acc_gyro.get_g_axes(ACCGYRO_acc_axes);
-    //ACCGYRO_mutex.unlock();
-    //pc.printf("LSM6DSL [acc/mg]:        %6ld\r\n", ACCGYRO_gyro_axes[0]);
+    //int i;
+    acc_gyro.get_x_axes(ACCGYRO_acc_axes);
+    acc_gyro.get_g_axes(ACCGYRO_gyro_axes);
+    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_x_set,ACCGYRO_gyro_axes[0]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_y_set,ACCGYRO_gyro_axes[1]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_z_set,ACCGYRO_gyro_axes[2]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_acc_x_set,ACCGYRO_acc_axes[0]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_acc_y_set,ACCGYRO_acc_axes[1]);
+    BEHAVIOUR_queue.call(BEHAVIOUR_acc_z_set,ACCGYRO_acc_axes[2]);
+    t. start();
+    for (i=0; i<5; i++)
+    {
+        gyro_x[i] = ACCGYRO_gyro_axes[0];
+    }
+    t.stop();
+    GYRO_X_Time = t.read();
+    //printf("The time taken was %f seconds\n", GYRO_X_Time);
+    t.reset();
+    GYRO_Xf = gyro_x[0];
+    GYRO_Xfold = gyro_x[4];
+    GYRO_X_D = (GYRO_Xf  + GYRO_Xfold)*0.5 * GYRO_X_Time *1000+ GYRO_X_Dold;
+    printf("%f\r\n", GYRO_X_D);
+/*
+    if(GYRO_X_D>180.0)
+        GYRO_X_D = GYRO_X_D - 360.0;
+    else
+    if(GYRO_X_D<-180.0)
+        GYRO_X_D = GYRO_X_D + 360.0;
+*/
 };
 
 void ACCGYRO_init()
 {
-    //pc.printf("Initing LSM6DSL\r\n", ACCGYRO_gyro_axes[0]);
+    printf("Initing LSM6DSL\r\n");
     acc_gyro.init(NULL);
     acc_gyro.enable_x();
     acc_gyro.enable_g();
-    //RtosTimer ACCGYRO_Timer(&ACCGYRO_task,osTimerPeriodic);
-    //ACCGYRO_Timer.start(100);
-    //pc.printf("Init done\r\n");
-    //ACCGYRO_queue.call_every(1000, ACCGYRO_task);
-    //ACCGYRO_queue.dispatch();
+    printf("Init done\r\n");
+    ACCGYRO_queue.call_every(100, ACCGYRO_task);
+    ACCGYRO_queue.dispatch();
 };