Merge pull request #9 from oroca/develop

merge from develop

examples/BLE/BLE_Driver/BLE_Driver.ino

@@ -0,0 +1,17 @@
+#define EDUBOT_DRIVER_BLE
+
+#include <EduBot.h>
+
+
+EduBot edubot;
+
+
+void setup() {
+  // put your setup code here, to run once:
+
+  edubot.begin(115200);                                     
+}
+
+void loop() {
+  // put your main code here, to run repeatedly
+}

examples/IMU/FaceUpHill/FaceUpHill.ino

@@ -0,0 +1,54 @@
+#include <EduBot.h>
+
+
+EduBot edubot;
+
+
+void setup() {
+  // put your setup code here, to run once:
+  int step_len = 500;
+
+  edubot.begin(115200);                                   
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  static uint32_t pre_time; 
+
+  int x;
+  int y;
+  int magnitudeSquared;
+  int16_t turnSpeed;
+  float move_angle;
+
+  x = (int)(edubot.imu.getPitch() * 10);
+  y = (int)(edubot.imu.getRoll()  * 10);
+  magnitudeSquared = x*x + y*y;
+
+
+  move_angle = 30 * 30;
+
+  if (magnitudeSquared > (int)(move_angle))
+  {
+    turnSpeed = y / 4;
+  }
+  else
+  {
+    turnSpeed = 0;
+  }
+
+  if (millis()-pre_time >= 100)
+  {
+    pre_time = millis();
+    edubot.lcd.clearDisplay();
+
+    edubot.lcd.printf(0, 16*0, "X : %d", x);
+    edubot.lcd.printf(0, 16*1, "Y : %d", y);
+    edubot.lcd.printf(0, 16*2, "M : %d", magnitudeSquared);
+    edubot.lcd.printf(0, 16*3, "S : %d", turnSpeed);
+
+    edubot.lcd.display();
+  }   
+
+  edubot.motor.setSpeed(-turnSpeed, turnSpeed);
+}

library.properties

@@ -1,5 +1,5 @@
 name=OROCA-EduBot
-version=0.0.5
+version=0.0.6
 author=OROCA
 license=Apache-2.0
 maintainer=Baram([email protected])

src/EduBot.cpp

@@ -38,6 +38,9 @@ bool EduBot::begin(int baud)
   bool ret = false;
   bool ret_log = false;
 
+
+  driverInit();
+
   Serial.begin(baud);
   Serial.println();
   Serial.println();
@@ -107,8 +110,6 @@ bool EduBot::begin(int baud)
     ,  1);
 
 
-  //ble.begin("OROCA EduBot");  
-
   return true;
 }
 

src/EduBot.h

@@ -10,22 +10,7 @@
 
 #include <Arduino.h>
 
-
-
-#include "./driver/imu/imu.h"
-#include "./driver/stepmotor/motor.h"
-#include "./driver/stepmotor/stepmotor.h"
-#include "./driver/audio/audio.h"
-#include "./driver/oled/oled.h"
-#include "./driver/oled/oled.h"
-#include "./driver/hangul/PHan_Lib.h"
-#include "./driver/vl53l0x/VL53L0X.h"
-#include "./driver/ir_remote/ir_remote.h"
-
-#include "./driver/ble/ble.h"
-#include "./driver/neopixel/neopixel.h"
-#include "./driver/adc_info/adc_info.h"
-#include "./driver/floor/floor.h"
+#include "./driver/driver.h"
 
 
 #define EDUBOT_VER_STR            "EduBot V190201R2"
@@ -49,7 +34,9 @@ class EduBot
     // for Extention Board
     IrRemote  ir_remote;
     OLed      lcd;
+#ifdef EDUBOT_DRIVER_BLE
     BLE       ble;
+#endif
     NeoPixel  led;
     VL53L0X   tof_L;
     VL53L0X   tof_R;

src/driver/ble/ble.cpp

@@ -1,6 +1,7 @@
 
 #include "ble.h"
-#include <Arduino.h>
+
+#ifdef EDUBOT_DRIVER_BLE
 
 static bool g_deviceConnected;
 static bool g_oldDeviceConnected;
@@ -96,4 +97,7 @@ void BLE::BLEServerCallback::onConnect(BLEServer* pServer)
 void BLE::BLEServerCallback::onDisconnect(BLEServer* pServer)
 {
   g_deviceConnected = false;
-}
+}
+
+
+#endif

src/driver/ble/ble.h

@@ -1,6 +1,10 @@
 #ifndef  _DRIVER_BLE_BLE_H
 #define  _DRIVER_BLE_BLE_H
 
