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/dcaitirobot/twist.py b/src/dcaitirobot/dcaitirobot/twist.py
index 7fd9f5a..5cbcbbb 100644
--- a/src/dcaitirobot/dcaitirobot/twist.py
+++ b/src/dcaitirobot/dcaitirobot/twist.py
@@ -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()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
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/dcaitirobot/launch/remote_control_launch.py b/src/dcaitirobot/launch/remote_control_launch.py
index 193078a..6a5af93 100644
--- a/src/dcaitirobot/launch/remote_control_launch.py
+++ b/src/dcaitirobot/launch/remote_control_launch.py
@@ -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",
+ ),
+ ]
+ )
diff --git a/src/dcaitirobot/launch/start_turtle_slam_sim.py b/src/dcaitirobot/launch/start_turtle_slam_sim.py
new file mode 100644
index 0000000..3a2804f
--- /dev/null
+++ b/src/dcaitirobot/launch/start_turtle_slam_sim.py
@@ -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")],
+ ),
+ ]
+ )
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 a1353df..7ea7eb1 100644
--- a/src/diffdrive_arduino/src/arduino_comms.cpp
+++ b/src/diffdrive_arduino/src/arduino_comms.cpp
@@ -30,6 +30,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 a4fb54b..d94ee72 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()
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;
}
diff --git a/src/diffdrive_arduino/src/fake_robot.cpp b/src/diffdrive_arduino/src/fake_robot.cpp
index cbc89e3..67ed34b 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 Fakerobot");
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