From a2a8891c220910929ae6ae57fada4c9e50f6d804 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 24 Jan 2024 18:39:58 +0100 Subject: [PATCH 01/17] position control and drive into firmware --- examples/firmware_01/firmware_01.ino | 48 +++++++++++++++++++++++----- library.properties | 2 +- src/definitions/robot_definitions.h | 2 +- 3 files changed, 42 insertions(+), 10 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index 18c8865..d258c10 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -9,6 +9,8 @@ */ +// WIP -> preliminary firmware + #include "Arduino_AlvikCarrier.h" #include "sensor_line.h" @@ -35,6 +37,7 @@ unsigned long timu=0; float left, right, value; +float linear, angular; uint8_t leds; uint8_t sensor_id = 0; @@ -77,20 +80,48 @@ void loop(){ switch (code){ case 'J': packeter.unpacketC2F(code,left,right); + alvik.disablePositionControl(); alvik.setRpm(left, right); break; + + case 'V': + packeter.unpacketC2F(code,linear,angular); + alvik.disablePositionControl(); + alvik.drive(linear,angular); + break; + case 'W': packeter.unpacketC2B1F(code,label,control_type,value); - if ((label == 'L') && (control_type == 'V')) { - alvik.motor_control_left->setRPM(value); + if (label=='L'){ + switch (control_type){ + case 'V': + alvik.disablePositionControlLeft(); + alvik.setRpmLeft(value); + break; + case 'P': + alvik.setPositionLeft(value); + break; + case 'Z': + alvik.resetPositionLeft(value); + break; + } } - else if ((label == 'R') && (control_type == 'V')) - { - alvik.motor_control_right->setRPM(value); + if (label=='R'){ + switch (control_type){ + case 'V': + alvik.disablePositionControlRight(); + alvik.setRpmRight(value); + break; + case 'P': + alvik.setPositionRight(value); + break; + case 'Z': + alvik.resetPositionRight(value); + break; + } } - break; - + case 'S': packeter.unpacketC2B(code,servo_A,servo_B); alvik.setServoA(servo_A); @@ -154,7 +185,8 @@ void loop(){ alvik.updateMotors(); msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight()); alvik.serial->write(packeter.msg,msg_size); - + msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight()); + alvik.serial->write(packeter.msg,msg_size); } if (millis()-timu>10){ diff --git a/library.properties b/library.properties index 48d1ddc..9ae0921 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Arduino Alvik Carrier -version=0.1.1 +version=0.1.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 5c5e6ae..dc07732 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -52,7 +52,7 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ); #define VERSION_BYTE_HIGH 0 #define VERSION_BYTE_MID 1 -#define VERSION_BYTE_LOW 0 +#define VERSION_BYTE_LOW 2 From 5a05865c291a431ba4d7b36d2531e16fc632299b Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 26 Jan 2024 16:00:20 +0100 Subject: [PATCH 02/17] disabled illuminator on begin --- examples/firmware_01/firmware_01.ino | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index d258c10..4972caa 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -52,6 +52,8 @@ uint8_t servo_A, servo_B; void setup(){ Serial.begin(115200); alvik.begin(); + alvik.disableIlluminator(); + alvik.setLeds(COLOR_ORANGE); alvik.setLedBuiltin(HIGH); line.begin(); tof.begin(); @@ -62,6 +64,7 @@ void setup(){ alvik.serial->write(packeter.msg,msg_size); alvik.setLedBuiltin(LOW); + alvik.setLeds(COLOR_BLACK); code=0; From 75b0efc3aa3bd25a8ac286f5bed7719989af3e3b Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Fri, 26 Jan 2024 18:02:58 +0100 Subject: [PATCH 03/17] not locking rotate --- examples/kinematics/kinematics.ino | 12 ++++++------ src/Arduino_AlvikCarrier.cpp | 20 ++++++++++++++++++-- src/Arduino_AlvikCarrier.h | 7 ++++++- src/definitions/robot_definitions.h | 7 +++++++ 4 files changed, 37 insertions(+), 9 deletions(-) diff --git a/examples/kinematics/kinematics.ino b/examples/kinematics/kinematics.ino index 056c93b..236785f 100644 --- a/examples/kinematics/kinematics.ino +++ b/examples/kinematics/kinematics.ino @@ -31,8 +31,7 @@ void loop() { if (millis()-tmotor>20){ tmotor=millis(); alvik.updateMotors(); - alvik.kinematics->inverse(alvik.motor_control_left->getRPM(),alvik.motor_control_right->getRPM()); - alvik.kinematics->updatePose(); + alvik.updateKinematics(); Serial.print("\t"); Serial.print(alvik.kinematics->getLinearVelocity()); Serial.print("\t"); @@ -46,20 +45,22 @@ void loop() { Serial.print("\n"); } - if (millis()-ttask>2000){ + if (millis()-ttask>5000){ ttask=millis(); switch (task){ case 0: alvik.rotate(90); break; case 1: - alvik.drive(40,0); + alvik.disableKinematicsMovement(); + alvik.drive(0,0); break; case 2: alvik.rotate(-90); break; case 3: - alvik.drive(-40,0); + alvik.disableKinematicsMovement(); + alvik.drive(0,0); break; } task++; @@ -67,5 +68,4 @@ void loop() { task=0; } } - } diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index d35fe00..bb2f9a2 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -76,6 +76,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ // kinematics kinematics = new Kinematics(WHEEL_TRACK_MM, WHEEL_DIAMETER_MM); kinematics_movement = 0; + rotate_pid = new PidController(ROTATE_KP_DEFAULT, ROTATE_KI_DEFAULT, ROTATE_KD_DEFAULT, ROTATE_CONTROL_PERIOD, ROTATE_MAX_SPEED); } @@ -672,7 +673,8 @@ void Arduino_AlvikCarrier::drive(const float linear, const float angular){ setRpm(kinematics->getLeftVelocity(), kinematics->getRightVelocity()); } -void Arduino_AlvikCarrier::rotate(const float angle){ + +void Arduino_AlvikCarrier::lockingRotate(const float angle){ float initial_angle = kinematics->getTheta(); float final_angle = angle+initial_angle; float error = angle; @@ -698,6 +700,13 @@ void Arduino_AlvikCarrier::rotate(const float angle){ } +void Arduino_AlvikCarrier::rotate(const float angle){ + rotate_pid->reset(); + rotate_pid->setReference(kinematics->getTheta()+angle); + kinematics_movement=1; +} + + void Arduino_AlvikCarrier::move(const float distance){ float initial_travel = kinematics->getTravel(); @@ -732,6 +741,13 @@ void Arduino_AlvikCarrier::updateKinematics(){ kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM()); kinematics->updatePose(); if (kinematics_movement!=0){ - // do controls + if (kinematics_movement==1){ + rotate_pid->update(kinematics->getTheta()); + drive(0,round(rotate_pid->getControlOutput()/10.0)*10); + } } } + +void Arduino_AlvikCarrier::disableKinematicsMovement(){ + kinematics_movement=0; +} diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index e6645bc..336142f 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -70,6 +70,8 @@ class Arduino_AlvikCarrier{ uint8_t kinematics_movement; + PidController * rotate_pid; + PidController * move_pid; @@ -215,8 +217,11 @@ class Arduino_AlvikCarrier{ void move(const float distance); // move of distance millimeters void rotate(const float angle); // rotate of angle degrees - + + void lockingRotate(const float angle); + + void disableKinematicsMovement(); }; diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index dc07732..6d6d174 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -34,6 +34,13 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO; #define POSITION_CONTROL_PERIOD 0.02 #define POSITION_MAX_SPEED 30.0 +#define ROTATE_KP_DEFAULT 5.0 +#define ROTATE_KI_DEFAULT 0.0 +#define ROTATE_KD_DEFAULT 0.001 +#define ROTATE_CONTROL_PERIOD 0.02 +#define ROTATE_MAX_SPEED 45 + + // Sensor fusioning parameters #define FROM_MG_TO_G 0.001f From c695dd2fa65ffbf3e80fe7bd94491c3ea011058e Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Mon, 29 Jan 2024 12:51:36 +0100 Subject: [PATCH 04/17] kinematics now returns the state --- src/Arduino_AlvikCarrier.cpp | 16 ++++++++++++++-- src/Arduino_AlvikCarrier.h | 3 +++ src/definitions/robot_definitions.h | 1 + 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index bb2f9a2..a5f2bee 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -76,6 +76,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ // kinematics kinematics = new Kinematics(WHEEL_TRACK_MM, WHEEL_DIAMETER_MM); kinematics_movement = 0; + kinematics_achieved = false; rotate_pid = new PidController(ROTATE_KP_DEFAULT, ROTATE_KI_DEFAULT, ROTATE_KD_DEFAULT, ROTATE_CONTROL_PERIOD, ROTATE_MAX_SPEED); } @@ -704,11 +705,12 @@ void Arduino_AlvikCarrier::rotate(const float angle){ rotate_pid->reset(); rotate_pid->setReference(kinematics->getTheta()+angle); kinematics_movement=1; + kinematics_achieved=false; } -void Arduino_AlvikCarrier::move(const float distance){ +void Arduino_AlvikCarrier::lockingMove(const float distance){ float initial_travel = kinematics->getTravel(); float final_travel = abs(distance)+initial_travel; float error = abs(distance); @@ -735,7 +737,10 @@ void Arduino_AlvikCarrier::move(const float distance){ } - +void Arduino_AlvikCarrier::move(const float distance){ + move_pid->reset(); + //move_pid->setReference() +} void Arduino_AlvikCarrier::updateKinematics(){ kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM()); @@ -744,6 +749,9 @@ void Arduino_AlvikCarrier::updateKinematics(){ if (kinematics_movement==1){ rotate_pid->update(kinematics->getTheta()); drive(0,round(rotate_pid->getControlOutput()/10.0)*10); + if (abs(rotate_pid->getError()) Date: Mon, 29 Jan 2024 17:45:59 +0100 Subject: [PATCH 05/17] fix --- src/Arduino_AlvikCarrier.cpp | 38 ++++++++++++++++++++++------- src/Arduino_AlvikCarrier.h | 5 +++- src/definitions/robot_definitions.h | 27 ++++++++++++++++---- src/robotics/kinematics.h | 4 +++ 4 files changed, 59 insertions(+), 15 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index a5f2bee..8b4ceac 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -75,10 +75,12 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ // kinematics kinematics = new Kinematics(WHEEL_TRACK_MM, WHEEL_DIAMETER_MM); - kinematics_movement = 0; + kinematics_movement = MOVEMENT_DISABLED; kinematics_achieved = false; + previous_travel = 0.0; + move_direction = 0.0; rotate_pid = new PidController(ROTATE_KP_DEFAULT, ROTATE_KI_DEFAULT, ROTATE_KD_DEFAULT, ROTATE_CONTROL_PERIOD, ROTATE_MAX_SPEED); - + move_pid = new PidController(MOVE_KP_DEFAULT, MOTOR_KI_DEFAULT, MOVE_KD_DEFAULT, MOVE_CONTROL_PERIOD, MOVE_MAX_SPEED); } int Arduino_AlvikCarrier::begin(){ @@ -148,7 +150,8 @@ void Arduino_AlvikCarrier::getVersion(uint8_t &high_byte, uint8_t &mid_byte, uin int Arduino_AlvikCarrier::beginAPDS(){ pinMode(APDS_LED, OUTPUT); - enableIlluminator(); + //enableIlluminator(); + disableIlluminator(); if (!apds9960->begin()){ return ERROR_APDS; } @@ -704,7 +707,7 @@ void Arduino_AlvikCarrier::lockingRotate(const float angle){ void Arduino_AlvikCarrier::rotate(const float angle){ rotate_pid->reset(); rotate_pid->setReference(kinematics->getTheta()+angle); - kinematics_movement=1; + kinematics_movement=MOVEMENT_ROTATE; kinematics_achieved=false; } @@ -739,25 +742,42 @@ void Arduino_AlvikCarrier::lockingMove(const float distance){ void Arduino_AlvikCarrier::move(const float distance){ move_pid->reset(); - //move_pid->setReference() + previous_travel=kinematics->getTravel(); + if (distance<0){ + move_direction=-1.0; + } + else{ + move_direction=1.0; + } + move_pid->setReference(distance); + kinematics_movement=MOVEMENT_MOVE; + kinematics_achieved=false; } void Arduino_AlvikCarrier::updateKinematics(){ kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM()); kinematics->updatePose(); - if (kinematics_movement!=0){ - if (kinematics_movement==1){ + if (kinematics_movement!=MOVEMENT_DISABLED){ + if (kinematics_movement==MOVEMENT_ROTATE){ rotate_pid->update(kinematics->getTheta()); - drive(0,round(rotate_pid->getControlOutput()/10.0)*10); + drive(0, round(rotate_pid->getControlOutput()/10.0)*10); if (abs(rotate_pid->getError())update((kinematics->getTravel()-previous_travel)*move_direction); + drive(round(move_pid->getControlOutput()/10.0)*10, 0); + + if (abs(move_pid->getError()) Date: Tue, 30 Jan 2024 10:30:49 +0100 Subject: [PATCH 06/17] getClear channel of apds --- src/Arduino_AlvikCarrier.cpp | 4 ++++ src/Arduino_AlvikCarrier.h | 1 + 2 files changed, 5 insertions(+) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 8b4ceac..48cbcf8 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -193,6 +193,10 @@ int Arduino_AlvikCarrier::getBlue(){ return bottom_blue; } +int Arduino_AlvikCarrier::getClear(){ + return bottom_clear; +} + int Arduino_AlvikCarrier::getProximity(){ return bottom_proximity; } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 1020114..0137cca 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -115,6 +115,7 @@ class Arduino_AlvikCarrier{ int getRed(); // red value 0-4095 int getGreen(); // green value 0-4095 int getBlue(); // blue value 0-4095 + int getClear(); // clear value 0-4095 int getProximity(); // proximity value 0-127 From 2b0e90ca75580feb1ad9f54d100f339b9c756d02 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jan 2024 14:35:31 +0100 Subject: [PATCH 07/17] fixed move and updateKinematics --- src/Arduino_AlvikCarrier.cpp | 5 +++-- src/Arduino_AlvikCarrier.h | 7 ++++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 48cbcf8..bd47e09 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -80,7 +80,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){ previous_travel = 0.0; move_direction = 0.0; rotate_pid = new PidController(ROTATE_KP_DEFAULT, ROTATE_KI_DEFAULT, ROTATE_KD_DEFAULT, ROTATE_CONTROL_PERIOD, ROTATE_MAX_SPEED); - move_pid = new PidController(MOVE_KP_DEFAULT, MOTOR_KI_DEFAULT, MOVE_KD_DEFAULT, MOVE_CONTROL_PERIOD, MOVE_MAX_SPEED); + move_pid = new PidController(MOVE_KP_DEFAULT, MOVE_KI_DEFAULT, MOVE_KD_DEFAULT, MOVE_CONTROL_PERIOD, MOVE_MAX_SPEED); } int Arduino_AlvikCarrier::begin(){ @@ -747,6 +747,7 @@ void Arduino_AlvikCarrier::lockingMove(const float distance){ void Arduino_AlvikCarrier::move(const float distance){ move_pid->reset(); previous_travel=kinematics->getTravel(); + actual_direction=1.0; if (distance<0){ move_direction=-1.0; } @@ -772,10 +773,10 @@ void Arduino_AlvikCarrier::updateKinematics(){ if (kinematics_movement==MOVEMENT_MOVE){ move_pid->update((kinematics->getTravel()-previous_travel)*move_direction); drive(round(move_pid->getControlOutput()/10.0)*10, 0); - if (abs(move_pid->getError()) Date: Tue, 30 Jan 2024 14:57:24 +0100 Subject: [PATCH 08/17] added robot velocity into firmware --- examples/firmware_01/firmware_01.ino | 8 +++++++- src/Arduino_AlvikCarrier.cpp | 8 ++++++++ src/Arduino_AlvikCarrier.h | 2 ++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index 4972caa..ef4010f 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -186,10 +186,16 @@ void loop(){ if (millis()-tmotor>20){ tmotor=millis(); alvik.updateMotors(); + alvik.updateKinematics(); + // joint speed msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight()); alvik.serial->write(packeter.msg,msg_size); + // joint position msg_size = packeter.packetC2F('w', alvik.getPositionLeft(),alvik.getPositionRight()); - alvik.serial->write(packeter.msg,msg_size); + alvik.serial->write(packeter.msg, msg_size); + // robot speed + msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity()); + alvik.serial->write(packeter.msg, msg_size); } if (millis()-timu>10){ diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index bd47e09..8ca772b 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -681,6 +681,14 @@ void Arduino_AlvikCarrier::drive(const float linear, const float angular){ setRpm(kinematics->getLeftVelocity(), kinematics->getRightVelocity()); } +float Arduino_AlvikCarrier::getLinearVelocity(){ + return kinematics->getLinearVelocity(); +} + +float Arduino_AlvikCarrier::getAngularVelocity(){ + return kinematics->getAngularVelocity(); +} + void Arduino_AlvikCarrier::lockingRotate(const float angle){ float initial_angle = kinematics->getTheta(); diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 877843e..fc86883 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -220,6 +220,8 @@ class Arduino_AlvikCarrier{ // Kinematics void updateKinematics(); // update pose/velocity of the robot and controls void drive(const float linear, const float angular); // set mm/s and deg/s of the robot + float getLinearVelocity(); + float getAngularVelocity(); void move(const float distance); // move of distance millimeters void rotate(const float angle); // rotate of angle degrees From b2ca4b74dc7d3a9455d5e1f5f3628e5e3d428986 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jan 2024 15:55:24 +0100 Subject: [PATCH 09/17] added move and rotate --- examples/firmware_01/firmware_01.ino | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index ef4010f..c2adc41 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -83,18 +83,21 @@ void loop(){ 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': @@ -145,6 +148,16 @@ void loop(){ alvik.setKPidRight(kp,ki,kd); } break; + + case 'R': + packeter.unpacketC1F(code,value); + alvik.rotate(value); + break; + + case 'G': + packeter.unpacketC1F(code,value); + alvik.move(value); + break; } } From 8506d0b7c546f9dde96c1214080f4de7f7b40a3f Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jan 2024 16:08:36 +0100 Subject: [PATCH 10/17] fixs on firmware --- examples/firmware_01/firmware_01.ino | 14 ++++++++++++++ src/Arduino_AlvikCarrier.cpp | 4 ++++ src/Arduino_AlvikCarrier.h | 1 + 3 files changed, 19 insertions(+) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index c2adc41..8ef96fc 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -209,6 +209,20 @@ void loop(){ // robot speed msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity()); alvik.serial->write(packeter.msg, msg_size); + + if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){ + if (alvik.isTargetReached()){ + if (alvik.getKinematicsMovement()==MOVEMENT_ROTATE){ + msg_size = packeter.packetC1B('x', 'R'); + } + if (alvik.getKinematicsMovement()==MOVEMENT_MOVE){ + msg_size = packeter.packetC1B('x', 'M'); + } + alvik.serial->write(packeter.msg, msg_size); + alvik.disableKinematicsMovement(); + } + + } } if (millis()-timu>10){ diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 8ca772b..162a789 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -796,3 +796,7 @@ void Arduino_AlvikCarrier::disableKinematicsMovement(){ bool Arduino_AlvikCarrier::isTargetReached(){ return kinematics_achieved; } + +uint8_t Arduino_AlvikCarrier::getKinematicsMovement(){ + return kinematics_movement; +} diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index fc86883..3a44441 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -232,6 +232,7 @@ class Arduino_AlvikCarrier{ void disableKinematicsMovement(); bool isTargetReached(); + uint8_t getKinematicsMovement(); // get which kind of motion is running in kinematic control }; From 33e6bf737d62c60df238ddc58569c799b6e24e6b Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jan 2024 16:19:14 +0100 Subject: [PATCH 11/17] cleanup --- src/Arduino_AlvikCarrier.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 3a44441..2ca1f3e 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -220,8 +220,8 @@ class Arduino_AlvikCarrier{ // Kinematics void updateKinematics(); // update pose/velocity of the robot and controls void drive(const float linear, const float angular); // set mm/s and deg/s of the robot - float getLinearVelocity(); - float getAngularVelocity(); + float getLinearVelocity(); // get linear velocity of the robot in mm/s + float getAngularVelocity(); // get angular velocity of the robot in deg/s void move(const float distance); // move of distance millimeters void rotate(const float angle); // rotate of angle degrees @@ -232,7 +232,7 @@ class Arduino_AlvikCarrier{ void disableKinematicsMovement(); bool isTargetReached(); - uint8_t getKinematicsMovement(); // get which kind of motion is running in kinematic control + uint8_t getKinematicsMovement(); // get which kind of motion is running in kinematic control }; From a32623000ec171156ed4c542f315d0b0cad0c6ef Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Tue, 30 Jan 2024 16:20:50 +0100 Subject: [PATCH 12/17] cleanup --- src/Arduino_AlvikCarrier.cpp | 54 ++++++++++++++---------------------- 1 file changed, 21 insertions(+), 33 deletions(-) diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 162a789..49acb99 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -669,12 +669,27 @@ void Arduino_AlvikCarrier::errorLed(const int error_code){ /* Kinematics */ /******************************************************************************************************/ - - - - - - +void Arduino_AlvikCarrier::updateKinematics(){ + kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM()); + kinematics->updatePose(); + if (kinematics_movement!=MOVEMENT_DISABLED){ + if (kinematics_movement==MOVEMENT_ROTATE){ + rotate_pid->update(kinematics->getTheta()); + drive(0, round(rotate_pid->getControlOutput()/10.0)*10); + if (abs(rotate_pid->getError())update((kinematics->getTravel()-previous_travel)*move_direction); + drive(round(move_pid->getControlOutput()/10.0)*10, 0); + if (abs(move_pid->getError())forward(linear, angular); @@ -689,7 +704,6 @@ float Arduino_AlvikCarrier::getAngularVelocity(){ return kinematics->getAngularVelocity(); } - void Arduino_AlvikCarrier::lockingRotate(const float angle){ float initial_angle = kinematics->getTheta(); float final_angle = angle+initial_angle; @@ -715,7 +729,6 @@ void Arduino_AlvikCarrier::lockingRotate(const float angle){ motor_control_right->brake(); } - void Arduino_AlvikCarrier::rotate(const float angle){ rotate_pid->reset(); rotate_pid->setReference(kinematics->getTheta()+angle); @@ -723,8 +736,6 @@ void Arduino_AlvikCarrier::rotate(const float angle){ kinematics_achieved=false; } - - void Arduino_AlvikCarrier::lockingMove(const float distance){ float initial_travel = kinematics->getTravel(); float final_travel = abs(distance)+initial_travel; @@ -751,7 +762,6 @@ void Arduino_AlvikCarrier::lockingMove(const float distance){ motor_control_right->brake(); } - void Arduino_AlvikCarrier::move(const float distance){ move_pid->reset(); previous_travel=kinematics->getTravel(); @@ -767,28 +777,6 @@ void Arduino_AlvikCarrier::move(const float distance){ kinematics_achieved=false; } -void Arduino_AlvikCarrier::updateKinematics(){ - kinematics->inverse(motor_control_left->getRPM(), motor_control_right->getRPM()); - kinematics->updatePose(); - if (kinematics_movement!=MOVEMENT_DISABLED){ - if (kinematics_movement==MOVEMENT_ROTATE){ - rotate_pid->update(kinematics->getTheta()); - drive(0, round(rotate_pid->getControlOutput()/10.0)*10); - if (abs(rotate_pid->getError())update((kinematics->getTravel()-previous_travel)*move_direction); - drive(round(move_pid->getControlOutput()/10.0)*10, 0); - if (abs(move_pid->getError()) Date: Tue, 30 Jan 2024 18:27:39 +0100 Subject: [PATCH 13/17] fix firmware, mpy ack --- examples/firmware_01/firmware_01.ino | 2 +- src/Arduino_AlvikCarrier.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index 8ef96fc..ed860ca 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -219,7 +219,7 @@ void loop(){ msg_size = packeter.packetC1B('x', 'M'); } alvik.serial->write(packeter.msg, msg_size); - alvik.disableKinematicsMovement(); + //alvik.disableKinematicsMovement(); } } diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 49acb99..d4fb9db 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -765,7 +765,6 @@ void Arduino_AlvikCarrier::lockingMove(const float distance){ void Arduino_AlvikCarrier::move(const float distance){ move_pid->reset(); previous_travel=kinematics->getTravel(); - actual_direction=1.0; if (distance<0){ move_direction=-1.0; } From 8d0f29d7c174c84747a0b13f80aefc6f2ed111dc Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jan 2024 10:24:31 +0100 Subject: [PATCH 14/17] robot pose --- examples/firmware_01/firmware_01.ino | 3 +++ src/Arduino_AlvikCarrier.cpp | 12 ++++++++++++ src/Arduino_AlvikCarrier.h | 3 +++ 3 files changed, 18 insertions(+) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index ed860ca..c2c191c 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -209,6 +209,9 @@ void loop(){ // robot speed 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()); + alvik.serial->write(packeter.msg, msg_size); if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){ if (alvik.isTargetReached()){ diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index d4fb9db..d45a3dc 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -704,6 +704,18 @@ float Arduino_AlvikCarrier::getAngularVelocity(){ return kinematics->getAngularVelocity(); } +float Arduino_AlvikCarrier::getX(){ + return kinematics->getX(); +} + +float Arduino_AlvikCarrier::getY(){ + return kinematics->getY(); +} + +float Arduino_AlvikCarrier::getTheta(){ + return kinematics->getTheta(); +} + void Arduino_AlvikCarrier::lockingRotate(const float angle){ float initial_angle = kinematics->getTheta(); float final_angle = angle+initial_angle; diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 2ca1f3e..2720470 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -222,6 +222,9 @@ class Arduino_AlvikCarrier{ void drive(const float linear, const float angular); // set mm/s and deg/s of the robot float getLinearVelocity(); // get linear velocity of the robot in mm/s float getAngularVelocity(); // get angular velocity of the robot in deg/s + float getX(); // absolute position in mm + float getY(); // absolute position in mm + float getTheta(); // angle in deg void move(const float distance); // move of distance millimeters void rotate(const float angle); // rotate of angle degrees From f44b776562abc1202800a943cb65ed2ea20dca18 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jan 2024 11:09:36 +0100 Subject: [PATCH 15/17] reset pose --- examples/firmware_01/firmware_01.ino | 12 +++++++++--- src/Arduino_AlvikCarrier.cpp | 4 ++++ src/Arduino_AlvikCarrier.h | 1 + 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index c2c191c..c7e82ba 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -45,6 +45,7 @@ uint8_t sensor_id = 0; uint8_t pid; float kp, ki, kd; +float x, y, theta; uint8_t servo_A, servo_B; @@ -150,14 +151,19 @@ void loop(){ break; case 'R': - packeter.unpacketC1F(code,value); + packeter.unpacketC1F(code, value); alvik.rotate(value); break; case 'G': - packeter.unpacketC1F(code,value); + packeter.unpacketC1F(code, value); alvik.move(value); break; + + case 'Z': + packeter.unpacketC3F(code, x, y, theta); + alvik.resetPose(x, y, theta); + break; } } @@ -234,4 +240,4 @@ void loop(){ msg_size = packeter.packetC6F('i', alvik.getAccelerationX(), alvik.getAccelerationY(), alvik.getAccelerationZ(), alvik.getAngularVelocityX(), alvik.getAngularVelocityY(), alvik.getAngularVelocityZ()); alvik.serial->write(packeter.msg,msg_size); } -} \ No newline at end of file +} diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index d45a3dc..e23961c 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -716,6 +716,10 @@ float Arduino_AlvikCarrier::getTheta(){ return kinematics->getTheta(); } +void Arduino_AlvikCarrier::resetPose(const float x0, const float y0, const float theta0){ + kinematics->resetPose(x0,y0,theta0); +} + void Arduino_AlvikCarrier::lockingRotate(const float angle){ float initial_angle = kinematics->getTheta(); float final_angle = angle+initial_angle; diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index 2720470..84cbba1 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -225,6 +225,7 @@ class Arduino_AlvikCarrier{ float getX(); // absolute position in mm float getY(); // absolute position in mm float getTheta(); // angle in deg + void resetPose(const float x0=0.0, const float y0=0.0, const float theta0=0.0); // reset pose in kinematics void move(const float distance); // move of distance millimeters void rotate(const float angle); // rotate of angle degrees From 2a5b9af7f9b9eb4c2cea283653c5e4a8a882dd8f Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jan 2024 11:18:16 +0100 Subject: [PATCH 16/17] firmware fix position control on move and rotate --- examples/firmware_01/firmware_01.ino | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino index c7e82ba..ac678b3 100644 --- a/examples/firmware_01/firmware_01.ino +++ b/examples/firmware_01/firmware_01.ino @@ -152,11 +152,13 @@ void loop(){ case 'R': packeter.unpacketC1F(code, value); + alvik.disablePositionControl(); alvik.rotate(value); break; case 'G': packeter.unpacketC1F(code, value); + alvik.disablePositionControl(); alvik.move(value); break; From 109f41f6ee224fedef561cd6a8fef3b13f512f7d Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 31 Jan 2024 13:07:50 +0100 Subject: [PATCH 17/17] update kinematics example --- examples/kinematics/kinematics.ino | 32 ++++++++++++++++++++++-------- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/examples/kinematics/kinematics.ino b/examples/kinematics/kinematics.ino index 236785f..8a595bc 100644 --- a/examples/kinematics/kinematics.ino +++ b/examples/kinematics/kinematics.ino @@ -9,7 +9,7 @@ */ -// WIP +// This example shows how to implement kinematics commands and retrieve odometry data from Arduino_AlvikCarrier class #include "Arduino_AlvikCarrier.h" @@ -17,13 +17,16 @@ Arduino_AlvikCarrier alvik; unsigned long tmotor=0; unsigned long ttask=0; +unsigned long tled=0; uint8_t task=0; +bool led_value = false; void setup() { Serial.begin(115200); alvik.begin(); ttask=millis(); tmotor=millis(); + tled=millis(); task=0; } @@ -33,19 +36,19 @@ void loop() { alvik.updateMotors(); alvik.updateKinematics(); Serial.print("\t"); - Serial.print(alvik.kinematics->getLinearVelocity()); + Serial.print(alvik.getLinearVelocity()); Serial.print("\t"); - Serial.print(alvik.kinematics->getAngularVelocity()); + Serial.print(alvik.getAngularVelocity()); Serial.print("\t"); - Serial.print(alvik.kinematics->getX()); + Serial.print(alvik.getX()); Serial.print("\t"); - Serial.print(alvik.kinematics->getY()); + Serial.print(alvik.getY()); Serial.print("\t"); - Serial.print(alvik.kinematics->getTheta()); + Serial.print(alvik.getTheta()); Serial.print("\n"); } - if (millis()-ttask>5000){ + if ((millis()-ttask>5000)||alvik.isTargetReached()){ ttask=millis(); switch (task){ case 0: @@ -56,7 +59,7 @@ void loop() { alvik.drive(0,0); break; case 2: - alvik.rotate(-90); + alvik.move(100); break; case 3: alvik.disableKinematicsMovement(); @@ -68,4 +71,17 @@ void loop() { task=0; } } + + if (millis()-tled>200){ + tled=millis(); + led_value=!led_value; + if (task==1){ + alvik.setLedLeftGreen(false); + alvik.setLedLeftRed(led_value); + } + if (task==3){ + alvik.setLedLeftRed(false); + alvik.setLedLeftGreen(led_value); + } + } }