+#include "../def.h"
+
+#ifdef EDUBOT_DRIVER_BLE
+
 #include <BLEDevice.h>
 #include <BLEServer.h>
 #include <BLEUtils.h>
@@ -34,5 +38,6 @@ class BLE
 };
 
 
+#endif
 
 #endif  /* _DRIVER_BLE_BLE_H */

src/driver/def.h

@@ -0,0 +1,9 @@
+#ifndef _DEF_H_
+#define _DEF_H_
+
+#include <Arduino.h>
+
+
+
+
+#endif

src/driver/driver.cpp

@@ -0,0 +1,13 @@
+#include "driver.h"
+
+
+
+
+
+
+
+void driverInit(void)
+{
+
+}
+

src/driver/driver.h

@@ -0,0 +1,28 @@
+#ifndef _DRIVER_H_
+#define _DRIVER_H_
+
+#include "def.h"
+
+
+#include "./imu/imu.h"
+#include "./stepmotor/motor.h"
+#include "./stepmotor/stepmotor.h"
+#include "./audio/audio.h"
+#include "./oled/oled.h"
+#include "./oled/oled.h"
+#include "./hangul/PHan_Lib.h"
+#include "./vl53l0x/VL53L0X.h"
+#include "./ir_remote/ir_remote.h"
+
+#include "./neopixel/neopixel.h"
+#include "./adc_info/adc_info.h"
+#include "./floor/floor.h"
+
+#ifdef EDUBOT_DRIVER_BLE
+#include "./ble/ble.h"
+#endif
+
+void driverInit(void);
+
+
+#endif

src/driver/imu/imu.cpp

@@ -1,6 +1,11 @@
 #include "imu.h"
 
 
+static xSemaphoreHandle lock;
+
+
+#define IMU_MUTEX_LOCK()    do {} while (xSemaphoreTake(lock, portMAX_DELAY) != pdPASS)
+#define IMU_MUTEX_UNLOCK()  xSemaphoreGive(lock)
 
 
 ImuSensor::ImuSensor(void)
@@ -22,6 +27,14 @@ bool ImuSensor::begin(void)
 {
   inv_error_t inv_error;
 
+  if(lock == NULL) 
+  {
+    lock = xSemaphoreCreateMutex();
+    if(lock == NULL) 
+    {
+      return false;
+    }
+  }
 
   // Call imu.begin() to verify communication and initialize
   if (m_imu.begin() != INV_SUCCESS)
@@ -61,6 +74,7 @@ bool ImuSensor::update(void)
   // Check for new data in the FIFO
   if ( m_imu.fifoAvailable() )
   {
+    IMU_MUTEX_LOCK();
     // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
     if ( m_imu.dmpUpdateFifo() == INV_SUCCESS)
     {
@@ -69,7 +83,107 @@ bool ImuSensor::update(void)
       m_imu.computeEulerAngles();
       ret = true;
     }
+    IMU_MUTEX_UNLOCK();
   }  
 
   return ret;
-}
+}
+
+float ImuSensor::getRoll(void) 
+{
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.roll;
+  IMU_MUTEX_UNLOCK();
+
+  return ret;; 
+}
+
+float ImuSensor::getPitch(void) 
+{
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.pitch;
+  IMU_MUTEX_UNLOCK();
+
+  return ret; 
+}
+
+float ImuSensor::getYaw(void) 
+{ 
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.yaw;
+  IMU_MUTEX_UNLOCK();
+
+  return ret; 
+}    
+
+float ImuSensor::getAccX(void) 
+{
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.calcAccel(m_imu.ax);
+  IMU_MUTEX_UNLOCK();
+
+  return ret; 
+};
+
+float ImuSensor::getAccY(void) 
+{
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.calcAccel(m_imu.ay);
+  IMU_MUTEX_UNLOCK();
+
+  return ret; 
+};
+
+float ImuSensor::getAccZ(void) 
+{ 
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.calcAccel(m_imu.az);
+  IMU_MUTEX_UNLOCK();
+
+  return ret; 
+};
+
+float ImuSensor::getGyroX(void) 
+{ 
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.calcGyro(m_imu.gx);
+  IMU_MUTEX_UNLOCK();
+
+  return ret; 
+};
+
+float ImuSensor::getGyroY(void) 
+{ 
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.calcGyro(m_imu.gy);
+  IMU_MUTEX_UNLOCK();
+
+  return ret;   
+};
+
+float ImuSensor::getGyroZ(void) 
+{ 
+  float ret = 0.0;
+
+  IMU_MUTEX_LOCK();
+  ret = m_imu.calcGyro(m_imu.gz);
+  IMU_MUTEX_UNLOCK();
+
+  return ret;   
+};