Merge pull request #22 from arduino-libraries/gbr1-exp

added tilt and shake

Author: gbr1

examples/firmware/firmware.ino

@@ -227,6 +227,8 @@ void loop(){
         alvik.updateTouch();
         msg_size = packeter.packetC1B('t', alvik.getTouchKeys());
         alvik.serial->write(packeter.msg,msg_size);
+        msg_size = packeter.packetC1B('m', alvik.getMotion());
+        alvik.serial->write(packeter.msg,msg_size);
         break;
       case 2:
         alvik.updateAPDS();

library.properties

@@ -1,5 +1,5 @@
 name=Arduino_AlvikCarrier
-version=1.0.1
+version=1.0.2
 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
 maintainer=Arduino <[email protected]>
 sentence=Library and firmware for Arduino Alvik Carrier board

src/Arduino_AlvikCarrier.cpp

@@ -67,6 +67,20 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
     ipKnobs = &iKnobs;
     imu_delta_time = MOTION_FX_ENGINE_DELTATIME;
     sample_to_discard = 0;
+    is_shaking = 0;
+    first_wakeup = true;
+    shake_time = 0;
+    shake_counter = 0;
+    tilt_status = 0x80;
+    xl = 0;
+    xh = 0;
+    yl = 0;
+    yh = 0;
+    zl = 0;
+    zh = 0;
+    tilt_time = 0;
+    tmp_tilt_status = 0;
+    tilt_filter = 0;
 
     // version
     version_high = VERSION_BYTE_HIGH;
@@ -619,6 +633,11 @@ int Arduino_AlvikCarrier::beginImu(){
     imu->Set_G_FS(2000);
     imu->Enable_X();
     imu->Enable_G();
+    imu->Enable_Wake_Up_Detection(LSM6DSO_INT1_PIN);
+    imu->Set_Wake_Up_Threshold(1);
+    imu->Set_Wake_Up_Duration(3);
+    imu->Enable_6D_Orientation(LSM6DSO_INT1_PIN);
+
 
     delay(10);
 
@@ -664,6 +683,60 @@ void Arduino_AlvikCarrier::updateImu(){
         sample_to_discard++;
     }
 
+    imu->Get_X_Event_Status(&imu_status);
+ 
+    if (imu_status.WakeUpStatus && 
+            (motor_control_left->getRPM()<1 && motor_control_left->getRPM()>-1) &&
+            (motor_control_right->getRPM()<1 && motor_control_right->getRPM()>-1)){
+        if (first_wakeup){
+            shake_time = millis();
+            first_wakeup = false;
+            shake_counter = 0;
+        }
+        shake_counter++;
+    }
+
+    if (millis()-shake_time>500){
+        if (shake_counter>10){
+            is_shaking = true;
+            shake_counter = 0;
+            shake_time_sig = millis();
+        }
+    }
+    if (is_shaking && (millis()-shake_time_sig>1000)){
+        is_shaking = false;
+        tilt_time = millis();
+    }
+
+    if ((!is_shaking) && (millis()-tilt_time>1000)){
+        imu->Get_6D_Orientation_XL(&xl);
+        imu->Get_6D_Orientation_XH(&xh);
+        imu->Get_6D_Orientation_YL(&yl);
+        imu->Get_6D_Orientation_YH(&yh);
+        imu->Get_6D_Orientation_ZL(&zl);
+        imu->Get_6D_Orientation_ZH(&zh);
+        
+        tmp_tilt_status = 0;
+        tmp_tilt_status |= xl<<4;
+        tmp_tilt_status |= xh<<5;
+        tmp_tilt_status |= zl<<7;
+        tmp_tilt_status |= zh<<6;
+        tmp_tilt_status |= yh<<3;
+        tmp_tilt_status |= yl<<2;
+
+        if (tilt_status !=  tmp_tilt_status){
+            tilt_filter++;
+        }else{
+            tilt_filter = 0;
+        }
+
+        if (tilt_filter>20){
+            tilt_status = tmp_tilt_status;
+            tilt_filter = 0;
+        }
+
+    }
+
 }
 
 float Arduino_AlvikCarrier::getAccelerationX(){
@@ -702,6 +775,14 @@ float Arduino_AlvikCarrier::getYaw(){
     return 360.0-filter_data.rotation[0];
 }
 
+bool Arduino_AlvikCarrier::isShaking(){
+    return is_shaking;
+}
+
+uint8_t Arduino_AlvikCarrier::getMotion(){
+    return tilt_status | isShaking();
+}
+
 
 
 /******************************************************************************************************/

src/Arduino_AlvikCarrier.h

@@ -56,6 +56,8 @@ class Arduino_AlvikCarrier{
         LSM6DSOSensor * imu;
         int32_t accelerometer[3];
         int32_t gyroscope[3];
+        LSM6DSO_Event_Status_t imu_status;
+
 
         float imu_delta_time;
         MFX_knobs_t iKnobs;
@@ -64,6 +66,15 @@ class Arduino_AlvikCarrier{
         MFX_input_t imu_data;
         MFX_output_t filter_data;
         uint16_t sample_to_discard;
+        bool is_shaking;
+        bool first_wakeup;
+        unsigned long shake_time, shake_time_sig;
+        uint16_t shake_counter;
+        uint8_t tilt_status;
+        uint8_t xl, xh, yl, yh, zl, zh;
+        unsigned long tilt_time;
+        uint8_t tilt_filter;
+        uint8_t tmp_tilt_status;
 
         uint8_t version_high;
         uint8_t version_mid;
@@ -220,6 +231,8 @@ class Arduino_AlvikCarrier{
         float getRoll();                                                                // get robot roll
         float getPitch();                                                               // get robot pitch
         float getYaw();                                                                 // get robot yaw
+        bool  isShaking();                                                              // get true if robot is shaking    
+        uint8_t getMotion();                                                            // get tilt + shaking                                                    
 
         void errorLed(const int error_code);                                            // error routine, locks on code blinking led
 

src/definitions/robot_definitions.h

@@ -88,7 +88,7 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 // Library version
 #define VERSION_BYTE_HIGH 1
 #define VERSION_BYTE_MID 0
-#define VERSION_BYTE_LOW 1
+#define VERSION_BYTE_LOW 2