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

Hopefully fixed wrong odometry

Browse Source
This commit is contained in:
Tim Korjakow 2023-07-10 13:22:02 +08:00
parent 09095001bc
commit a0a884065b
13 changed files with 76 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -13,18 +13,23 @@ diff_cont:
publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled
base_frame_id: base_link
publish_limited_velocity: true
left_wheel_names: ['left_wheel_joint']
right_wheel_names: ['right_wheel_joint']
wheel_separation: 0.215
wheel_radius: 0.049338032
# linear:
# x:
# max_acceleration: 0.15
# has_acceleration_limits: true
use_stamped_vel: false
# joint_broad:
# ros__parameters:
joint_limits:
ros__parameters:
left_wheel_joint:
has_position_limits: false # Continuous joint
has_velocity_limits: true
max_velocity: 10.0
right_wheel_joint:
has_position_limits: false # Continuous joint
has_velocity_limits: true
max_velocity: 10.0

1
src/dcaiti_control/config/empty.yaml
View File

@ -1 +0,0 @@
# This is an empty file, so that git commits the folder correctly

6
src/dcaiti_control/description/lidar.xacro
View File

@ -34,19 +34,19 @@
<sensor name="laser" type="ray">
<pose> 0 0 0 0 0 0 </pose>
<visualize>true</visualize>
<visualize>false</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<samples>360*5</samples>
<min_angle>-3.14</min_angle>
<max_angle>3.14</max_angle>
</horizontal>
</scan>
<range>
<min>0.3</min>
<max>25</max>
<max>100</max>
</range>
</ray>
<plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">

18
src/dcaiti_control/description/robot.urdf.xacro
View File

@ -5,6 +5,24 @@
<xacro:arg name="use_ros2_control" default="true"/>
<xacro:arg name="use_sim" default="false"/>
<xacro:arg name="tube_thickness" default="0.024"/>
<xacro:arg name="tube_radius" default="false"/>
<xacro:arg name="camera_height" default="0.201"/>
<xacro:arg name="camera_offset_x" default="0.016"/>
<xacro:arg name="lidar_height" default="0.201"/>
<xacro:arg name="wheel_radius" default="0.05"/>
<xacro:arg name="wheel_width" default="0.02"/>
<xacro:arg name="base_height" default="0.1"/>
<xacro:arg name="base_width" default="0.2"/>
<xacro:arg name="base_length" default="0.2"/>
<!-- Include the colours -->
<xacro:include filename="colours.xacro" />
<!-- Include the inertial macros -->

4
src/dcaiti_control/description/ros2_control.xacro
View File

@ -9,7 +9,7 @@
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<!-- <state_interface name="position" /> -->
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
<joint name="right_wheel_joint">
@ -18,7 +18,7 @@
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity" />
<!-- <state_interface name="position" /> -->
<state_interface name="position" />
</joint>
</ros2_control>
<gazebo>

2
src/dcaitirobot/embedded/include/communication/UValue.hpp
View File

@ -37,7 +37,7 @@ public:
* @param value_left encoder value left
* @param value_right encoder value right
*/
static void sendBothEncoderValues(float value_left, float value_right);
static void sendBothEncoderValues(float speed_right, float speed_left, float pos_right, float pos_left);
};
#endif

1
src/dcaitirobot/embedded/include/misc/Utils.hpp
View File

@ -13,7 +13,6 @@
*/
#include "Arduino.h"
#include "R2WD.h"
class Utils {
private:

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

@ -3,6 +3,7 @@
#include "communication/UValue.hpp"
#include "misc/Utils.hpp"
#include "constants.h"
#include "MotorWheel.h"
uint8_t UARTCom::data[MAX_PACKET_LENGTH];
uint8_t currentParseIndex;
@ -170,7 +171,7 @@ void handleRequest (uint8_t* d)
break;
}
case DRIVE_ENCODER: {
UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM());
UValue::sendBothEncoderValues(-wheelLeft.getGearedSpeedRPM(), wheelRight.getGearedSpeedRPM(), wheelLeft.getPosition(), wheelRight.getPosition());
break;
}
case PID_PARAMETER: {

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

@ -1,5 +1,7 @@
#include "communication/UCommands.hpp"
#include "misc/Utils.hpp"
#include "MotorWheel.h"
extern MotorWheel wheelLeft;
extern MotorWheel wheelRight;

18
src/dcaitirobot/embedded/src/communication/UValue.cpp
View File

@ -19,20 +19,24 @@ void UValue::sendEncoderValue (U_Component cp, float value)
UARTCom::send(VALUE, 4, cp, switched_b);
}
void UValue::sendBothEncoderValues (float value_right, float value_left)
void UValue::sendBothEncoderValues (float speed_right, float speed_left, float pos_right, float pos_left)
{
union {
float f[2];
uint8_t b[8];
float f[4];
uint8_t b[16];
} u;
u.f[0] = value_right;
u.f[1] = value_left;
u.f[0] = speed_right;
u.f[1] = speed_left;
u.f[2] = pos_right;
u.f[3] = pos_left;
uint8_t switched_byte_array[8];
uint8_t switched_byte_array[16];
for (int i = 0; i < 4; i++) {
switched_byte_array[i] = u.b[3 - i];
switched_byte_array[i+4] = u.b[7 - i];
switched_byte_array[i+8] = u.b[11 - i];
switched_byte_array[i+12] = u.b[15 - i];
}
UARTCom::send(VALUE, 8, ALL_MOTORS, switched_byte_array);
UARTCom::send(VALUE, 16, ALL_MOTORS, switched_byte_array);
}

