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']
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

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

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

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

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

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

@ -14,7 +14,6 @@
#include "Arduino.h"
#include "constants.h"
#include "R2WD.h"
class UARTCom {
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;
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: {

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

@ -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: {

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

@ -10,7 +10,6 @@ int speed = 0;
#include <PinChangeIntConfig.h>
#include <PID_Beta6.h>
#include <MotorWheel.h>
#include <R2WD.h>
#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);
}

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

@ -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();

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)
{
std::vector<uint8_t> payload = {DRIVE_MOTOR};
auto bytes_left_speed = marshalling(left_speed);
payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end());

23
src/diffdrive_arduino/src/diffdrive_arduino.cpp
View File

@ -5,6 +5,7 @@
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include <math.h>
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;
}

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
// 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");

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

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