From ebb17be5af97421d52a44bf746f270fa9d11848b Mon Sep 17 00:00:00 2001 From: Tim Korjakow Date: Thu, 22 Jun 2023 21:03:35 +0200 Subject: [PATCH] Fixed real <> sim transfer --- src/dcaiti_control/config/dcaiti_config.yml | 10 ++++---- .../description/real_robot_control.xacro | 9 ++++--- .../description/robot_core.xacro | 24 +++++++++---------- .../include/communication/UARTCom.hpp | 1 - .../embedded/src/communication/UARTCom.cpp | 5 ++-- .../embedded/src/communication/UCommands.cpp | 24 ++++++++++++------- src/dcaitirobot/embedded/src/main.cpp | 9 ++++--- .../include/diffdrive_arduino/wheel.h | 5 ++-- src/diffdrive_arduino/src/arduino_comms.cpp | 1 + .../src/diffdrive_arduino.cpp | 23 +++++++++++++----- src/diffdrive_arduino/src/fake_robot.cpp | 4 ++-- src/diffdrive_arduino/src/wheel.cpp | 12 +++------- 12 files changed, 68 insertions(+), 59 deletions(-) diff --git a/src/dcaiti_control/config/dcaiti_config.yml b/src/dcaiti_control/config/dcaiti_config.yml index e72efb9..2a07f76 100644 --- a/src/dcaiti_control/config/dcaiti_config.yml +++ b/src/dcaiti_control/config/dcaiti_config.yml @@ -17,12 +17,12 @@ diff_cont: left_wheel_names: ['left_wheel_joint'] right_wheel_names: ['right_wheel_joint'] wheel_separation: 0.215 - wheel_radius: 0.0477464829 + wheel_radius: 0.049338032 - linear: - x: - max_acceleration: 0.05 - has_acceleration_limits: true +# linear: +# x: +# max_acceleration: 0.15 +# has_acceleration_limits: true use_stamped_vel: false diff --git a/src/dcaiti_control/description/real_robot_control.xacro b/src/dcaiti_control/description/real_robot_control.xacro index fbf6f23..18e52cc 100644 --- a/src/dcaiti_control/description/real_robot_control.xacro +++ b/src/dcaiti_control/description/real_robot_control.xacro @@ -9,21 +9,20 @@ /dev/ttyUSB0 57600 1000 - 3436 - -0.005 - 0.005 + -0.5 + 0.5 - -0.005 - 0.005 + -0.5 + 0.5 diff --git a/src/dcaiti_control/description/robot_core.xacro b/src/dcaiti_control/description/robot_core.xacro index fd4ed23..e7f1ec8 100644 --- a/src/dcaiti_control/description/robot_core.xacro +++ b/src/dcaiti_control/description/robot_core.xacro @@ -41,17 +41,17 @@ - + - + - + @@ -73,16 +73,16 @@ - + - + - + @@ -110,16 +110,16 @@ - + - + - + @@ -142,16 +142,16 @@ - + - + - + diff --git a/src/dcaitirobot/embedded/include/communication/UARTCom.hpp b/src/dcaitirobot/embedded/include/communication/UARTCom.hpp index bb64930..50ae844 100644 --- a/src/dcaitirobot/embedded/include/communication/UARTCom.hpp +++ b/src/dcaitirobot/embedded/include/communication/UARTCom.hpp @@ -14,7 +14,6 @@ #include "Arduino.h" #include "constants.h" -#include "R2WD.h" class UARTCom { private: diff --git a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp b/src/dcaitirobot/embedded/src/communication/UARTCom.cpp index 975ba38..358d3bf 100644 --- a/src/dcaitirobot/embedded/src/communication/UARTCom.cpp +++ b/src/dcaitirobot/embedded/src/communication/UARTCom.cpp @@ -8,7 +8,8 @@ uint8_t UARTCom::data[MAX_PACKET_LENGTH]; uint8_t currentParseIndex; int payloadLength; uint32_t currentTimestamp = 0; -extern R2WD _2WD; +extern MotorWheel wheelLeft; +extern MotorWheel wheelRight; /** * @brief extracts frametype from data package @@ -169,7 +170,7 @@ void handleRequest (uint8_t* d) break; } case DRIVE_ENCODER: { - UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0); + UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM()); break; } case PID_PARAMETER: { diff --git a/src/dcaitirobot/embedded/src/communication/UCommands.cpp b/src/dcaitirobot/embedded/src/communication/UCommands.cpp index 17201f0..ac675ed 100644 --- a/src/dcaitirobot/embedded/src/communication/UCommands.cpp +++ b/src/dcaitirobot/embedded/src/communication/UCommands.cpp @@ -1,7 +1,7 @@ #include "communication/UCommands.hpp" #include "misc/Utils.hpp" -#include "R2WD.h" -extern R2WD _2WD; +extern MotorWheel wheelLeft; +extern MotorWheel wheelRight; void UCommands::setSetpoint (uint8_t* d, U_Component c) { @@ -15,16 +15,17 @@ void UCommands::setSetpoint (uint8_t* d, U_Component c) d[COMMAND_POS + 6], d[COMMAND_POS + 7], d[COMMAND_POS + 8]); - _2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE); - _2WD.wheelRightSetSpeedMMPS(abs(speed2*1000), speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF); + wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE); + wheelRight.setGearedRPM(speed2, speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF); + break; } case RIGHT_MOTOR: { - _2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE); + wheelRight.setGearedRPM(speed1, speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF); break; } case LEFT_MOTOR: { - _2WD.wheelRightSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF); + wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE); break; } default: { @@ -47,17 +48,22 @@ void UCommands::setPIDParameter (uint8_t* d, U_Component c) d[COMMAND_POS + 10], d[COMMAND_POS + 11], d[COMMAND_POS + 12]); + int T = Utils::bytesToInt(d[COMMAND_POS + 9], + d[COMMAND_POS + 10], + d[COMMAND_POS + 11], + d[COMMAND_POS + 12]); switch (c) { case ALL_MOTORS: { - _2WD.PIDTune(P,I,D,10); + wheelLeft.PIDSetup(P,I,D,T); + wheelRight.PIDSetup(P,I,D,T); break; } case RIGHT_MOTOR: { - _2WD.wheelLeftPIDTune(P,I,D,10); + wheelLeft.PIDSetup(P,I,D,T); break; } case LEFT_MOTOR: { - _2WD.wheelRightPIDTune(P,I,D,10); + wheelRight.PIDSetup(P,I,D,T); break; } default: { diff --git a/src/dcaitirobot/embedded/src/main.cpp b/src/dcaitirobot/embedded/src/main.cpp index cabab23..814d0c3 100644 --- a/src/dcaitirobot/embedded/src/main.cpp +++ b/src/dcaitirobot/embedded/src/main.cpp @@ -10,7 +10,6 @@ int speed = 0; #include #include #include -#include #ifndef MICROS_PER_SEC #define MICROS_PER_SEC 1000000 #endif @@ -18,14 +17,13 @@ irqISR(irq1, isr1); irqISR(irq2, isr2); MotorWheel wheelLeft(9, 8, 4, 5, &irq1, REDUCTION_RATIO, 300); // Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7 MotorWheel wheelRight(10, 11, 6, 7, &irq2, REDUCTION_RATIO, 300); // Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7 -R2WD _2WD(&wheelLeft, &wheelRight, WHEELSPAN); void setup(void) { TCCR1B = TCCR1B & 0xf8 | 0x01; // Pin9,Pin10 PWM 31250Hz, Silent PWM - _2WD.PIDEnable(0.26,0.02,0.02,10);// Enable PID + wheelLeft.PIDEnable(0.26,0.02,0.02,10);// Enable PID + wheelRight.PIDEnable(0.26,0.02,0.02,10);// Enable PID - //_2WD.PIDEnable(KC, TAUI, TAUD, 10); UARTCom::init(UARTCom::data); controlTimer = millis(); uartTransmitTimer = millis(); @@ -38,6 +36,7 @@ void setup(void) void loop(void) { - _2WD.PIDRegulate(); + wheelLeft.PIDRegulate(); + wheelRight.PIDRegulate(); UARTCom::read(UARTCom::data); } \ No newline at end of file diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h b/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h index 712f25e..3c9deab 100644 --- a/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h +++ b/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h @@ -16,13 +16,12 @@ class Wheel double vel = 0; double eff = 0; double velSetPt = 0; - double rads_per_count = 0; Wheel() = default; - Wheel(const std::string &wheel_name, int counts_per_rev); + Wheel(const std::string &wheel_name); - void setup(const std::string &wheel_name, int counts_per_rev); + void setup(const std::string &wheel_name); double calcEncAngle(); diff --git a/src/diffdrive_arduino/src/arduino_comms.cpp b/src/diffdrive_arduino/src/arduino_comms.cpp index 5d03e04..fdeb06d 100644 --- a/src/diffdrive_arduino/src/arduino_comms.cpp +++ b/src/diffdrive_arduino/src/arduino_comms.cpp @@ -31,6 +31,7 @@ void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed) void ArduinoComms::setMotorValues(float left_speed, float right_speed) { + std::vector payload = {DRIVE_MOTOR}; auto bytes_left_speed = marshalling(left_speed); payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end()); diff --git a/src/diffdrive_arduino/src/diffdrive_arduino.cpp b/src/diffdrive_arduino/src/diffdrive_arduino.cpp index ea8192c..b80283e 100644 --- a/src/diffdrive_arduino/src/diffdrive_arduino.cpp +++ b/src/diffdrive_arduino/src/diffdrive_arduino.cpp @@ -5,6 +5,7 @@ #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/rclcpp.hpp" +#include namespace diffdrive_arduino { @@ -31,11 +32,10 @@ CallbackReturn DiffDriveArduino::on_init(const hardware_interface::HardwareInfo cfg_.device = info_.hardware_parameters["device"]; cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]); cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]); - cfg_.enc_counts_per_rev = std::stoi(info_.hardware_parameters["enc_counts_per_rev"]); // Set up the wheels - l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); - r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); + l_wheel_.setup(cfg_.left_wheel_name); + r_wheel_.setup(cfg_.right_wheel_name); // Set up the Arduino arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout); @@ -106,9 +106,16 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp return return_type::ERROR; } - arduino_.readEncoderValues(l_wheel_.vel , r_wheel_.vel); + 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()); + // 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 ; - // Force the wheel position + + // Force the wheel position l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds; r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds; return return_type::OK; @@ -120,7 +127,11 @@ return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcp { return return_type::ERROR; } - arduino_.setMotorValues(l_wheel_.cmd, r_wheel_.cmd); + // 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()); + return return_type::OK; } diff --git a/src/diffdrive_arduino/src/fake_robot.cpp b/src/diffdrive_arduino/src/fake_robot.cpp index 6794043..979259a 100644 --- a/src/diffdrive_arduino/src/fake_robot.cpp +++ b/src/diffdrive_arduino/src/fake_robot.cpp @@ -26,8 +26,8 @@ CallbackReturn FakeRobot::on_init(const hardware_interface::HardwareInfo & info) // Note: It doesn't matter that we haven't set encoder counts per rev // since the fake robot bypasses the encoder code completely - l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); - r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); + l_wheel_.setup(cfg_.left_wheel_name); + r_wheel_.setup(cfg_.right_wheel_name); RCLCPP_INFO(logger_, "Finished Configuration"); diff --git a/src/diffdrive_arduino/src/wheel.cpp b/src/diffdrive_arduino/src/wheel.cpp index 60d8ede..e18884b 100644 --- a/src/diffdrive_arduino/src/wheel.cpp +++ b/src/diffdrive_arduino/src/wheel.cpp @@ -3,19 +3,13 @@ #include -Wheel::Wheel(const std::string &wheel_name, int counts_per_rev) +Wheel::Wheel(const std::string &wheel_name) { - setup(wheel_name, counts_per_rev); + setup(wheel_name); } -void Wheel::setup(const std::string &wheel_name, int counts_per_rev) +void Wheel::setup(const std::string &wheel_name) { name = wheel_name; - rads_per_count = (2*M_PI)/counts_per_rev; -} - -double Wheel::calcEncAngle() -{ - return enc * rads_per_count; } \ No newline at end of file