Diff: main.cpp

Revision:

4:f412749d800d

Parent:

3:c2ab3a0de448

--- a/main.cpp	Mon Sep 26 05:29:51 2016 +0000
+++ b/main.cpp	Mon May 01 11:18:03 2017 +0000
@@ -4,11 +4,15 @@
 #include "OLED_types.h"
 #include "OpenSans_Font.h"
 #include "string.h"
+#include "FXAS21002.h"
+#include "FXOS8700.h"
 
 #define LED_ON      0
 #define LED_OFF     1
+#define DECLINATION -4.94
+#define PI 3.14159
 
-void UpdateSensorData(void);
+
 void StartHaptic(void);
 void StopHaptic(void const *n);
 void txTask(void);
@@ -18,6 +22,8 @@
 DigitalOut blueLed(LED3,1);
 DigitalOut haptic(PTB9);
 
+
+Serial pc(USBTX, USBRX);
 /* Define timer for haptic feedback */
 RtosTimer hapticTimer(StopHaptic, osTimerOnce);
 
@@ -27,6 +33,19 @@
 /* Instantiate the SSD1351 OLED Driver */ 
 SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); /* (MOSI,SCLK,POWER,CS,RST,DC) */
 
+/* Instantiate the gyro */
+FXAS21002 gyro(PTC11,PTC10);
+FXOS8700 accel(PTC11, PTC10);
+FXOS8700 mag(PTC11, PTC10);
+// Storage for the data from the sensor
+float gyro_data[3];  float gyro_rms=0.0;
+float accel_data[3]; float accel_rms=0.0;
+float mag_data[3];   float mag_rms=0.0;
+float roll,pitch,heading;
+float new_roll,new_pitch,new_heading;
+int del_roll,del_pitch; 
+volatile bool flag=false; 
+Timeout timeout;
 /*Create a Thread to handle sending BLE Sensor Data */ 
 Thread txThread;
 
@@ -42,6 +61,22 @@
 uint16_t y = 5000;
 uint16_t z = 10000;
 
+/*Get the new value of roll and pitch*/
+void get_new_value(){
+    gyro.acquire_gyro_data_dps(gyro_data);
+    accel.acquire_accel_data_g(accel_data);
+    new_roll = atan2(accel_data[2],accel_data[1]);
+    new_pitch = atan2(-accel_data[0],sqrt(accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2]));
+    new_pitch *= 180.0 / PI;
+    new_roll  *= 180.0 / PI;
+    new_roll+=180;
+}
+/*Click the button to start it*/
+void Start_Detect(){
+    if(flag==true)
+    return;
+    flag=true;    
+}
 /****************************Call Back Functions*******************************/
 void ButtonRight(void)
 {
@@ -76,7 +111,10 @@
     kw40z_device.attach_buttonLeft(&ButtonLeft);
     kw40z_device.attach_buttonRight(&ButtonRight);
     kw40z_device.attach_passkey(&PassKey);
-
+    kw40z_device.attach_buttonUp(&Start_Detect);//Click the button to start it
+    gyro.gyro_config();
+    accel.accel_config();
+    mag.mag_config();
     /* Turn on the backlight of the OLED Display */
     oled.DimScreenON();
     
@@ -124,59 +162,48 @@
    
    while (true) 
    {
-        UpdateSensorData();
-        
-        /*Notify Hexiwear App that it is running Sensor Tag mode*/
-        kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG);
-                
-        /*The following is sending dummy data over BLE. Replace with real data*/
-    
-        /*Send Battery Level for 20% */ 
-        kw40z_device.SendBatteryLevel(battery);
-               
-        /*Send Ambient Light Level at 50% */ 
-        kw40z_device.SendAmbientLight(light);
-        
-        /*Send Humidity at 90% */
-        kw40z_device.SendHumidity(humidity);
-        
-        /*Send Temperature at 25 degrees Celsius */
-        kw40z_device.SendTemperature(temperature);
-
-        /*Send Pressure at 100kPA */ 
-        kw40z_device.SendPressure(pressure);
         
-        /*Send Mag,Accel,Gyro Data. */
-        kw40z_device.SendGyro(x,y,z);
-        kw40z_device.SendAccel(z,x,y);
-        kw40z_device.SendMag(y,z,x);
-
-        Thread::wait(1000);                 
+        if(flag){
+            kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG);
+            gyro.acquire_gyro_data_dps(gyro_data);
+            accel.acquire_accel_data_g(accel_data);
+            roll = atan2(accel_data[2],accel_data[1]);
+            pitch = atan2(-accel_data[0],sqrt(accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2]));
+            pitch *= 180.0 / PI;
+            roll  *= 180.0 / PI;
+            roll+=180;
+            
+            Thread::wait(1000);        
+            get_new_value();
+            float cmd; 
+            
+            if(abs(new_roll-roll)>abs(new_pitch-pitch)){
+                if(new_roll-roll>30)
+                    cmd=100;
+                else if(new_roll-roll<-30)
+                    cmd=200;
+                else 
+                    cmd=0;   
+            }else if(abs(new_roll-roll)<abs(new_pitch-pitch)){
+                if(new_pitch-pitch>10)
+                    cmd=300;
+                else if(new_pitch-pitch<-10)
+                    cmd=400;
+                else 
+                    cmd=0;
+            }else   
+                    cmd=0;
+            pc.printf("cmd=%f",cmd);
+            kw40z_device.SendTemperature(cmd);
+            Thread::wait(6000);//Make some delay to wait the data upload to the server
+            kw40z_device.SendTemperature(0);//send 0 to the server
+            flag=false;
+        }
+        
+        Thread::wait(500);                 
     }
 }
 
-void UpdateSensorData(void)
-{    
-    battery -= 5;
-    if(battery < 5) battery = 100;
-    
-    light += 20;
-    if(light > 100) light = 0;
-    
-    humidity += 500;
-    if(humidity > 8000) humidity = 2000;
-    
-    temperature -= 200;
-    if(temperature < 200) temperature = 4200;
-    
-    pressure += 300;
-    if(pressure > 10300) pressure = 7500;
-    
-    x += 1400;
-    y -= 2300;
-    z += 1700;
-}
-
 void StartHaptic(void)  {
     hapticTimer.start(50);
     haptic = 1;