From 771bfb89c0e1629c54ab098b227f1215546ba3d9 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 12 Jul 2024 16:15:05 +0200 Subject: [PATCH 01/23] added shaking PoC by isShaking() method --- src/Arduino_AlvikCarrier.cpp | 38 ++++++++++++++++++++++++++++++++++++ src/Arduino_AlvikCarrier.h | 7 +++++++ 2 files changed, 45 insertions(+) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index a29c924..9bb8865 100644 --- 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 index 3da1dbf..a3dfcb0 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,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 From f1e98205bf17d2481f7850d0047a87252c3821b5 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 12 Jul 2024 16:27:11 +0200 Subject: [PATCH 02/23] hyp firmware update to get shaking ('d' LSB) --- examples/firmware/firmware.ino | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index ce7b3f1..2dacf98 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('d', alvik.isShaking()); + alvik.serial->write(packeter.msg,msg_size); break; case 2: alvik.updateAPDS(); From dc6c521fd4283bb6e47a506efde9c5a32d8b486a Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Thu, 25 Jul 2024 16:08:29 +0200 Subject: [PATCH 03/23] motion message is now with code 'm' --- examples/firmware/firmware.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index 2dacf98..bafb51e 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -227,7 +227,7 @@ void loop(){ alvik.updateTouch(); msg_size = packeter.packetC1B('t', alvik.getTouchKeys()); alvik.serial->write(packeter.msg,msg_size); - msg_size = packeter.packetC1B('d', alvik.isShaking()); + msg_size = packeter.packetC1B('m', alvik.isShaking()); alvik.serial->write(packeter.msg,msg_size); break; case 2: From b7919cd629dad26a2662647a418359f43759497d Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 26 Jul 2024 12:39:18 +0200 Subject: [PATCH 04/23] preliminary tilt --- examples/firmware/firmware.ino | 2 +- src/Arduino_AlvikCarrier.cpp | 28 ++++++++++++++++++++++++++-- src/Arduino_AlvikCarrier.h | 5 ++++- 3 files changed, 31 insertions(+), 4 deletions(-) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index bafb51e..1ddd2e2 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -227,7 +227,7 @@ void loop(){ alvik.updateTouch(); msg_size = packeter.packetC1B('t', alvik.getTouchKeys()); alvik.serial->write(packeter.msg,msg_size); - msg_size = packeter.packetC1B('m', alvik.isShaking()); + msg_size = packeter.packetC1B('m', alvik.getMotion()); alvik.serial->write(packeter.msg,msg_size); break; case 2: diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 9bb8865..bf48731 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -71,6 +71,13 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ first_wakeup = true; shake_time = 0; shake_counter = 0; + tilt_status = 0; + xl = 0; + xh = 0; + yl = 0; + yh = 0; + zl = 0; + zh = 0; // version version_high = VERSION_BYTE_HIGH; @@ -626,6 +633,8 @@ int Arduino_AlvikCarrier::beginImu(){ 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); @@ -695,9 +704,20 @@ void Arduino_AlvikCarrier::updateImu(){ is_shaking = false; } - + 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); - + tilt_status = 0; + tilt_status |= xl<<4; + tilt_status |= xh<<5; + tilt_status |= zl<<7; + tilt_status |= zh<<6; + tilt_status |= yh<<3; + tilt_status |= yl<<2; } float Arduino_AlvikCarrier::getAccelerationX(){ @@ -740,6 +760,10 @@ 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 a3dfcb0..b56b931 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -70,6 +70,8 @@ class Arduino_AlvikCarrier{ 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; uint8_t version_high; uint8_t version_mid; @@ -226,7 +228,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 + 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 From 0936fca33306f533aadabfb9d5518c2783984cd7 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jul 2024 12:39:14 +0200 Subject: [PATCH 05/23] removed tilt on shaking --- src/Arduino_AlvikCarrier.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index bf48731..65f51db 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -761,7 +761,12 @@ bool Arduino_AlvikCarrier::isShaking(){ } uint8_t Arduino_AlvikCarrier::getMotion(){ - return tilt_status | isShaking(); + if (isShaking){ + return 0x01; + } + else{ + return tilt_status; + } } From 9647c2770530feaada7f2d3eef5489a135c896eb Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jul 2024 12:41:18 +0200 Subject: [PATCH 06/23] fix --- src/Arduino_AlvikCarrier.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 65f51db..51ef68e 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -761,7 +761,7 @@ bool Arduino_AlvikCarrier::isShaking(){ } uint8_t Arduino_AlvikCarrier::getMotion(){ - if (isShaking){ + if (isShaking()){ return 0x01; } else{ From 808614f90ba8f5fc1c9f2887bd519ab19e4116fd Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jul 2024 12:53:26 +0200 Subject: [PATCH 07/23] not update tilt when shaking --- src/Arduino_AlvikCarrier.cpp | 38 +++++++++++++++++------------------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 51ef68e..329e3e6 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -704,20 +704,23 @@ void Arduino_AlvikCarrier::updateImu(){ is_shaking = false; } - 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); - - tilt_status = 0; - tilt_status |= xl<<4; - tilt_status |= xh<<5; - tilt_status |= zl<<7; - tilt_status |= zh<<6; - tilt_status |= yh<<3; - tilt_status |= yl<<2; + if (!is_shaking){ + 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); + + tilt_status = 0; + tilt_status |= xl<<4; + tilt_status |= xh<<5; + tilt_status |= zl<<7; + tilt_status |= zh<<6; + tilt_status |= yh<<3; + tilt_status |= yl<<2; + } + } float Arduino_AlvikCarrier::getAccelerationX(){ @@ -761,12 +764,7 @@ bool Arduino_AlvikCarrier::isShaking(){ } uint8_t Arduino_AlvikCarrier::getMotion(){ - if (isShaking()){ - return 0x01; - } - else{ - return tilt_status; - } + return tilt_status | isShaking(); } From a36cb24f651e3cb72458d45c2110df3fb2d8e383 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jul 2024 13:10:50 +0200 Subject: [PATCH 08/23] added retard on tilt after shake --- src/Arduino_AlvikCarrier.cpp | 4 +++- src/Arduino_AlvikCarrier.h | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 329e3e6..6de3910 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -78,6 +78,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ yh = 0; zl = 0; zh = 0; + tilt_time = 0; // version version_high = VERSION_BYTE_HIGH; @@ -702,9 +703,10 @@ void Arduino_AlvikCarrier::updateImu(){ } if (is_shaking && (millis()-shake_time_sig>1000)){ is_shaking = false; + tilt_time = millis(); } - if (!is_shaking){ + 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); diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index b56b931..421f771 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -72,6 +72,7 @@ class Arduino_AlvikCarrier{ uint16_t shake_counter; uint8_t tilt_status; uint8_t xl, xh, yl, yh, zl, zh; + unsigned long tilt_time; uint8_t version_high; uint8_t version_mid; From e97923d8efd7f29a8e39d392e222415df7ead309 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jul 2024 14:44:16 +0200 Subject: [PATCH 09/23] added filter --- src/Arduino_AlvikCarrier.cpp | 27 ++++++++++++++++++++------- src/Arduino_AlvikCarrier.h | 2 ++ 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 6de3910..6eaffa4 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -79,6 +79,8 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ zl = 0; zh = 0; tilt_time = 0; + tmp_tilt_status = 0; + tilt_filter = 0; // version version_high = VERSION_BYTE_HIGH; @@ -714,13 +716,24 @@ void Arduino_AlvikCarrier::updateImu(){ imu->Get_6D_Orientation_ZL(&zl); imu->Get_6D_Orientation_ZH(&zh); - tilt_status = 0; - tilt_status |= xl<<4; - tilt_status |= xh<<5; - tilt_status |= zl<<7; - tilt_status |= zh<<6; - tilt_status |= yh<<3; - tilt_status |= yl<<2; + 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; + } + } } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 421f771..2388075 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -73,6 +73,8 @@ class Arduino_AlvikCarrier{ 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; From ae94421dae85c5ef5fbaf55968eb1114ab3cbd92 Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Tue, 30 Jul 2024 15:18:54 +0200 Subject: [PATCH 10/23] fix: not sending tilts --- src/Arduino_AlvikCarrier.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 6eaffa4..ac7cc05 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -71,7 +71,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ first_wakeup = true; shake_time = 0; shake_counter = 0; - tilt_status = 0; + tilt_status = 0x80; xl = 0; xh = 0; yl = 0; @@ -724,7 +724,7 @@ void Arduino_AlvikCarrier::updateImu(){ tmp_tilt_status |= yh<<3; tmp_tilt_status |= yl<<2; - if (tilt_status == tmp_tilt_status){ + if (tilt_status != tmp_tilt_status){ tilt_filter++; }else{ tilt_filter = 0; @@ -732,6 +732,7 @@ void Arduino_AlvikCarrier::updateImu(){ if (tilt_filter>20){ tilt_status = tmp_tilt_status; + tilt_filter = 0; } } From fa2a4ddb4a5e54d63c0a23fde78cbc4172e95c42 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jul 2024 12:14:10 +0200 Subject: [PATCH 11/23] 1.0.2 --- library.properties | 2 +- src/definitions/robot_definitions.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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/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 From d51412bbea2488d36020fcd300f0bf327448d094 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Mon, 12 Aug 2024 11:30:59 +0200 Subject: [PATCH 12/23] isPositionReached in motor_control class --- src/definitions/robot_definitions.h | 1 + src/motor_control/motor_control.cpp | 10 ++++++++++ src/motor_control/motor_control.h | 2 ++ 3 files changed, 13 insertions(+) diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index bede86c..d03010a 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -37,6 +37,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO; #define POSITION_KD_DEFAULT 0.0001 #define POSITION_CONTROL_PERIOD 0.02 #define POSITION_MAX_SPEED 30.0 +#define POSITION_THRESHOLD 2.0 diff --git a/src/motor_control/motor_control.cpp b/src/motor_control/motor_control.cpp index 13aa831..843061f 100644 --- a/src/motor_control/motor_control.cpp +++ b/src/motor_control/motor_control.cpp @@ -35,6 +35,7 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp pos_controller_period = _pos_controller_period; pos_max_velocity = _pos_max_velocity; position_control_enabled = false; + is_position_reached = false; @@ -168,6 +169,9 @@ void MotorControl::update(){ if (position_control_enabled){ pos_pid->update(position); setRPM(round(pos_pid->getControlOutput()/10.0)*10); + if (abs(pos_pid->getError())setReference(degree); enablePositionControl(); + is_position_reached = false; +} + +bool MotorControl::isPositionReached(){ + return is_position_reached; } diff --git a/src/motor_control/motor_control.h b/src/motor_control/motor_control.h index e3ffa5a..3adffb4 100644 --- a/src/motor_control/motor_control.h +++ b/src/motor_control/motor_control.h @@ -43,6 +43,7 @@ class MotorControl{ float pos_controller_period; float pos_max_velocity; bool position_control_enabled; + bool is_position_reached; float position; float angle; @@ -112,6 +113,7 @@ class MotorControl{ void setPosition(const float degree); // set the reference for position control float getPosition(); // get the actual angle in degrees of motor void resetPosition(const float p0=0.0); // reset/set the position value + bool isPositionReached(); float getError(); From e5c6ec1bcd2c47c09a5e3496d6aa8b92f8d211b6 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Mon, 12 Aug 2024 12:15:46 +0200 Subject: [PATCH 13/23] isPositionReached() is now exposed in AlvikCarrier class --- src/Arduino_AlvikCarrier.cpp | 12 ++++++++++++ src/Arduino_AlvikCarrier.h | 3 +++ src/definitions/robot_definitions.h | 3 +++ 3 files changed, 18 insertions(+) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index ac7cc05..139089a 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -362,6 +362,10 @@ float Arduino_AlvikCarrier::getPositionLeft(){ return motor_control_left->getPosition(); } +bool Arduino_AlvikCarrier::isPositionLeftReached(){ + return motor_control_left->isPositionReached(); +} + void Arduino_AlvikCarrier::setPositionRight(const float degrees){ motor_control_right->setPosition(degrees); } @@ -370,6 +374,10 @@ float Arduino_AlvikCarrier::getPositionRight(){ return motor_control_right->getPosition(); } +bool Arduino_AlvikCarrier::isPositionRightReached(){ + return motor_control_right->isPositionReached(); +} + void Arduino_AlvikCarrier::setPosition(const float left_deg, const float right_deg){ setPositionLeft(left_deg); setPositionRight(right_deg); @@ -380,6 +388,10 @@ void Arduino_AlvikCarrier::getPosition(float & left_deg, float & right_deg){ right_deg = getPositionRight(); } +bool Arduino_AlvikCarrier::isPositionReached(){ + return isPositionLeftReached() && isPositionRightReached(); +} + void Arduino_AlvikCarrier::resetPositionLeft(const float initial_position){ motor_control_left->resetPosition(initial_position); } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 2388075..9d683aa 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -172,10 +172,13 @@ class Arduino_AlvikCarrier{ void setKPidRight(const float kp, const float ki, const float kd); // set PID parameters for right wheel void setPositionLeft(const float degrees); // set position in degrees on left wheel float getPositionLeft(); // get left wheel position in degrees + bool isPositionLeftReached(); // return true if left wheel position is reached void setPositionRight(const float degrees); // set position in degrees on right wheel float getPositionRight(); // get right wheel position in degrees + bool isPositionRightReached(); // return true if right wheel position is reached void setPosition(const float left_deg, const float right_deg); // set positions on both wheels void getPosition(float & left_deg, float & right_deg); // get both wheels position + bool isPositionReached(); // return true if both wheels position are reached void resetPositionLeft(const float initial_position=0.0); // reset/set value of position for left wheel void resetPositionRight(const float initial_position=0.0); // reset/set value of position for right wheel void disablePositionControlLeft(); // disable the position control on left wheel diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index d03010a..31415de 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -59,6 +59,9 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO; #define MOVEMENT_DISABLED 0 #define MOVEMENT_ROTATE 1 #define MOVEMENT_MOVE 2 +#define MOVEMENT_LEFT 3 +#define MOVEMENT_RIGHT 4 +#define MOVEMENT_POSITION 5 #define LIFT_THRESHOLD 80 From 22f29d9f9d3f035c3746ed65c6b28dec91454be3 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Mon, 12 Aug 2024 12:29:49 +0200 Subject: [PATCH 14/23] updated firmware --- examples/firmware/firmware.ino | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index 1ddd2e2..df7ffa5 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -122,6 +122,8 @@ void loop(){ break; case 'P': alvik.setPositionLeft(value); + ack_required=MOVEMENT_LEFT; + ack_check=true; break; case 'Z': alvik.resetPositionLeft(value); @@ -136,6 +138,8 @@ void loop(){ break; case 'P': alvik.setPositionRight(value); + ack_required=MOVEMENT_RIGHT; + ack_check=true; break; case 'Z': alvik.resetPositionRight(value); @@ -149,6 +153,8 @@ void loop(){ packeter.unpacketC2F(code,position_left, position_right); alvik.disableKinematicsMovement(); alvik.setPosition(position_left, position_right); + ack_required=MOVEMENT_POSITION; + ack_check=true; break; @@ -280,13 +286,22 @@ void loop(){ msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]); alvik.serial->write(packeter.msg,msg_size); } - if (ack_check && alvik.isTargetReached()){ + if (ack_check && (alvik.isTargetReached() || alvik.isPositionReached() || alvik.isPositionLeftReached() || alvik.isPositionRightReached())){ if (ack_required == MOVEMENT_ROTATE){ msg_size = packeter.packetC1B('x', 'R'); } if (ack_required == MOVEMENT_MOVE){ msg_size = packeter.packetC1B('x', 'M'); } + if (ack_required == MOVEMENT_POSITION){ + msg_size = packeter.packetC1B('x', 'P'); + } + if (ack_required == MOVEMENT_LEFT){ + msg_size = packeter.packetC1B('x', 'P'); + } + if (ack_required == MOVEMENT_RIGHT){ + msg_size = packeter.packetC1B('x', 'P'); + } } else{ msg_size = packeter.packetC1B('x', 0); From 987a89829e579a0291839534532ffb3adce44a14 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Mon, 12 Aug 2024 12:43:28 +0200 Subject: [PATCH 15/23] threshold on position 10 --- src/definitions/robot_definitions.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index 31415de..5c5c587 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -37,7 +37,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO; #define POSITION_KD_DEFAULT 0.0001 #define POSITION_CONTROL_PERIOD 0.02 #define POSITION_MAX_SPEED 30.0 -#define POSITION_THRESHOLD 2.0 +#define POSITION_THRESHOLD 10 From 6c1baf1cbaa97ffa28098e69c890387534c3e493 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Mon, 12 Aug 2024 12:51:46 +0200 Subject: [PATCH 16/23] version update --- library.properties | 2 +- src/definitions/robot_definitions.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/library.properties b/library.properties index b221ab0..a03415b 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Arduino_AlvikCarrier -version=1.0.2 +version=1.0.3 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi maintainer=Arduino sentence=Library and firmware for Arduino Alvik Carrier board diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index 5c5c587..54540ab 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -92,7 +92,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 2 +#define VERSION_BYTE_LOW 3 From a7a796fff07126f91aa644c83c38ddae1cba7cf8 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 8 Oct 2024 10:35:26 +0200 Subject: [PATCH 17/23] fixed bms check --- src/Arduino_AlvikCarrier.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 139089a..49c05f5 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -137,7 +137,7 @@ int Arduino_AlvikCarrier::begin(){ beginServo(); - if (beginBMS()!=0){ + if (!beginBMS()){ errorLed(ERROR_BMS); } @@ -270,8 +270,7 @@ void Arduino_AlvikCarrier::disconnectExternalI2C(){ int Arduino_AlvikCarrier::beginBMS(){ while(digitalRead(NANO_CHK)==HIGH){} - bms->begin(); - return 0; + return bms->begin(); } void Arduino_AlvikCarrier::updateBMS(){ From 397912969cc869846cc71e92c56a90da78aa82a9 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 9 Oct 2024 09:53:03 +0200 Subject: [PATCH 18/23] fix behaviour --- examples/firmware/firmware.ino | 3 ++ src/Arduino_AlvikCarrier.cpp | 45 ++++++++++++----------------- src/definitions/robot_definitions.h | 1 + 3 files changed, 22 insertions(+), 27 deletions(-) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index df7ffa5..ec56275 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -213,6 +213,9 @@ void loop(){ case 1: alvik.setBehaviour(LIFT_ILLUMINATOR, true); break; + case 2: + alvik.setBehaviour(BATTERY_ALERT, true); + break; default: alvik.setBehaviour(ALL_BEHAVIOURS, false); } diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 49c05f5..2e2665f 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -973,39 +973,30 @@ void Arduino_AlvikCarrier::beginBehaviours(){ void Arduino_AlvikCarrier::updateBehaviours(){ - if (behaviours|=1 == 1){ - /* + if ((1<<(LIFT_ILLUMINATOR-1)) & behaviours){ + if (isLifted()&&first_lift){ - first_lift = false; - prev_illuminator_state = illuminator_state; - disableIlluminator(); + //disableIlluminator(); + setIlluminator(LOW); + first_lift=false; } - if (isLifted()&&!first_lift) { - if (prev_illuminator_state!=0){ - disableIlluminator(); + else{ + if (!isLifted()){ + setIlluminator(prev_illuminator_state); } - } - if (!isLifted()&&!first_lift){ - if (prev_illuminator_state!=0){ - //first_lift = true; - enableIlluminator(); + if (!isLifted()&&!first_lift){ + first_lift = true; } } - */ - if (isLifted()&&first_lift){ - //disableIlluminator(); - setIlluminator(LOW); - first_lift=false; - } - else{ - if (!isLifted()){ - setIlluminator(prev_illuminator_state); - } - if (!isLifted()&&!first_lift){ - first_lift = true; - } - } } + + if ((1 << (BATTERY_ALERT-1)) & behaviours){ + led1->setRed(true); + } + else{ + led1->setRed(false); + } + } void Arduino_AlvikCarrier::setBehaviour(const uint8_t behaviour, const bool enable){ diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index 54540ab..33508eb 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -68,6 +68,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO; #define ALL_BEHAVIOURS 255 #define LIFT_ILLUMINATOR 1 +#define BATTERY_ALERT 2 From ac54a92aefb3e9fdad853120d92bd2bb4e6e47fd Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 9 Oct 2024 10:55:02 +0200 Subject: [PATCH 19/23] battery alert behaviour --- src/Arduino_AlvikCarrier.cpp | 27 ++++++++++++++++++++++----- src/Arduino_AlvikCarrier.h | 2 ++ src/definitions/robot_definitions.h | 3 +++ 3 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 2e2665f..358857d 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -969,10 +969,14 @@ void Arduino_AlvikCarrier::beginBehaviours(){ prev_illuminator_state = illuminator_state; behaviours = 0; first_lift = true; + battery_alert_time = millis(); + battery_alert_wave = 100; } void Arduino_AlvikCarrier::updateBehaviours(){ + + // illuminator off on lift if ((1<<(LIFT_ILLUMINATOR-1)) & behaviours){ if (isLifted()&&first_lift){ @@ -990,11 +994,24 @@ void Arduino_AlvikCarrier::updateBehaviours(){ } } - if ((1 << (BATTERY_ALERT-1)) & behaviours){ - led1->setRed(true); - } - else{ - led1->setRed(false); + // battery alert + if ((1<<(BATTERY_ALERT-1)) & behaviours){ + if (getBatteryVoltage()battery_alert_wave){ + battery_alert_time = millis(); + if (battery_alert_wave==100){ + setAllLeds(COLOR_RED); + battery_alert_wave=400; + } + else{ + setAllLeds(COLOR_BLACK); + battery_alert_wave=100; + } + setRpm(0,0); + motor_left->stop(); + motor_right->stop(); + } + } } } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 9d683aa..43bf766 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -94,6 +94,8 @@ class Arduino_AlvikCarrier{ bool prev_illuminator_state; uint8_t behaviours; bool first_lift; + unsigned long battery_alert_time; + unsigned long battery_alert_wave; diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index 33508eb..78aea4a 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -95,6 +95,9 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ); #define VERSION_BYTE_MID 0 #define VERSION_BYTE_LOW 3 +// Battery stats +#define BATTERY_ALERT_MINIMUM_VOLTAGE 3.4 + #endif \ No newline at end of file From bfb9cf7601f0766c61e2018139288c8fcbe77664 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 9 Oct 2024 12:54:37 +0200 Subject: [PATCH 20/23] battery percentage is negative if discharging --- examples/firmware/firmware.ino | 2 +- src/Arduino_AlvikCarrier.cpp | 23 +++++++++++++++++------ src/Arduino_AlvikCarrier.h | 3 ++- src/definitions/robot_definitions.h | 2 +- 4 files changed, 21 insertions(+), 9 deletions(-) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index ec56275..66843a8 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -329,7 +329,7 @@ void loop(){ if (millis()-tbattery>1000){ tbattery = millis(); alvik.updateBMS(); - msg_size = packeter.packetC1F('p', alvik.getBatteryChargePercentage()); + msg_size = packeter.packetC1F('p', alvik.isBatteryCharging()*alvik.getBatteryChargePercentage()); alvik.serial->write(packeter.msg,msg_size); } } diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 358857d..71d5e13 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -269,6 +269,7 @@ void Arduino_AlvikCarrier::disconnectExternalI2C(){ /******************************************************************************************************/ int Arduino_AlvikCarrier::beginBMS(){ + charging = 0.0; while(digitalRead(NANO_CHK)==HIGH){} return bms->begin(); } @@ -276,6 +277,12 @@ int Arduino_AlvikCarrier::beginBMS(){ void Arduino_AlvikCarrier::updateBMS(){ voltage = bms->readVCell(); state_of_charge = bms->readSoc(); + if (bms->isCharging()){ + charging = 1.0; + } + else{ + charging = -1.0; + } } @@ -287,6 +294,10 @@ float Arduino_AlvikCarrier::getBatteryChargePercentage(){ return state_of_charge; } +float Arduino_AlvikCarrier::isBatteryCharging(){ + return charging; +} + /******************************************************************************************************/ @@ -997,15 +1008,15 @@ void Arduino_AlvikCarrier::updateBehaviours(){ // battery alert if ((1<<(BATTERY_ALERT-1)) & behaviours){ if (getBatteryVoltage()battery_alert_wave){ + if (millis()-battery_alert_time>battery_alert_wave){ battery_alert_time = millis(); - if (battery_alert_wave==100){ - setAllLeds(COLOR_RED); - battery_alert_wave=400; + if (battery_alert_wave==400){ + setLeds(COLOR_RED); + battery_alert_wave=100; } else{ - setAllLeds(COLOR_BLACK); - battery_alert_wave=100; + setLeds(COLOR_BLACK); + battery_alert_wave=400; } setRpm(0,0); motor_left->stop(); diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 43bf766..a1d50c9 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -45,7 +45,7 @@ class Arduino_AlvikCarrier{ MAX17332 * bms; - float voltage, state_of_charge; + float voltage, state_of_charge, charging; AT42QT2120 * touch_sensor; @@ -158,6 +158,7 @@ class Arduino_AlvikCarrier{ void updateBMS(); // update the BMS float getBatteryVoltage(); // get Voltage float getBatteryChargePercentage(); // get battery percentage + float isBatteryCharging(); // return -1 if battery discharge or 1 if charging diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index 78aea4a..a3b8c6d 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -96,7 +96,7 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ); #define VERSION_BYTE_LOW 3 // Battery stats -#define BATTERY_ALERT_MINIMUM_VOLTAGE 3.4 +#define BATTERY_ALERT_MINIMUM_VOLTAGE 4.0 From 5a938c7720436670063878ef0cfed07e765fb195 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 9 Oct 2024 15:49:16 +0200 Subject: [PATCH 21/23] battery alert --- examples/firmware/firmware.ino | 207 ++++++++++++++-------------- src/Arduino_AlvikCarrier.cpp | 19 ++- src/Arduino_AlvikCarrier.h | 3 +- src/definitions/robot_definitions.h | 3 +- 4 files changed, 125 insertions(+), 107 deletions(-) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index 66843a8..cb998a1 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -96,110 +96,113 @@ void loop(){ } if (packeter.checkPayload()) { code = packeter.payloadTop(); - switch (code){ - case 'J': - packeter.unpacketC2F(code,left,right); - alvik.disableKinematicsMovement(); - alvik.disablePositionControl(); - alvik.setRpm(left, right); - break; - - case 'V': - packeter.unpacketC2F(code,linear,angular); - alvik.disableKinematicsMovement(); - alvik.disablePositionControl(); - alvik.drive(linear,angular); - break; - - case 'W': - packeter.unpacketC2B1F(code,label,control_type,value); - alvik.disableKinematicsMovement(); - if (label=='L'){ - switch (control_type){ - case 'V': - alvik.disablePositionControlLeft(); - alvik.setRpmLeft(value); - break; - case 'P': - alvik.setPositionLeft(value); - ack_required=MOVEMENT_LEFT; - ack_check=true; - break; - case 'Z': - alvik.resetPositionLeft(value); - break; + if (!alvik.isBatteryAlert()){ + switch (code){ + case 'J': + packeter.unpacketC2F(code,left,right); + alvik.disableKinematicsMovement(); + alvik.disablePositionControl(); + alvik.setRpm(left, right); + break; + + case 'V': + packeter.unpacketC2F(code,linear,angular); + alvik.disableKinematicsMovement(); + alvik.disablePositionControl(); + alvik.drive(linear,angular); + break; + + case 'W': + packeter.unpacketC2B1F(code,label,control_type,value); + alvik.disableKinematicsMovement(); + if (label=='L'){ + switch (control_type){ + case 'V': + alvik.disablePositionControlLeft(); + alvik.setRpmLeft(value); + break; + case 'P': + alvik.setPositionLeft(value); + ack_required=MOVEMENT_LEFT; + ack_check=true; + break; + case 'Z': + alvik.resetPositionLeft(value); + break; + } } - } - if (label=='R'){ - switch (control_type){ - case 'V': - alvik.disablePositionControlRight(); - alvik.setRpmRight(value); - break; - case 'P': - alvik.setPositionRight(value); - ack_required=MOVEMENT_RIGHT; - ack_check=true; - break; - case 'Z': - alvik.resetPositionRight(value); - break; + if (label=='R'){ + switch (control_type){ + case 'V': + alvik.disablePositionControlRight(); + alvik.setRpmRight(value); + break; + case 'P': + alvik.setPositionRight(value); + ack_required=MOVEMENT_RIGHT; + ack_check=true; + break; + case 'Z': + alvik.resetPositionRight(value); + break; + } } - } - break; - - - case 'A': - packeter.unpacketC2F(code,position_left, position_right); - alvik.disableKinematicsMovement(); - alvik.setPosition(position_left, position_right); - ack_required=MOVEMENT_POSITION; - ack_check=true; - break; - - - case 'S': - packeter.unpacketC2B(code,servo_A,servo_B); - alvik.setServoA(servo_A); - alvik.setServoB(servo_B); - break; - - case 'L': - packeter.unpacketC1B(code,leds); - alvik.setAllLeds(leds); - break; - - case 'P': - packeter.unpacketC1B3F(code,pid,kp,ki,kd); - if (pid=='L'){ - alvik.setKPidLeft(kp,ki,kd); - } - if (pid=='R'){ - alvik.setKPidRight(kp,ki,kd); - } - break; - - case 'R': - packeter.unpacketC1F(code, value); - alvik.disablePositionControl(); - alvik.rotate(value); - ack_required=MOVEMENT_ROTATE; - ack_check=true; - break; - - case 'G': - packeter.unpacketC1F(code, value); - alvik.disablePositionControl(); - alvik.move(value); - ack_required=MOVEMENT_MOVE; - ack_check=true; - break; - - case 'Z': - packeter.unpacketC3F(code, x, y, theta); - alvik.resetPose(x, y, theta); - break; - + break; + + + case 'A': + packeter.unpacketC2F(code,position_left, position_right); + alvik.disableKinematicsMovement(); + alvik.setPosition(position_left, position_right); + ack_required=MOVEMENT_POSITION; + ack_check=true; + break; + + + case 'S': + packeter.unpacketC2B(code,servo_A,servo_B); + alvik.setServoA(servo_A); + alvik.setServoB(servo_B); + break; + + case 'L': + packeter.unpacketC1B(code,leds); + alvik.setAllLeds(leds); + break; + + case 'P': + packeter.unpacketC1B3F(code,pid,kp,ki,kd); + if (pid=='L'){ + alvik.setKPidLeft(kp,ki,kd); + } + if (pid=='R'){ + alvik.setKPidRight(kp,ki,kd); + } + break; + + case 'R': + packeter.unpacketC1F(code, value); + alvik.disablePositionControl(); + alvik.rotate(value); + ack_required=MOVEMENT_ROTATE; + ack_check=true; + break; + + case 'G': + packeter.unpacketC1F(code, value); + alvik.disablePositionControl(); + alvik.move(value); + ack_required=MOVEMENT_MOVE; + ack_check=true; + break; + + case 'Z': + packeter.unpacketC3F(code, x, y, theta); + alvik.resetPose(x, y, theta); + break; + } + } + switch (code){ case 'X': packeter.unpacketC1B(code, ack_code); if (ack_code == 'K') { diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 71d5e13..f72b692 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -298,6 +298,10 @@ float Arduino_AlvikCarrier::isBatteryCharging(){ return charging; } +bool Arduino_AlvikCarrier::isBatteryAlert(){ + return battery_alert; +} + /******************************************************************************************************/ @@ -982,6 +986,7 @@ void Arduino_AlvikCarrier::beginBehaviours(){ first_lift = true; battery_alert_time = millis(); battery_alert_wave = 100; + battery_alert = false; } @@ -1007,7 +1012,7 @@ void Arduino_AlvikCarrier::updateBehaviours(){ // battery alert if ((1<<(BATTERY_ALERT-1)) & behaviours){ - if (getBatteryVoltage()battery_alert_wave){ battery_alert_time = millis(); if (battery_alert_wave==400){ @@ -1018,11 +1023,19 @@ void Arduino_AlvikCarrier::updateBehaviours(){ setLeds(COLOR_BLACK); battery_alert_wave=400; } + } + if (getBatteryChargePercentage()stop(); - motor_right->stop(); + setIlluminator(false); } } + else{ + battery_alert = false; + } + } + else{ + battery_alert = false; } } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index a1d50c9..17c0165 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -96,6 +96,7 @@ class Arduino_AlvikCarrier{ bool first_lift; unsigned long battery_alert_time; unsigned long battery_alert_wave; + bool battery_alert; @@ -159,6 +160,7 @@ class Arduino_AlvikCarrier{ float getBatteryVoltage(); // get Voltage float getBatteryChargePercentage(); // get battery percentage float isBatteryCharging(); // return -1 if battery discharge or 1 if charging + bool isBatteryAlert(); // return true if battery is LOW and alert behaviour is setted, otherwise false @@ -190,7 +192,6 @@ class Arduino_AlvikCarrier{ - // Touch int beginTouch(); // initialize touch void updateTouch(); // update touch status diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index a3b8c6d..ed52ae2 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -96,7 +96,8 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ); #define VERSION_BYTE_LOW 3 // Battery stats -#define BATTERY_ALERT_MINIMUM_VOLTAGE 4.0 +#define BATTERY_ALERT_MINIMUM_CHARGE 96.0 +#define BATTERY_ALERT_STOP_CHARGE 94.0 From 721dc823125e073f6d5749817fbc55082a262d91 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Thu, 10 Oct 2024 19:06:41 +0200 Subject: [PATCH 22/23] 1.0.4 - battery alert --- library.properties | 2 +- src/Arduino_AlvikCarrier.cpp | 2 ++ src/definitions/robot_definitions.h | 6 +++--- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/library.properties b/library.properties index a03415b..ceb15a5 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Arduino_AlvikCarrier -version=1.0.3 +version=1.0.4 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 f72b692..bf8f04f 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -1017,10 +1017,12 @@ void Arduino_AlvikCarrier::updateBehaviours(){ battery_alert_time = millis(); if (battery_alert_wave==400){ setLeds(COLOR_RED); + setLedBuiltin(HIGH); battery_alert_wave=100; } else{ setLeds(COLOR_BLACK); + setLedBuiltin(LOW); battery_alert_wave=400; } } diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index ed52ae2..8f606b9 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -93,11 +93,11 @@ 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 3 +#define VERSION_BYTE_LOW 4 // Battery stats -#define BATTERY_ALERT_MINIMUM_CHARGE 96.0 -#define BATTERY_ALERT_STOP_CHARGE 94.0 +#define BATTERY_ALERT_MINIMUM_CHARGE 20.0 +#define BATTERY_ALERT_STOP_CHARGE 10.0 From 430f13075cec5b079fb20d9e5aeab6dbb5bc0266 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 16 Oct 2024 16:28:57 +0200 Subject: [PATCH 23/23] fix battery is charging, 1.1.0 version --- library.properties | 2 +- src/Arduino_AlvikCarrier.cpp | 2 +- src/definitions/robot_definitions.h | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/library.properties b/library.properties index ceb15a5..a754aa2 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Arduino_AlvikCarrier -version=1.0.4 +version=1.1.0 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 bf8f04f..24a61c4 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -277,7 +277,7 @@ int Arduino_AlvikCarrier::beginBMS(){ void Arduino_AlvikCarrier::updateBMS(){ voltage = bms->readVCell(); state_of_charge = bms->readSoc(); - if (bms->isCharging()){ + if (bms->readCurrent()>=0){ charging = 1.0; } else{ diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index 8f606b9..bae10e0 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -92,8 +92,8 @@ 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 4 +#define VERSION_BYTE_MID 1 +#define VERSION_BYTE_LOW 0 // Battery stats #define BATTERY_ALERT_MINIMUM_CHARGE 20.0