4
src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h
View File

@ -34,11 +34,11 @@ public:
void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms);
void sendEmptyMsg();
void readEncoderValues(double &val_1, double &val_2);
void readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
void setMotorValues(float val_1, float val_2);
void setPidValues(float k_p, float k_d, float k_i, float k_o);
const std::vector<uint8_t> buildMsg(uint8_t frame_type, uint8_t frame_component, long time, std::vector<uint8_t> payload);
void readSpeed(double &left_speed, double &right_speed);
void readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos);
bool connected() const { return serial_conn_.isOpen(); }

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

@ -21,12 +21,12 @@ void ArduinoComms::sendEmptyMsg()
{
}
void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed)
void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
{
std::vector<uint8_t> payload = {DRIVE_ENCODER};
std::vector<uint8_t> msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload);
sendMsg(msg);
readSpeed(left_speed, right_speed);
readSpeedandPosition(left_speed, right_speed, left_pos, right_pos);
}
void ArduinoComms::setMotorValues(float left_speed, float right_speed)
@ -49,7 +49,7 @@ void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o)
ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r";
}
void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
void ArduinoComms::readSpeedandPosition(double &left_speed, double &right_speed, double &left_pos, double &right_pos)
{
std::vector<uint8_t> buffer;
int read_bytes = 0;
@ -91,13 +91,21 @@ void ArduinoComms::readSpeed(double &left_speed, double &right_speed)
if (frame_type == VALUE && frame_component == BOTH_MOTORS)
{
std::vector<uint8_t> left_speed_bytes(payload.begin(), payload.begin() + 4);
std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.end());
std::vector<uint8_t> right_speed_bytes(payload.begin() + 4, payload.begin()+8);
std::vector<uint8_t> left_pos_bytes(payload.begin() + 8, payload.begin()+12);
std::vector<uint8_t> right_pos_bytes(payload.begin() + 12, payload.end());
std::reverse(left_speed_bytes.begin(), left_speed_bytes.end());
std::reverse(right_speed_bytes.begin(), right_speed_bytes.end());
std::reverse(left_pos_bytes.begin(), left_pos_bytes.end());
std::reverse(right_pos_bytes.begin(), right_pos_bytes.end());
float left_speed_float = *reinterpret_cast<float*>(&left_speed_bytes[0]);
float right_speed_float = *reinterpret_cast<float*>(&right_speed_bytes[0]);
float left_pos_float = *reinterpret_cast<float*>(&left_pos_bytes[0]);
float right_pos_float = *reinterpret_cast<float*>(&right_pos_bytes[0]);
left_speed = left_speed_float;
right_speed = right_speed_float;
left_pos = left_pos_float;
right_pos = right_pos_float;
}
}

24
src/diffdrive_arduino/src/diffdrive_arduino.cpp
View File

@ -7,6 +7,11 @@
#include "rclcpp/rclcpp.hpp"
#include <math.h>
#define RADSTORPM(a)(a*60.0/(2.0*M_PI))
#define RPMSTORADS(a)(a*2.0*M_PI/60.0)
#define REVSTORAD(a)(a*2.0*M_PI)
#define RADSTOREV(a)(a/(2.0*M_PI))
namespace diffdrive_arduino
{
@ -106,18 +111,15 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp
return return_type::ERROR;
}
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());
arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos);
// 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 ;
l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
// Force the wheel position
l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
// convert rev to rad
l_wheel_.pos = REVSTORAD(l_wheel_.pos);
r_wheel_.pos = REVSTORAD(r_wheel_.pos);
return return_type::OK;
}
@ -128,9 +130,7 @@ return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcp
return return_type::ERROR;
}
// 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());
arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd));
return return_type::OK;
}