From a0a884065b55eafdf6d9cb8817885d57733b065a Mon Sep 17 00:00:00 2001 From: Tim Korjakow Date: Mon, 10 Jul 2023 13:22:02 +0800 Subject: [PATCH] Hopefully fixed wrong odometry --- src/dcaiti_control/config/dcaiti_config.yml | 17 ++++++++----- src/dcaiti_control/config/empty.yaml | 1 - src/dcaiti_control/description/lidar.xacro | 6 ++--- .../description/robot.urdf.xacro | 18 ++++++++++++++ .../description/ros2_control.xacro | 4 ++-- .../embedded/include/communication/UValue.hpp | 2 +- .../embedded/include/misc/Utils.hpp | 1 - .../embedded/src/communication/UARTCom.cpp | 3 ++- .../embedded/src/communication/UCommands.cpp | 2 ++ .../embedded/src/communication/UValue.cpp | 18 ++++++++------ .../include/diffdrive_arduino/arduino_comms.h | 4 ++-- src/diffdrive_arduino/src/arduino_comms.cpp | 16 +++++++++---- .../src/diffdrive_arduino.cpp | 24 +++++++++---------- 13 files changed, 76 insertions(+), 40 deletions(-) delete mode 100644 src/dcaiti_control/config/empty.yaml diff --git a/src/dcaiti_control/config/dcaiti_config.yml b/src/dcaiti_control/config/dcaiti_config.yml index 2a07f76..f325a76 100644 --- a/src/dcaiti_control/config/dcaiti_config.yml +++ b/src/dcaiti_control/config/dcaiti_config.yml @@ -13,18 +13,23 @@ diff_cont: publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled base_frame_id: base_link + publish_limited_velocity: true left_wheel_names: ['left_wheel_joint'] right_wheel_names: ['right_wheel_joint'] wheel_separation: 0.215 wheel_radius: 0.049338032 -# linear: -# x: -# max_acceleration: 0.15 -# has_acceleration_limits: true use_stamped_vel: false -# joint_broad: -# ros__parameters: \ No newline at end of file +joint_limits: + ros__parameters: + left_wheel_joint: + has_position_limits: false # Continuous joint + has_velocity_limits: true + max_velocity: 10.0 + right_wheel_joint: + has_position_limits: false # Continuous joint + has_velocity_limits: true + max_velocity: 10.0 \ No newline at end of file diff --git a/src/dcaiti_control/config/empty.yaml b/src/dcaiti_control/config/empty.yaml deleted file mode 100644 index 04700ba..0000000 --- a/src/dcaiti_control/config/empty.yaml +++ /dev/null @@ -1 +0,0 @@ -# This is an empty file, so that git commits the folder correctly \ No newline at end of file diff --git a/src/dcaiti_control/description/lidar.xacro b/src/dcaiti_control/description/lidar.xacro index 7a90890..d4df832 100644 --- a/src/dcaiti_control/description/lidar.xacro +++ b/src/dcaiti_control/description/lidar.xacro @@ -34,19 +34,19 @@ 0 0 0 0 0 0 - true + false 10 - 360 + 360*5 -3.14 3.14 0.3 - 25 + 100 diff --git a/src/dcaiti_control/description/robot.urdf.xacro b/src/dcaiti_control/description/robot.urdf.xacro index f1e5c8a..1843437 100644 --- a/src/dcaiti_control/description/robot.urdf.xacro +++ b/src/dcaiti_control/description/robot.urdf.xacro @@ -5,6 +5,24 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/dcaiti_control/description/ros2_control.xacro b/src/dcaiti_control/description/ros2_control.xacro index edf6acf..a3d9c22 100644 --- a/src/dcaiti_control/description/ros2_control.xacro +++ b/src/dcaiti_control/description/ros2_control.xacro @@ -9,7 +9,7 @@ -0.5 0.5 - + @@ -18,7 +18,7 @@ 0.5 - + diff --git a/src/dcaitirobot/embedded/include/communication/UValue.hpp b/src/dcaitirobot/embedded/include/communication/UValue.hpp index eee5727..640b6ab 100644 --- a/src/dcaitirobot/embedded/include/communication/UValue.hpp +++ b/src/dcaitirobot/embedded/include/communication/UValue.hpp @@ -37,7 +37,7 @@ public: * @param value_left encoder value left * @param value_right encoder value right */ - static void sendBothEncoderValues(float value_left, float value_right); + static void sendBothEncoderValues(float speed_right, float speed_left, float pos_right, float pos_left); }; #endif \ No newline at end of file diff --git a/src/dcaitirobot/embedded/include/misc/Utils.hpp b/src/dcaitirobot/embedded/include/misc/Utils.hpp index 7377910..4c735f7 100644 --- a/src/dcaitirobot/embedded/include/misc/Utils.hpp +++ b/src/dcaitirobot/embedded/include/misc/Utils.hpp @@ -13,7 +13,6 @@ */ #include "Arduino.h" -#include "R2WD.h" class Utils { private: diff --git a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp b/src/dcaitirobot/embedded/src/communication/UARTCom.cpp index 358d3bf..934b8d6 100644 --- a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp +++ b/src/dcaitirobot/embedded/src/communication/UARTCom.cpp @@ -3,6 +3,7 @@ #include "communication/UValue.hpp" #include "misc/Utils.hpp" #include "constants.h" +#include "MotorWheel.h" uint8_t UARTCom::data[MAX_PACKET_LENGTH]; uint8_t currentParseIndex; @@ -170,7 +171,7 @@ void handleRequest (uint8_t* d) break; } case DRIVE_ENCODER: { - UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM()); + UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM(), wheelLeft.getPosition(), wheelRight.getPosition()); break; } case PID_PARAMETER: { diff --git a/src/dcaitirobot/embedded/src/communication/UCommands.cpp b/src/dcaitirobot/embedded/src/communication/UCommands.cpp index ac675ed..87f9761 100644 --- a/src/dcaitirobot/embedded/src/communication/UCommands.cpp +++ b/src/dcaitirobot/embedded/src/communication/UCommands.cpp @@ -1,5 +1,7 @@ #include "communication/UCommands.hpp" #include "misc/Utils.hpp" +#include "MotorWheel.h" + extern MotorWheel wheelLeft; extern MotorWheel wheelRight; diff --git a/src/dcaitirobot/embedded/src/communication/UValue.cpp b/src/dcaitirobot/embedded/src/communication/UValue.cpp index 05594df..dbbc4bd 100644 --- a/src/dcaitirobot/embedded/src/communication/UValue.cpp +++ b/src/dcaitirobot/embedded/src/communication/UValue.cpp @@ -19,20 +19,24 @@ void UValue::sendEncoderValue (U_Component cp, float value) UARTCom::send(VALUE, 4, cp, switched_b); } -void UValue::sendBothEncoderValues (float value_right, float value_left) +void UValue::sendBothEncoderValues (float speed_right, float speed_left, float pos_right, float pos_left) { union { - float f[2]; - uint8_t b[8]; + float f[4]; + uint8_t b[16]; } u; - u.f[0] = value_right; - u.f[1] = value_left; + u.f[0] = speed_right; + u.f[1] = speed_left; + u.f[2] = pos_right; + u.f[3] = pos_left; - uint8_t switched_byte_array[8]; + uint8_t switched_byte_array[16]; for (int i = 0; i < 4; i++) { switched_byte_array[i] = u.b[3 - i]; switched_byte_array[i+4] = u.b[7 - i]; + switched_byte_array[i+8] = u.b[11 - i]; + switched_byte_array[i+12] = u.b[15 - i]; } - UARTCom::send(VALUE, 8, ALL_MOTORS, switched_byte_array); + UARTCom::send(VALUE, 16, ALL_MOTORS, switched_byte_array); } \ No newline at end of file diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h b/src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h index 21e4ff6..7a8ef9d 100644 --- a/src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h +++ b/src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h @@ -34,11 +34,11 @@ public: void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms); void sendEmptyMsg(); - void readEncoderValues(double &val_1, double &val_2); + void readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos); void setMotorValues(float val_1, float val_2); void setPidValues(float k_p, float k_d, float k_i, float k_o); const std::vector buildMsg(uint8_t frame_type, uint8_t frame_component, long time, std::vector payload); - void readSpeed(double &left_speed, double &right_speed); + void readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos); bool connected() const { return serial_conn_.isOpen(); } diff --git a/src/diffdrive_arduino/src/arduino_comms.cpp b/src/diffdrive_arduino/src/arduino_comms.cpp index fdeb06d..803f0ca 100644 --- a/src/diffdrive_arduino/src/arduino_comms.cpp +++ b/src/diffdrive_arduino/src/arduino_comms.cpp @@ -21,12 +21,12 @@ void ArduinoComms::sendEmptyMsg() { } -void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed) +void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos) { std::vector payload = {DRIVE_ENCODER}; std::vector msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload); sendMsg(msg); - readSpeed(left_speed, right_speed); + readSpeedandPosition(left_speed, right_speed, left_pos, right_pos); } void ArduinoComms::setMotorValues(float left_speed, float right_speed) @@ -49,7 +49,7 @@ void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o) ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r"; } -void ArduinoComms::readSpeed(double &left_speed, double &right_speed) +void ArduinoComms::readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos) { std::vector buffer; int read_bytes = 0; @@ -91,13 +91,21 @@ void ArduinoComms::readSpeed(double &left_speed, double &right_speed) if (frame_type == VALUE && frame_component == BOTH_MOTORS) { std::vector left_speed_bytes(payload.begin(), payload.begin() + 4); - std::vector right_speed_bytes(payload.begin() + 4, payload.end()); + std::vector right_speed_bytes(payload.begin() + 4, payload.begin()+8); + std::vector left_pos_bytes(payload.begin() + 8, payload.begin()+12); + std::vector right_pos_bytes(payload.begin() + 12, payload.end()); std::reverse(left_speed_bytes.begin(), left_speed_bytes.end()); std::reverse(right_speed_bytes.begin(), right_speed_bytes.end()); + std::reverse(left_pos_bytes.begin(), left_pos_bytes.end()); + std::reverse(right_pos_bytes.begin(), right_pos_bytes.end()); float left_speed_float = *reinterpret_cast(&left_speed_bytes[0]); float right_speed_float = *reinterpret_cast(&right_speed_bytes[0]); + float left_pos_float = *reinterpret_cast(&left_pos_bytes[0]); + float right_pos_float = *reinterpret_cast(&right_pos_bytes[0]); left_speed = left_speed_float; right_speed = right_speed_float; + left_pos = left_pos_float; + right_pos = right_pos_float; } } diff --git a/src/diffdrive_arduino/src/diffdrive_arduino.cpp b/src/diffdrive_arduino/src/diffdrive_arduino.cpp index b80283e..5154079 100644 --- a/src/diffdrive_arduino/src/diffdrive_arduino.cpp +++ b/src/diffdrive_arduino/src/diffdrive_arduino.cpp @@ -7,6 +7,11 @@ #include "rclcpp/rclcpp.hpp" #include +#define RADSTORPM(a)(a*60.0/(2.0*M_PI)) +#define RPMSTORADS(a)(a*2.0*M_PI/60.0) +#define REVSTORAD(a)(a*2.0*M_PI) +#define RADSTOREV(a)(a/(2.0*M_PI)) + namespace diffdrive_arduino { @@ -106,18 +111,15 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp return return_type::ERROR; } - arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel); - // write l_wheel_.cmd to string and print via RCLCPP_INFO - std::string msg = "Left Wheel Read: " + std::to_string(l_wheel_.vel); - RCLCPP_INFO(logger_, msg.c_str()); + arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos); // convert rpm to rad/s - l_wheel_.vel = (l_wheel_.vel / 60) * 2 * M_PI * 0.049 ; - r_wheel_.vel = (r_wheel_.vel / 60) * 2 * M_PI * 0.049 ; + l_wheel_.vel = RPMSTORADS(l_wheel_.vel); + r_wheel_.vel = RPMSTORADS(r_wheel_.vel); - // Force the wheel position - l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds; - r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds; + // convert rev to rad + l_wheel_.pos = REVSTORAD(l_wheel_.pos); + r_wheel_.pos = REVSTORAD(r_wheel_.pos); return return_type::OK; } @@ -128,9 +130,7 @@ return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcp return return_type::ERROR; } // convert rad/s to rpm and write to motors - arduino_.setMotorValues(l_wheel_.cmd * 30 / M_PI , r_wheel_.cmd * 30 / M_PI ); - std::string msg = "Left Wheel Command: " + std::to_string(l_wheel_.cmd* 30 / M_PI); - RCLCPP_INFO(logger_, msg.c_str()); + arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd)); return return_type::OK; }