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

Merge pull request #1 from dcaiti-robot/main

Browse Source 
updating branch
This commit is contained in:
dcaiti-robot 2023-07-06 17:32:30 +02:00 committed by GitHub
commit a3f169ecb4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 196 additions and 96 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>

94
src/dcaitirobot/dcaitirobot/twist.py
View File

@ -7,54 +7,98 @@ from sensor_msgs.msg import Joy
import numpy as np
SCALE_LINEAR_VELOCITY = 0.5
SCALE_ANGULAR_VELOCITY = 0.4
PUBLISH_FREQUENCY_HZ = 10
SCALE_LINEAR_VELOCITY = 0.1
SCALE_ANGULAR_VELOCITY = 0.1
PUBLISH_FREQUENCY_HZ = 30
SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
def calc_speed_from_joy(x, y):
linear = -y * SCALE_LINEAR_VELOCITY
angular = x * SCALE_ANGULAR_VELOCITY
return linear, angular
class TwistPublisher(Node):
def __init__(
self,
smooth_weights=np.ones(5),
linear_scale=1.0,
angular_scale=1.0,
publish_frequency_hz=30,
):
super().__init__("twist_publisher")
self.linear_window = np.zeros(smooth_weights.shape[0])
self.angular_window = np.zeros(smooth_weights.shape[0])
self.linear_recent = 0.0
self.angular_recent = 0.0
self.linear_out = 0.0
self.angular_out = 0.0
self.weights = smooth_weights / smooth_weights.sum()
self.window_size = smooth_weights.shape[0]
self.linear_scale = linear_scale
self.angular_scale = angular_scale
self.publisher_ = self.create_publisher(
Twist, "/diff_cont/cmd_vel_unstamped", 1
)
self.publisher_2 = self.create_publisher(Twist, "/cmd_vel", 1)
def __init__(self):
super().__init__('twist_publisher')
self.linear = 0.0
self.angular = 0.0
self.publisher_ = self.create_publisher(Twist, '/diff_cont/cmd_vel_unstamped', 1)
self.create_subscription(Joy, "/joy", self.joy_callback, 1)
self.timer = self.create_timer(1/PUBLISH_FREQUENCY_HZ, self.timer_callback)
self.timer = self.create_timer(1 / publish_frequency_hz, self.timer_callback)
def joy_callback(self, joy_message):
x, y = joy_message.axes[:2]
self.linear_recent = -y * SCALE_LINEAR_VELOCITY
self.angular_recent = x * SCALE_ANGULAR_VELOCITY
self.linear, self.angular = calc_speed_from_joy(x,y)
def update_smooth_window(self):
self.linear_window = np.append(self.linear_window, self.linear_recent)[
-self.window_size :
]
self.angular_window = np.append(self.angular_window, self.angular_recent)[
-self.window_size :
]
def calc_smooth_speed(self):
self.linear_out = np.average(self.linear_window, weights=self.weights)
self.angular_out = np.average(self.angular_window, weights=self.weights)
def timer_callback(self):
msg = Twist()
msg.linear.x = self.linear
msg.angular.z = self.angular
self.update_smooth_window()
self.calc_smooth_speed()
msg.linear.x = self.linear_out
msg.angular.z = self.angular_out
self.publisher_.publish(msg)
self.get_logger().info('Publishing: "%s"' % msg)
self.publisher_2.publish(msg)
self.get_logger().info(
"Linear_window: {}, Linear_out: {}, Angular_window: {}, Angular_out: {}".format(
self.linear_window,
self.linear_out,
self.angular_window,
self.angular_out,
)
)
def main(args=None):
rclpy.init(args=args)
minimal_publisher = TwistPublisher()
minimal_publisher = TwistPublisher(
SMOOTH_WEIGHTS,
SCALE_LINEAR_VELOCITY,
SCALE_ANGULAR_VELOCITY,
PUBLISH_FREQUENCY_HZ,
)
rclpy.spin(minimal_publisher)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
minimal_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
if __name__ == "__main__":
main()

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);
}

27
src/dcaitirobot/launch/remote_control_launch.py
View File

@ -1,16 +1,19 @@
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='dcaitirobot',
executable='joy',
name='joystick',
),
Node(
package='dcaitirobot',
executable='twistcalc',
name='twistcalc',
)
])
return LaunchDescription(
[
Node(
package="dcaitirobot",
executable="joy",
name="joystick",
),
Node(
package="dcaitirobot",
executable="twistcalc",
name="twistcalc",
),
]
)

44
src/dcaitirobot/launch/start_turtle_slam_sim.py Normal file
View File

@ -0,0 +1,44 @@
from pathlib import Path
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python import get_package_share_directory
def generate_launch_description():
return LaunchDescription(
[
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
str(Path(__file__).parent.absolute() / "remote_control_launch.py")
)
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
str(
(
Path(
get_package_share_directory("turtlebot3_gazebo")
).absolute()
/ "launch"
/ "turtlebot3_world.launch.py"
)
)
)
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
str(
(
Path(get_package_share_directory("slam_toolbox")).absolute()
/ "launch"
/ "online_async_launch.py"
)
)
),
launch_arguments=[("use_sim_time", "True")],
),
]
)

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

@ -30,6 +30,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()
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()
{
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 Fakerobot");

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;
}