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
parent ca6945a2de 4d41485c47
commit a3f169ecb4
15 changed files with 196 additions and 96 deletions
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;
}