added shaking PoC by isShaking() method

arduino-libraries · Jul 12, 2024 · 771bfb8 · 771bfb8
1 parent 1dc4029
commit 771bfb8
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 0 deletions.
diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp
@@ -67,6 +67,10 @@ 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;
 
     // version
     version_high = VERSION_BYTE_HIGH;
@@ -619,6 +623,9 @@ 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);
 
     delay(10);
 
@@ -664,6 +671,33 @@ 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;
+    }
+
+
+
+
 }
 
 float Arduino_AlvikCarrier::getAccelerationX(){
@@ -702,6 +736,10 @@ float Arduino_AlvikCarrier::getYaw(){
     return 360.0-filter_data.rotation[0];
 }
 
+bool Arduino_AlvikCarrier::isShaking(){
+    return is_shaking;
+}
+
 
 
 /******************************************************************************************************/

diff --git a/src/Arduino_AlvikCarrier.h b/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,10 @@ 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 version_high;
         uint8_t version_mid;
@@ -220,6 +226,7 @@ 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                                                        
 
         void errorLed(const int error_code);                                            // error routine, locks on code blinking led