From 245cf6aec2a5054e64c786b8a0f0ed21b869afa6 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jan 2024 17:11:24 +0100 Subject: [PATCH 01/11] changed code in firmware for pose (s -> z) --- examples/firmware_01/firmware_01.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index ac678b3..bb39b1c 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -218,7 +218,7 @@ void loop(){ msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity()); alvik.serial->write(packeter.msg, msg_size); // pose - msg_size = packeter.packetC3F('s', alvik.getX(), alvik.getY(), alvik.getTheta()); + msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta()); alvik.serial->write(packeter.msg, msg_size); if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){ From 6a4ec77cbd441eba96a68b2001d1269b03210573 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jan 2024 17:42:58 +0100 Subject: [PATCH 02/11] flush serial on alvik begin --- src/Arduino_AlvikCarrier.cpp | 1 + src/robotics/kinematics.h | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index e23961c..c3cdc10 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -87,6 +87,7 @@ int Arduino_AlvikCarrier::begin(){ beginLeds(); serial->begin(UART_BAUD); + serial->flush(); // setup alternate functions AF_Tim2_pwm(); diff --git a/src/robotics/kinematics.h b/src/robotics/kinematics.h index 7e850f1..0b7e001 100644 --- a/src/robotics/kinematics.h +++ b/src/robotics/kinematics.h @@ -174,7 +174,6 @@ class Kinematics{ delta_left=0.0; delta_right=0.0; delta_travel=0.0; - } void resetTravel(){ From 969ac086fae1174a284643da64113407cf5dfee2 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jan 2024 17:50:38 +0100 Subject: [PATCH 03/11] added disableKinematics after reaching target --- src/Arduino_AlvikCarrier.cpp | 2 ++ src/definitions/robot_definitions.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index c3cdc10..ff13527 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -679,6 +679,7 @@ void Arduino_AlvikCarrier::updateKinematics(){ drive(0, round(rotate_pid->getControlOutput()/10.0)*10); if (abs(rotate_pid->getError())getControlOutput()/10.0)*10, 0); if (abs(move_pid->getError()) Date: Thu, 1 Feb 2024 15:17:34 +0100 Subject: [PATCH 04/11] partial fix on ack --- examples/firmware_01/firmware_01.ino | 14 +++++++++++--- src/Arduino_AlvikCarrier.cpp | 2 ++ 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index bb39b1c..0ffb44a 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -28,6 +28,7 @@ uint8_t code; uint8_t label; uint8_t control_type; uint8_t msg_size; +uint8_t ack_required=0; unsigned long tmotor=0; @@ -154,12 +155,14 @@ void loop(){ packeter.unpacketC1F(code, value); alvik.disablePositionControl(); alvik.rotate(value); + ack_required=MOVEMENT_ROTATE; break; case 'G': packeter.unpacketC1F(code, value); alvik.disablePositionControl(); alvik.move(value); + ack_required=MOVEMENT_MOVE; break; case 'Z': @@ -221,16 +224,21 @@ void loop(){ msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta()); alvik.serial->write(packeter.msg, msg_size); - if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){ + if (ack_required!=0){ if (alvik.isTargetReached()){ - if (alvik.getKinematicsMovement()==MOVEMENT_ROTATE){ + Serial.print(alvik.isTargetReached()); + Serial.print("\t"); + Serial.println(alvik.getKinematicsMovement()); + + if (ack_required==MOVEMENT_ROTATE){ msg_size = packeter.packetC1B('x', 'R'); } - if (alvik.getKinematicsMovement()==MOVEMENT_MOVE){ + if (ack_required==MOVEMENT_MOVE){ msg_size = packeter.packetC1B('x', 'M'); } alvik.serial->write(packeter.msg, msg_size); //alvik.disableKinematicsMovement(); + ack_required=0; } } diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index ff13527..f1bd382 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -680,6 +680,7 @@ void Arduino_AlvikCarrier::updateKinematics(){ if (abs(rotate_pid->getError())getError()) Date: Thu, 1 Feb 2024 16:30:39 +0100 Subject: [PATCH 05/11] experimental acknowledge --- examples/firmware_01/firmware_01.ino | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index 0ffb44a..d743d0c 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -35,6 +35,7 @@ unsigned long tmotor=0; unsigned long tsend=0; unsigned long tsensor=0; unsigned long timu=0; +unsigned long tack=0; float left, right, value; @@ -74,6 +75,7 @@ void setup(){ tsend=millis(); tsensor=millis(); timu=millis(); + tack=millis(); } void loop(){ @@ -223,18 +225,21 @@ void loop(){ // pose msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta()); alvik.serial->write(packeter.msg, msg_size); - +/* if (ack_required!=0){ + //if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){ if (alvik.isTargetReached()){ Serial.print(alvik.isTargetReached()); Serial.print("\t"); - Serial.println(alvik.getKinematicsMovement()); + if (ack_required==MOVEMENT_ROTATE){ msg_size = packeter.packetC1B('x', 'R'); + Serial.println("R"); } if (ack_required==MOVEMENT_MOVE){ msg_size = packeter.packetC1B('x', 'M'); + Serial.println("M"); } alvik.serial->write(packeter.msg, msg_size); //alvik.disableKinematicsMovement(); @@ -242,6 +247,21 @@ void loop(){ } } + */ + } + + if (millis()-tack>100){ + tack=millis(); + msg_size = packeter.packetC1B('x', 0); + if (ack_required==MOVEMENT_ROTATE){ + msg_size = packeter.packetC1B('x', 'R'); + Serial.println("R"); + } + if (ack_required==MOVEMENT_MOVE){ + msg_size = packeter.packetC1B('x', 'M'); + Serial.println("M"); + } + alvik.serial->write(packeter.msg, msg_size); } if (millis()-timu>10){ From b796a6c119f54808767714e9cc3a8c410644c54f Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Thu, 1 Feb 2024 17:55:48 +0100 Subject: [PATCH 06/11] minor fix --- examples/firmware_01/firmware_01.ino | 27 +++++++++++++++++++++------ src/Arduino_AlvikCarrier.cpp | 7 +++++-- 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index d743d0c..159bed0 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -29,6 +29,7 @@ uint8_t label; uint8_t control_type; uint8_t msg_size; uint8_t ack_required=0; +int ack_counter=0; unsigned long tmotor=0; @@ -158,6 +159,7 @@ void loop(){ alvik.disablePositionControl(); alvik.rotate(value); ack_required=MOVEMENT_ROTATE; + ack_counter=5; break; case 'G': @@ -165,6 +167,7 @@ void loop(){ alvik.disablePositionControl(); alvik.move(value); ack_required=MOVEMENT_MOVE; + ack_counter=5; break; case 'Z': @@ -253,14 +256,26 @@ void loop(){ if (millis()-tack>100){ tack=millis(); msg_size = packeter.packetC1B('x', 0); - if (ack_required==MOVEMENT_ROTATE){ - msg_size = packeter.packetC1B('x', 'R'); - Serial.println("R"); + + if (alvik.isTargetReached()){ + if (ack_required==MOVEMENT_ROTATE){ + msg_size = packeter.packetC1B('x', 'R'); + //ack_counter--; + Serial.println("R"); + } + if (ack_required==MOVEMENT_MOVE){ + msg_size = packeter.packetC1B('x', 'M'); + //ack_counter--; + Serial.println("M"); + } } - if (ack_required==MOVEMENT_MOVE){ - msg_size = packeter.packetC1B('x', 'M'); - Serial.println("M"); + + + if (ack_counter<=0){ + ack_counter=0; + ack_required=MOVEMENT_DISABLED; } + alvik.serial->write(packeter.msg, msg_size); } diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index f1bd382..c5222c1 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -751,10 +751,11 @@ void Arduino_AlvikCarrier::lockingRotate(const float angle){ } void Arduino_AlvikCarrier::rotate(const float angle){ + disableKinematicsMovement(); + kinematics_achieved=false; rotate_pid->reset(); rotate_pid->setReference(kinematics->getTheta()+angle); kinematics_movement=MOVEMENT_ROTATE; - kinematics_achieved=false; } void Arduino_AlvikCarrier::lockingMove(const float distance){ @@ -784,6 +785,9 @@ void Arduino_AlvikCarrier::lockingMove(const float distance){ } void Arduino_AlvikCarrier::move(const float distance){ + disableKinematicsMovement(); + kinematics_achieved=false; + move_pid->reset(); previous_travel=kinematics->getTravel(); if (distance<0){ @@ -794,7 +798,6 @@ void Arduino_AlvikCarrier::move(const float distance){ } move_pid->setReference(distance); kinematics_movement=MOVEMENT_MOVE; - kinematics_achieved=false; } void Arduino_AlvikCarrier::disableKinematicsMovement(){ From a3e873ce8f4a0f702079b4f62f0a7c897a115a13 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 2 Feb 2024 12:43:59 +0100 Subject: [PATCH 07/11] ack only one time --- examples/firmware_01/firmware_01.ino | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index 159bed0..ffeaac8 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -30,7 +30,7 @@ uint8_t control_type; uint8_t msg_size; uint8_t ack_required=0; int ack_counter=0; - +bool ack_check=false; unsigned long tmotor=0; unsigned long tsend=0; @@ -160,6 +160,7 @@ void loop(){ alvik.rotate(value); ack_required=MOVEMENT_ROTATE; ack_counter=5; + ack_check=true; break; case 'G': @@ -168,6 +169,7 @@ void loop(){ alvik.move(value); ack_required=MOVEMENT_MOVE; ack_counter=5; + ack_check=true; break; case 'Z': @@ -257,7 +259,7 @@ void loop(){ tack=millis(); msg_size = packeter.packetC1B('x', 0); - if (alvik.isTargetReached()){ + if (ack_check&&alvik.isTargetReached()){ if (ack_required==MOVEMENT_ROTATE){ msg_size = packeter.packetC1B('x', 'R'); //ack_counter--; @@ -268,6 +270,7 @@ void loop(){ //ack_counter--; Serial.println("M"); } + ack_check=false; } From bf43c6ab8ec93f88da4f88bdcdc0d0abe4fcd40d Mon Sep 17 00:00:00 2001 From: Lucio Rossi Date: Fri, 2 Feb 2024 16:40:02 +0100 Subject: [PATCH 08/11] feat: on tgt reached stop sending ack if host responds with ack received: XK --- examples/firmware_01/firmware_01.ino | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index ffeaac8..43a2a7d 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -31,6 +31,7 @@ uint8_t msg_size; uint8_t ack_required=0; int ack_counter=0; bool ack_check=false; +uint8_t ack_code=0; unsigned long tmotor=0; unsigned long tsend=0; @@ -176,6 +177,14 @@ void loop(){ packeter.unpacketC3F(code, x, y, theta); alvik.resetPose(x, y, theta); break; + case 'X': + packeter.unpacketC1B(code, ack_code); + Serial.print("Ack received "); + Serial.println(ack_code); + if (ack_code == 'K') { + ack_check = false; + } + break; } } @@ -270,7 +279,7 @@ void loop(){ //ack_counter--; Serial.println("M"); } - ack_check=false; + // ack_check=false; } From 086119e42a2fb5a336e9f83a2343fd3a0c2c5077 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 2 Feb 2024 16:44:26 +0100 Subject: [PATCH 09/11] cleanup, road to 0.2.0 --- examples/firmware_01/firmware_01.ino | 56 ++++++---------------------- 1 file changed, 12 insertions(+), 44 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index 43a2a7d..4572e6e 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -29,7 +29,6 @@ uint8_t label; uint8_t control_type; uint8_t msg_size; uint8_t ack_required=0; -int ack_counter=0; bool ack_check=false; uint8_t ack_code=0; @@ -169,7 +168,6 @@ void loop(){ alvik.disablePositionControl(); alvik.move(value); ack_required=MOVEMENT_MOVE; - ack_counter=5; ack_check=true; break; @@ -177,10 +175,9 @@ void loop(){ packeter.unpacketC3F(code, x, y, theta); alvik.resetPose(x, y, theta); break; + case 'X': packeter.unpacketC1B(code, ack_code); - Serial.print("Ack received "); - Serial.println(ack_code); if (ack_code == 'K') { ack_check = false; } @@ -188,6 +185,7 @@ void loop(){ } } + // sensors publish if (millis()-tsensor>10){ tsensor=millis(); switch(sensor_id){ @@ -223,6 +221,7 @@ void loop(){ } } + // motors update & publish if (millis()-tmotor>20){ tmotor=millis(); alvik.updateMotors(); @@ -239,58 +238,27 @@ void loop(){ // pose msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta()); alvik.serial->write(packeter.msg, msg_size); -/* - if (ack_required!=0){ - //if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){ - if (alvik.isTargetReached()){ - Serial.print(alvik.isTargetReached()); - Serial.print("\t"); - - - if (ack_required==MOVEMENT_ROTATE){ - msg_size = packeter.packetC1B('x', 'R'); - Serial.println("R"); - } - if (ack_required==MOVEMENT_MOVE){ - msg_size = packeter.packetC1B('x', 'M'); - Serial.println("M"); - } - alvik.serial->write(packeter.msg, msg_size); - //alvik.disableKinematicsMovement(); - ack_required=0; - } - } - */ } - if (millis()-tack>100){ - tack=millis(); - msg_size = packeter.packetC1B('x', 0); - - if (ack_check&&alvik.isTargetReached()){ - if (ack_required==MOVEMENT_ROTATE){ + // acknowledge + if (millis()-tack > 100){ + tack = millis(); + if (ack_check && alvik.isTargetReached()){ + if (ack_required == MOVEMENT_ROTATE){ msg_size = packeter.packetC1B('x', 'R'); - //ack_counter--; - Serial.println("R"); } - if (ack_required==MOVEMENT_MOVE){ + if (ack_required == MOVEMENT_MOVE){ msg_size = packeter.packetC1B('x', 'M'); - //ack_counter--; - Serial.println("M"); } - // ack_check=false; } - - - if (ack_counter<=0){ - ack_counter=0; - ack_required=MOVEMENT_DISABLED; + else{ + msg_size = packeter.packetC1B('x', 0); } - alvik.serial->write(packeter.msg, msg_size); } + // imu update if (millis()-timu>10){ timu=millis(); alvik.updateImu(); From 1ff6eb69c4af705a6876ff01e66555bcf55abc67 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 2 Feb 2024 17:40:29 +0100 Subject: [PATCH 10/11] 0.2.0 --- examples/firmware_01/firmware_01.ino | 1 - library.properties | 2 +- src/Arduino_AlvikCarrier.h | 4 ++-- src/definitions/robot_definitions.h | 4 ++-- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index 4572e6e..eaf8092 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -238,7 +238,6 @@ void loop(){ // pose msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta()); alvik.serial->write(packeter.msg, msg_size); - } // acknowledge diff --git a/library.properties b/library.properties index 9ae0921..63df9ea 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Arduino Alvik Carrier -version=0.1.2 +version=0.2.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.h b/src/Arduino_AlvikCarrier.h index 84cbba1..d251008 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -234,8 +234,8 @@ class Arduino_AlvikCarrier{ void lockingRotate(const float angle); // rotate of angle degrees void lockingMove(const float distance); // move of distance millimeters - void disableKinematicsMovement(); - bool isTargetReached(); + void disableKinematicsMovement(); // disable movements that requires kinematics + bool isTargetReached(); // get true if a movement is accomplished uint8_t getKinematicsMovement(); // get which kind of motion is running in kinematic control diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index aeb7cc2..2fe39bc 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -79,8 +79,8 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ); // Library version #define VERSION_BYTE_HIGH 0 -#define VERSION_BYTE_MID 1 -#define VERSION_BYTE_LOW 2 +#define VERSION_BYTE_MID 2 +#define VERSION_BYTE_LOW 0 From d06a2ad56343064daa70a17ca89c83a52536afb1 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 2 Feb 2024 17:45:38 +0100 Subject: [PATCH 11/11] fix --- examples/firmware_01/firmware_01.ino | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index eaf8092..18b499b 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -159,7 +159,6 @@ void loop(){ alvik.disablePositionControl(); alvik.rotate(value); ack_required=MOVEMENT_ROTATE; - ack_counter=5; ack_check=true; break;