t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed real <> sim transfer

Browse Source
This commit is contained in:
Tim Korjakow 2023-06-22 21:03:35 +02:00
parent 951058e644
commit ebb17be5af
12 changed files with 68 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/dcaiti_control/config/dcaiti_config.yml
View File

@ -17,12 +17,12 @@ diff_cont:
left_wheel_names: ['left_wheel_joint'] left_wheel_names: ['left_wheel_joint']
right_wheel_names: ['right_wheel_joint'] right_wheel_names: ['right_wheel_joint']
wheel_separation: 0.215 wheel_separation: 0.215
wheel_radius: 0.0477464829 wheel_radius: 0.049338032
linear: # linear:
x: # x:
max_acceleration: 0.05 # max_acceleration: 0.15
has_acceleration_limits: true # has_acceleration_limits: true
use_stamped_vel: false use_stamped_vel: false

9
src/dcaiti_control/description/real_robot_control.xacro
View File

@ -9,21 +9,20 @@
<param name="device">/dev/ttyUSB0</param> <param name="device">/dev/ttyUSB0</param>
<param name="baud_rate">57600</param> <param name="baud_rate">57600</param>
<param name="timeout">1000</param> <param name="timeout">1000</param>
<param name="enc_counts_per_rev">3436</param>
</hardware> </hardware>
<!-- Note everything below here is the same as the Gazebo one --> <!-- Note everything below here is the same as the Gazebo one -->
<joint name="left_wheel_joint"> <joint name="left_wheel_joint">
<command_interface name="velocity"> <command_interface name="velocity">
<param name="min">-0.005</param> <param name="min">-0.5</param>
<param name="max">0.005</param> <param name="max">0.5</param>
</command_interface> </command_interface>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="position"/> <state_interface name="position"/>
</joint> </joint>
<joint name="right_wheel_joint"> <joint name="right_wheel_joint">
<command_interface name="velocity"> <command_interface name="velocity">
<param name="min">-0.005</param> <param name="min">-0.5</param>
<param name="max">0.005</param> <param name="max">0.5</param>
</command_interface> </command_interface>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="position"/> <state_interface name="position"/>

24
src/dcaiti_control/description/robot_core.xacro
View File

@ -41,17 +41,17 @@
<link name="left_wheel"> <link name="left_wheel">
<visual> <visual>
<geometry> <geometry>
<cylinder radius="${0.3/(2*pi)}" length="0.04"/> <cylinder radius="${0.31/(2*pi)}" length="0.04"/>
</geometry> </geometry>
<material name="blue"/> <material name="blue"/>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<!-- Better collosion behaviour than a cylinder --> <!-- Better collosion behaviour than a cylinder -->
<sphere radius="${0.3/(2*pi)}"/> <sphere radius="${0.31/(2*pi)}"/>
</geometry> </geometry>
</collision> </collision>
<xacro:inertial_cylinder mass="0.1" radius="${0.3/(2*pi)}" length="0.04"> <xacro:inertial_cylinder mass="0.1" radius="${0.31/(2*pi)}" length="0.04">
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder> </xacro:inertial_cylinder>
</link> </link>
@ -73,16 +73,16 @@
<visual> <visual>
<geometry> <geometry>
<!-- Better collosion behaviour than a cylinder --> <!-- Better collosion behaviour than a cylinder -->
<cylinder radius="${0.3/(2*pi)}" length="0.04"/> <cylinder radius="${0.31/(2*pi)}" length="0.04"/>
</geometry> </geometry>
<material name="blue"/> <material name="blue"/>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<sphere radius="${0.3/(2*pi)}" /> <sphere radius="${0.31/(2*pi)}" />
</geometry> </geometry>
</collision> </collision>
<xacro:inertial_cylinder mass="0.1" radius="${0.3/(2*pi)}" length="0.04"> <xacro:inertial_cylinder mass="0.1" radius="${0.31/(2*pi)}" length="0.04">
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder> </xacro:inertial_cylinder>
</link> </link>
@ -110,16 +110,16 @@
<link name="caster_wheel_front"> <link name="caster_wheel_front">
<visual> <visual>
<geometry> <geometry>
<sphere radius="${0.3/(2*pi)}"/> <sphere radius="${0.31/(2*pi)}"/>
</geometry> </geometry>
<material name="black"/> <material name="black"/>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<sphere radius="${0.3/(2*pi)}"/> <sphere radius="${0.31/(2*pi)}"/>
</geometry> </geometry>
</collision> </collision>
<xacro:inertial_sphere mass="0.1" radius="${0.3/(2*pi)}"> <xacro:inertial_sphere mass="0.1" radius="${0.31/(2*pi)}">
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_sphere> </xacro:inertial_sphere>
</link> </link>
@ -142,16 +142,16 @@
<link name="caster_wheel_back"> <link name="caster_wheel_back">
<visual> <visual>
<geometry> <geometry>
<sphere radius="${0.3/(2*pi)}"/> <sphere radius="${0.31/(2*pi)}"/>
</geometry> </geometry>
<material name="black"/> <material name="black"/>
</visual> </visual>
<collision> <collision>
<geometry> <geometry>
<sphere radius="${0.3/(2*pi)}"/> <sphere radius="${0.31/(2*pi)}"/>
</geometry> </geometry>
</collision> </collision>
<xacro:inertial_sphere mass="0.1" radius="${0.3/(2*pi)}"> <xacro:inertial_sphere mass="0.1" radius="${0.31/(2*pi)}">
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_sphere> </xacro:inertial_sphere>
</link> </link>

