diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index ce7b3f1..1ddd2e2 100644 --- a/examples/firmware/firmware.ino +++ b/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(); diff --git a/library.properties b/library.properties index 6b1cf07..b221ab0 100644 --- a/library.properties +++ b/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 sentence=Library and firmware for Arduino Alvik Carrier board diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index a29c924..ac7cc05 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/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(); +} + /******************************************************************************************************/ diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 3da1dbf..2388075 100644 --- 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,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 diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index 0562bf5..bede86c 100644 --- a/src/definitions/robot_definitions.h +++ b/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