1
src/dcaitirobot/embedded/include/communication/UARTCom.hpp
View File

@ -14,7 +14,6 @@
#include "Arduino.h" #include "Arduino.h"
#include "constants.h" #include "constants.h"
#include "R2WD.h"
class UARTCom { class UARTCom {
private: private:

5
src/dcaitirobot/embedded/src/communication/UARTCom.cpp
View File

@ -8,7 +8,8 @@ uint8_t UARTCom::data[MAX_PACKET_LENGTH];
uint8_t currentParseIndex; uint8_t currentParseIndex;
int payloadLength; int payloadLength;
uint32_t currentTimestamp = 0; uint32_t currentTimestamp = 0;
extern R2WD _2WD; extern MotorWheel wheelLeft;
extern MotorWheel wheelRight;
/** /**
* @brief extracts frametype from data package * @brief extracts frametype from data package
@ -169,7 +170,7 @@ void handleRequest (uint8_t* d)
break; break;
} }
case DRIVE_ENCODER: { case DRIVE_ENCODER: {
UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0); UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM());
break; break;
} }
case PID_PARAMETER: { case PID_PARAMETER: {

24
src/dcaitirobot/embedded/src/communication/UCommands.cpp
View File

@ -1,7 +1,7 @@
#include "communication/UCommands.hpp" #include "communication/UCommands.hpp"
#include "misc/Utils.hpp" #include "misc/Utils.hpp"
#include "R2WD.h" extern MotorWheel wheelLeft;
extern R2WD _2WD; extern MotorWheel wheelRight;
void UCommands::setSetpoint (uint8_t* d, U_Component c) 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 + 6],
d[COMMAND_POS + 7], d[COMMAND_POS + 7],
d[COMMAND_POS + 8]); d[COMMAND_POS + 8]);
_2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE); wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
_2WD.wheelRightSetSpeedMMPS(abs(speed2*1000), speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF); wheelRight.setGearedRPM(speed2, speed2 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
break; break;
} }
case RIGHT_MOTOR: { case RIGHT_MOTOR: {
_2WD.wheelLeftSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE); wheelRight.setGearedRPM(speed1, speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF);
break; break;
} }
case LEFT_MOTOR: { case LEFT_MOTOR: {
_2WD.wheelRightSetSpeedMMPS(abs(speed1*1000), speed1 > 0 ? DIR_ADVANCE : DIR_BACKOFF); wheelLeft.setGearedRPM(speed1, speed1 > 0 ? DIR_BACKOFF : DIR_ADVANCE);
break; break;
} }
default: { default: {
@ -47,17 +48,22 @@ void UCommands::setPIDParameter (uint8_t* d, U_Component c)
d[COMMAND_POS + 10], d[COMMAND_POS + 10],
d[COMMAND_POS + 11], d[COMMAND_POS + 11],
d[COMMAND_POS + 12]); 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) { switch (c) {
case ALL_MOTORS: { case ALL_MOTORS: {
_2WD.PIDTune(P,I,D,10); wheelLeft.PIDSetup(P,I,D,T);
wheelRight.PIDSetup(P,I,D,T);
break; break;
} }
case RIGHT_MOTOR: { case RIGHT_MOTOR: {
_2WD.wheelLeftPIDTune(P,I,D,10); wheelLeft.PIDSetup(P,I,D,T);
break; break;
} }
case LEFT_MOTOR: { case LEFT_MOTOR: {
_2WD.wheelRightPIDTune(P,I,D,10); wheelRight.PIDSetup(P,I,D,T);
break; break;
} }
default: { default: {

9
src/dcaitirobot/embedded/src/main.cpp
View File

@ -10,7 +10,6 @@ int speed = 0;
#include <PinChangeIntConfig.h> #include <PinChangeIntConfig.h>
#include <PID_Beta6.h> #include <PID_Beta6.h>
#include <MotorWheel.h> #include <MotorWheel.h>
#include <R2WD.h>
#ifndef MICROS_PER_SEC #ifndef MICROS_PER_SEC
#define MICROS_PER_SEC 1000000 #define MICROS_PER_SEC 1000000
#endif #endif
@ -18,14 +17,13 @@ irqISR(irq1, isr1);
irqISR(irq2, isr2); irqISR(irq2, isr2);
MotorWheel wheelLeft(9, 8, 4, 5, &irq1, REDUCTION_RATIO, 300); // Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7 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 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) void setup(void)
{ {
TCCR1B = TCCR1B & 0xf8 | 0x01; // Pin9,Pin10 PWM 31250Hz, Silent PWM 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); UARTCom::init(UARTCom::data);
controlTimer = millis(); controlTimer = millis();
uartTransmitTimer = millis(); uartTransmitTimer = millis();
@ -38,6 +36,7 @@ void setup(void)
void loop(void) void loop(void)
{ {
_2WD.PIDRegulate(); wheelLeft.PIDRegulate();
wheelRight.PIDRegulate();
UARTCom::read(UARTCom::data); UARTCom::read(UARTCom::data);
} }

5
src/diffdrive_arduino/include/diffdrive_arduino/wheel.h
View File

@ -16,13 +16,12 @@ class Wheel
double vel = 0; double vel = 0;
double eff = 0; double eff = 0;
double velSetPt = 0; double velSetPt = 0;
double rads_per_count = 0;
Wheel() = default; 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(); double calcEncAngle();

1
src/diffdrive_arduino/src/arduino_comms.cpp
View File

@ -31,6 +31,7 @@ void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed)
void ArduinoComms::setMotorValues(float left_speed, float right_speed) void ArduinoComms::setMotorValues(float left_speed, float right_speed)
{ {
std::vector<uint8_t> payload = {DRIVE_MOTOR}; std::vector<uint8_t> payload = {DRIVE_MOTOR};
auto bytes_left_speed = marshalling(left_speed); auto bytes_left_speed = marshalling(left_speed);
payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end()); payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end());

19
src/diffdrive_arduino/src/diffdrive_arduino.cpp
View File

@ -5,6 +5,7 @@
#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include <math.h>
namespace diffdrive_arduino namespace diffdrive_arduino
{ {
@ -31,11 +32,10 @@ CallbackReturn DiffDriveArduino::on_init(const hardware_interface::HardwareInfo
cfg_.device = info_.hardware_parameters["device"]; cfg_.device = info_.hardware_parameters["device"];
cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]); cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]);
cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]); 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 // Set up the wheels
l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); l_wheel_.setup(cfg_.left_wheel_name);
r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); r_wheel_.setup(cfg_.right_wheel_name);
// Set up the Arduino // Set up the Arduino
arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout); arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout);
@ -107,6 +107,13 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp
} }
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; l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
@ -120,7 +127,11 @@ return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcp
{ {
return return_type::ERROR; 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; return return_type::OK;
} }

4
src/diffdrive_arduino/src/fake_robot.cpp
View File

@ -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 // Note: It doesn't matter that we haven't set encoder counts per rev
// since the fake robot bypasses the encoder code completely // since the fake robot bypasses the encoder code completely
l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); l_wheel_.setup(cfg_.left_wheel_name);
r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); r_wheel_.setup(cfg_.right_wheel_name);
RCLCPP_INFO(logger_, "Finished Configuration"); RCLCPP_INFO(logger_, "Finished Configuration");

12
src/diffdrive_arduino/src/wheel.cpp
View File

@ -3,19 +3,13 @@
#include <cmath> #include <cmath>
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; name = wheel_name;
rads_per_count = (2*M_PI)/counts_per_rev;
}
double Wheel::calcEncAngle()
{
return enc * rads_per_count;
} }