diff --git a/motors_control_card/firmware/include/Communication.hpp b/motors_control_card/firmware/include/Communication.hpp
index 1ee5d5b36803ec0c2c0277d1f47c26e2c955f8d6..06862fc92208e073f7e700801cc986a806fea1ee 100755
--- a/motors_control_card/firmware/include/Communication.hpp
+++ b/motors_control_card/firmware/include/Communication.hpp
@@ -6,6 +6,7 @@
#include <std_srvs/SetBool.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/Empty.h>
+#include <mccd_msgs/Odometry.h>
#include "mccd_msgs/Status.h"
#include "mccd_msgs/Odometry.h"
@@ -42,7 +43,7 @@ private:
void resetErrCallback(const std_msgs::Empty &e);
void enablePwmCallback(const std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp);
- void setPoseCallback(const mccd_srvs::SetPose::Request &req, mccd_srvs::SetPose::Response &resp);
+ void setPoseCallback(const mccd_msgs::Odometry &msg);
NodeHandle &m_nodeHandle;
@@ -57,6 +58,7 @@ private:
ros::Subscriber<mccd_msgs::CmdVel, Communication> *m_cmdVelSubscriber;
ros::Subscriber<std_msgs::Empty, Communication> *m_resetErrSubscriber;
+ ros::Subscriber<mccd_msgs::Odometry, Communication> *m_setPoseSubscriber;
ros::Publisher *m_odomPublisher;
ros::Publisher *m_statusPublisher;
@@ -64,7 +66,7 @@ private:
ros::Publisher *m_rightMotorCurrentPublisher;
ros::ServiceServer<std_srvs::SetBool::Request, std_srvs::SetBool::Response, Communication> *m_enablePwmService;
- ros::ServiceServer<mccd_srvs::SetPose::Request, mccd_srvs::SetPose::Response, Communication> *m_setPoseService;
+ //ros::ServiceServer<mccd_srvs::SetPose::Request, mccd_srvs::SetPose::Response, Communication> *m_setPoseService;
mccd_msgs::Odometry m_odomMsg;
std_msgs::UInt32 m_currentMsg;
diff --git a/motors_control_card/firmware/src/Communication.cpp b/motors_control_card/firmware/src/Communication.cpp
index 999100ed1f84522713c52c5a4c871c28c54f7a4e..452315c08d445e4ba52a743b1d2b0b6036f41806 100755
--- a/motors_control_card/firmware/src/Communication.cpp
+++ b/motors_control_card/firmware/src/Communication.cpp
@@ -10,12 +10,12 @@ Communication::Communication(NodeHandle &nodeHandle, Odometer &odometer, LeftMot
{
m_cmdVelSubscriber = new ros::Subscriber<mccd_msgs::CmdVel, Communication>("/mccd/cmd_vel", &Communication::cmdVelCallback, this);
m_resetErrSubscriber = new ros::Subscriber<std_msgs::Empty, Communication>("/mccd/reset_error", &Communication::resetErrCallback, this);
+ m_setPoseSubscriber = new ros::Subscriber<mccd_msgs::Odometry, Communication>("/mccd/odom/set_pose", &Communication::setPoseCallback, this);
m_odomPublisher = new ros::Publisher("/mccd/odom", &m_odomMsg);
m_statusPublisher = new ros::Publisher("/mccd/status", &m_statusMsg);
m_leftMotorCurrentPublisher = new ros::Publisher("/mccd/left_motor/current", &m_currentMsg);
m_rightMotorCurrentPublisher = new ros::Publisher("/mccd/right_motor/current", &m_currentMsg);
m_enablePwmService = new ros::ServiceServer<std_srvs::SetBool::Request, std_srvs::SetBool::Response, Communication>("/mccd/enable_pwm", &Communication::enablePwmCallback, this);
- m_setPoseService = new ros::ServiceServer<mccd_srvs::SetPose::Request, mccd_srvs::SetPose::Response, Communication>("/mccd/odom/set_pose", &Communication::setPoseCallback, this);
}
Communication::~Communication()
@@ -26,19 +26,19 @@ Communication::~Communication()
delete m_leftMotorCurrentPublisher;
delete m_rightMotorCurrentPublisher;
delete m_enablePwmService;
- delete m_setPoseService;
+ delete m_setPoseSubscriber;
}
void Communication::init()
{
m_nodeHandle.subscribe(*m_cmdVelSubscriber);
m_nodeHandle.subscribe(*m_resetErrSubscriber);
+ m_nodeHandle.subscribe(*m_setPoseSubscriber);
m_nodeHandle.advertise(*m_odomPublisher);
m_nodeHandle.advertise(*m_leftMotorCurrentPublisher);
m_nodeHandle.advertise(*m_rightMotorCurrentPublisher);
m_nodeHandle.advertise(*m_statusPublisher);
m_nodeHandle.advertiseService(*m_enablePwmService);
- m_nodeHandle.advertiseService(*m_setPoseService);
}
void Communication::update(double dt)
@@ -94,7 +94,7 @@ void Communication::enablePwmCallback(const std_srvs::SetBool::Request &req, std
const bool &enablePwm = req.data;
m_security.setLastCmdVel(HAL_GetTick());
m_speedRegulator.reset();
- m_odometer.reset();
+ //m_odometer.reset();
if (enablePwm)
{
@@ -113,9 +113,11 @@ void Communication::enablePwmCallback(const std_srvs::SetBool::Request &req, std
resp.success = true;
}
-void Communication::setPoseCallback(const mccd_srvs::SetPose::Request &setPoseReq, mccd_srvs::SetPose::Response &setPoseResp)
+void Communication::setPoseCallback(const mccd_msgs::Odometry &msg)
{
- m_odometer.setPosition({setPoseReq.x, setPoseReq.y, setPoseReq.theta});
- setPoseResp.message = "Pose set.";
- setPoseResp.success = true;
+ Odom::Position pos;
+ pos.x = msg.x;
+ pos.y = msg.y;
+ pos.theta = msg.theta;
+ m_odometer.setPosition(pos);
}
diff --git a/motors_control_card/firmware/src/MccdNode.cpp b/motors_control_card/firmware/src/MccdNode.cpp
index f516260db31c6587ff952a504583ec53126f23cf..91edef574b231adcd2fd84bfac3be0ee5acf12a2 100755
--- a/motors_control_card/firmware/src/MccdNode.cpp
+++ b/motors_control_card/firmware/src/MccdNode.cpp
@@ -1,5 +1,5 @@
#include "MccdNode.hpp"
-
+#include <DigitalOut.h>
// #include "motor/LeftMotor.hpp"
// #include "motor/RightMotor.hpp"
@@ -15,6 +15,8 @@ static const Odom::Geometry geometry = {
// -----------------------------------------
// | ROS NODE |
// -----------------------------------------
+mbed::DigitalOut checkStatusLed(LED3);
+bool led_value = 0;
MccdNode::MccdNode()
{
@@ -29,7 +31,7 @@ MccdNode::MccdNode()
m_rightMotorPid = new PID(-1, 1);
m_speedRegulator = new SpeedRegulator(*m_leftMotor, *m_rightMotor, *m_leftMotorPid, *m_rightMotorPid, *m_odometer);
- m_security = new Security(*m_leftMotor, *m_rightMotor, 2000, 1000); // 2000mA max current limit, 1000ms cmd_vel timeout
+ m_security = new Security(*m_leftMotor, *m_rightMotor, 2000, 10000); // 2000mA max current limit, 1000ms cmd_vel timeout
m_communication = new Communication(m_nodeHandle, *m_odometer, *m_leftMotor, *m_rightMotor, *m_speedRegulator, *m_security);
}
@@ -95,6 +97,8 @@ void MccdNode::start()
// Waiting for connection to the roscore
while (!m_nodeHandle.connected())
{
+ led_value = !led_value;
+ checkStatusLed = led_value;
wait_ms(communicationUpdateInterval);
m_nodeHandle.spinOnce();
diff --git a/motors_control_card/firmware/src/MccdNodeRosless.cpp b/motors_control_card/firmware/src/MccdNodeRosless.cpp
index f3ec7045ddddb8395a995a41b313a08c61b3a1b2..08b19e1c22b8cc237fa7996440679935252548a1 100755
--- a/motors_control_card/firmware/src/MccdNodeRosless.cpp
+++ b/motors_control_card/firmware/src/MccdNodeRosless.cpp
@@ -3,7 +3,7 @@
// #include "motor/LeftMotor.hpp"
// #include "motor/RightMotor.hpp"
-#include "mbed.h"
+#include "mbed.h"
Serial pc(USBTX, USBRX); // tx, rx
@@ -22,8 +22,8 @@ static const Odom::Geometry geometry = {
MccdNodeRosless::MccdNodeRosless()
{
- m_rightEncoder = new Encoder(D7, D8, 1000);
- m_leftEncoder = new Encoder(D10, D12, 1024);
+ m_rightEncoder = new Encoder(D10, D12, 1000);
+ m_leftEncoder = new Encoder(D7, D8, 1024);
m_odometer = new Odometer(geometry, *m_leftEncoder, *m_rightEncoder);
m_leftMotor = new LeftMotor;
@@ -36,7 +36,6 @@ MccdNodeRosless::MccdNodeRosless()
m_security = new Security(*m_leftMotor, *m_rightMotor, 2000, 1000); // 2000mA max current limit, 1000ms cmd_vel timeout
m_communication = new Communication(m_nodeHandle, *m_odometer, *m_leftMotor, *m_rightMotor, *m_speedRegulator, *m_security);
-
}
MccdNodeRosless::~MccdNodeRosless()
@@ -56,7 +55,6 @@ MccdNodeRosless::~MccdNodeRosless()
delete m_communication;
}
-
#define KP_L 3.285
#define KI_L 64.40766
#define KD_L 0.0
@@ -74,13 +72,11 @@ void MccdNodeRosless::init()
m_leftMotor->init();
m_rightMotor->init();
-
m_speedRegulator->setSpeedProfile(0.8, 0.8, 3.14, 3.14);
m_leftMotorPid->setGains(KP_L, KI_L, KD_L);
m_rightMotorPid->setGains(KP_R, KI_R, KD_R);
pc.printf("init ok\n");
-
}
void MccdNodeRosless::start()
@@ -88,17 +84,25 @@ void MccdNodeRosless::start()
pc.printf("start\n");
- int time; // current time since start up (ms)
+ int time;
+ double dt = 0.0001; // current time since start up (ms)
int lastTime = HAL_GetTick(); // last loop time (ms)
int elapsedTime = 0;
int dtLogic = 0; // delta time (in ms) for logic update (pid + odometry)
int dtCom = 0; // delta time (in ms) for communication update (send
- int dtSpin = 0; // delta time (in ms) for spin update (send
+ int dtSpin = 0; // delta time (in ms) for spin update (send
// odometry + transform + ...)
// Interval in ms at which the logic is updated
int logicUpdateInterval = 10;
int spinUpdateInterval = 25;
+ double last_x = 0;
+ double last_y = 0;
+ double speed_target = 0.1;
+ double angular_speed_target = 0.0;
+
+ double distance_accum = 0;
+ double theta_accum = 0;
// Interval in ms at which the communication updates (publish messages and execute callbacks for subscribers)
// It can be modified via rosparam (for pid calibration which requires higher sampling rate, for example)
@@ -122,31 +126,20 @@ void MccdNodeRosless::start()
m_security->setStatus(Status::CMD_VEL_TIMEOUT);
m_security->setStatus(Status::CMD_VEL_TIMEOUT);
-
wait_ms(1000);
pc.printf("enable pwm\n");
m_leftMotor->enablePwm();
m_rightMotor->enablePwm();
-
+
wait_ms(1000);
pc.printf("enter while loop\n");
m_speedRegulator->reset();
int one_time = 0;
- //dist = dist - distance_correction_offset ;
- double distance_accum = 0 ;
- double last_x = m_odometer->getPosition().x;
- double last_y = m_odometer->getPosition().y;
- pc.printf("distance accum %d\n", distance_accum);
-
-
-
-
- while (1 && distance_accum < 0.5)
+ while (1)
{
-
time = HAL_GetTick();
elapsedTime = time - lastTime;
lastTime = time;
@@ -155,84 +148,76 @@ void MccdNodeRosless::start()
dtCom += elapsedTime;
dtSpin += elapsedTime;
-
if (dtCom >= communicationUpdateInterval)
{
-
+
dtCom = 0;
}
-
if (dtLogic >= logicUpdateInterval)
{
- double dt = dtLogic / 1000.0;
+ dt = dtLogic / 1000.0;
m_odometer->update(dt); // updates the velocity of the robot, and the velocities of the right and left wheel
- //m_security->update(dt);
+ // m_security->update(dt);
- //if (m_security->getStatus() == Status::POWER_ON)
+ // if (m_security->getStatus() == Status::POWER_ON)
//{
- m_speedRegulator->update(dt);
+ m_speedRegulator->update(dt);
//}
+ m_speedRegulator->setSpeedTarget(speed_target, 0);
- dtLogic = 0;
+ distance_accum = distance_accum + sqrt((m_odometer->getPosition().x - last_x) * (m_odometer->getPosition().x - last_x) +
+ (m_odometer->getPosition().y - last_y) * (m_odometer->getPosition().y - last_y));
+
+ theta_accum = theta_accum + m_odometer->getPosition().theta ;
last_x = m_odometer->getPosition().x;
last_y = m_odometer->getPosition().y;
- distance_accum = distance_accum + sqrt((m_odometer->getPosition().x - last_x) * (m_odometer->getPosition().x - last_x) +
- (m_odometer->getPosition().y - last_y) * (m_odometer->getPosition().y - last_y)) ;
- if(m_odometer->getLeftWheelSpeed() !=0){
- pc.printf("distance accum %.2f\n", m_odometer->getLeftWheelSpeed());
-
+ if ((int)(distance_accum * 100) != 0)
+ {
+ pc.printf("distance %d \n", (int)(distance_accum * 100));
}
- m_speedRegulator->setSpeedTarget(0.5,0);
-
-
+ dtLogic = 0;
}
-
-
-
// ****** PROFIL ******** //
-
-
-
-
+ if (distance_accum> 0.2 && one_time == 0)
+ {
+ m_speedRegulator->reset();
+ speed_target = 0;
+ one_time++;
+ }
}
-
}
+void MccdNodeRosless ::distTarget(double dist, bool forward)
+{
+ double distance_accum = 0;
+ double last_x = m_odometer->getPosition().x;
+ double last_y = m_odometer->getPosition().y;
-/*void MccdNodeRosless :: distTarget(double dist, bool forward){
- dist = dist - distance_correction_offset ;
- double distance_accum = 0 ;
- double last_x = m_odometer->getPosition().x;
- double last_y = m_odometer->getPosition().y;
-
-
-
- while (distance_accum < dist){
-
- start_slope_speed = min_lin_speed + distance_accum / \
- start_slope_distance_threshold * \
- (max_lin_speed - min_lin_speed)
- end_slope_speed = min_lin_speed + \
- (dist - distance_accum) / \
- end_slope_distance_threshold * (max_lin_speed - min_lin_speed)
+ while (distance_accum < dist)
+ {
- speed = max(min_lin_speed, min(max_lin_speed,
- min(start_slope_speed, end_slope_speed)))
+ /*start_slope_speed = min_lin_speed + distance_accum / \
+ start_slope_distance_threshold * \
+ (max_lin_speed - min_lin_speed)
+ end_slope_speed = min_lin_speed + \
+ (dist - distance_accum) / \
+ end_slope_distance_threshold * (max_lin_speed - min_lin_speed)
- self.set_speed(speed if forward else -speed, 0)
- m_speedRegulator->setSpeedTarget(0.2, 0);
+ speed = max(min_lin_speed, min(max_lin_speed,
+ min(start_slope_speed, end_slope_speed)))
- distance_accum = distance_accum + sqrt((m_odometer->getPosition().x - last_x) * (m_odometer->getPosition().x - last_x) +
- (m_odometer->getPosition().y - last_y) * (m_odometer->getPosition().y - last_y)) ;
+ self.set_speed(speed if forward else -speed, 0)*/
+ m_speedRegulator->setSpeedTarget(0.2, 0);
- last_x = m_odometer->getPosition().x;
- last_y = m_odometer->getPosition().y;
- wait_ms(0.1);
- }
-
-}*/
+ distance_accum = distance_accum + sqrt((m_odometer->getPosition().x - last_x) * (m_odometer->getPosition().x - last_x) +
+ (m_odometer->getPosition().y - last_y) * (m_odometer->getPosition().y - last_y));
+ last_x = m_odometer->getPosition().x;
+ last_y = m_odometer->getPosition().y;
+ wait_ms(0.1);
+ }
+}
diff --git a/motors_control_card/firmware/src/main.cpp b/motors_control_card/firmware/src/main.cpp
index a671d5a94e411aef9b7ea97140bcdace9c91d04d..3e2dffe041b1dc628af82c2ca087b8090f14f805 100755
--- a/motors_control_card/firmware/src/main.cpp
+++ b/motors_control_card/firmware/src/main.cpp
@@ -1,3 +1,4 @@
+#define ROS_VERSION
#ifdef ROS_VERSION
diff --git a/motors_control_card/firmware/src/odometry/Odometer.cpp b/motors_control_card/firmware/src/odometry/Odometer.cpp
index 2564ad605f7180f54cb1d32061b93b288939b2d8..9cf6cafa20f2368d23f3d4cc5e74182b743bc434 100755
--- a/motors_control_card/firmware/src/odometry/Odometer.cpp
+++ b/motors_control_card/firmware/src/odometry/Odometer.cpp
@@ -49,7 +49,7 @@ void Odometer::update(double dt)
// Constrain theta between 0 and 2*PI
m_pos.theta = std::fmod(m_pos.theta + dTheta, Math::TWO_PI);
- m_pos.x += std::cos(m_pos.theta) * dDistance;
+ m_pos.x += std::cos(m_pos.theta) * dDistance ;
m_pos.y += std::sin(m_pos.theta) * dDistance;
// Prevent division by zero
@@ -85,7 +85,9 @@ const Odom::Geometry &Odometer::getGeometry() const
void Odometer::setPosition(const Odom::Position &position)
{
- m_pos = position;
+ m_pos.x = position.x;
+ m_pos.y = position.y;
+ m_pos.theta = position.theta;
}
double Odometer::getLeftWheelSpeed() const
diff --git a/pid_calibration/launch/calibration_zigler_nichols.launch b/pid_calibration/launch/calibration_zigler_nichols.launch
index e4735d0544dc70108215e5e10a28bda30a0949df..ebbf6f47260c114acc9f07884bf5a94312c6dcd6 100755
--- a/pid_calibration/launch/calibration_zigler_nichols.launch
+++ b/pid_calibration/launch/calibration_zigler_nichols.launch
@@ -1,14 +1,11 @@
<launch>
- <machine name="lagalere" address="lagalere.local" env-loader="/home/lagalere/ros_ws/env_loader.sh" default="false" user="lagalere" />
- <machine name="local" address="localhost" default="true" />
+ <arg name="mccd_port" default="/dev/ttyACM_STM"/>
+ <arg name="mccd_baud" default="115200"/>
- <arg name="port" default="/dev/mccd"/>
- <arg name="baud" default="115200"/>
-
- <node machine="lagalere" name="mccd" pkg="rosserial_python" type="serial_node.py" output="screen" respawn="true">
- <param name="port" type="string" value="$(arg port)"/>
- <param name="baud" type="int" value="$(arg baud)"/>
+ <node name="mccd" pkg="rosserial_python" type="serial_node.py" output="screen" respawn="true">
+ <param name="port" type="string" value="$(arg mccd_port)"/>
+ <param name="baud" type="int" value="$(arg mccd_baud)"/>
<param name="sampling_period" type="int" value="20"/>
<!-- Set large value for acceleration max not to impact pid calibration with speed profile -->
<param name="speed_profile/linear_speed_acceleration_max" type="double" value="999999"/>
@@ -17,7 +14,7 @@
<param name="speed_profile/angular_speed_deceleration_max" type="double" value="999999"/>
</node>
- <node machine="local" name="calibration_ziegler_nichols" pkg="pid_calibration" type="ziegler_nichols.py" output="screen" />
- <node machine="local" name="rqt_calibration" pkg="pid_calibration" type="rqt_delayed_node.py" output="screen" />
- <node machine="local" name="rosbag_record_speed" pkg="rosbag" type="record" args="record -o speed_record /cmd_vel /mccd/odom"/>
+ <node name="calibration_ziegler_nichols" pkg="pid_calibration" type="ziegler_nichols.py" output="screen" />
+ <node name="rqt_calibration" pkg="pid_calibration" type="rqt_delayed_node.py" output="screen" />
+ <node name="rosbag_record_speed" pkg="rosbag" type="record" args="record -o speed_record /cmd_vel /mccd/odom"/>
</launch>
diff --git a/pid_calibration/nodes/ziegler_nichols.py b/pid_calibration/nodes/ziegler_nichols.py
index e069c2a786b27fd33d25bc20bc3d847273559867..1ef7868f6c18266e7e4f5c6cd98242fd8b97da1b 100755
--- a/pid_calibration/nodes/ziegler_nichols.py
+++ b/pid_calibration/nodes/ziegler_nichols.py
@@ -6,6 +6,7 @@ import rosparam
from mccd_msgs.msg import Odometry
from std_srvs.srv import SetBool
from geometry_msgs.msg import Twist
+from mccd_msgs.msg import CmdVel
import time
cmd_vel_pub = None
@@ -53,14 +54,10 @@ def input_yes_no(msg):
def send_linear_speed(lin_speed):
- twist = Twist()
- twist.linear.x = lin_speed
- twist.linear.y = 0
- twist.linear.z = 0
- twist.angular.x = 0
- twist.angular.y = 0
- twist.angular.z = 0
- cmd_vel_pub.publish(twist)
+ cmd_vel_msg = CmdVel()
+ cmd_vel_msg.linear_velocity = lin_speed
+ cmd_vel_msg.angular_velocity = 0
+ cmd_vel_pub.publish(cmd_vel_msg)
def compute_gains(Ku, Tu):
@@ -103,7 +100,7 @@ def calibrate_motor(left_side):
input("Press [enter] when ready to run...")
rospy.loginfo("Running for 5s")
- enable_pwm(True)
+ change_pwm(True)
start = time.time()
@@ -116,7 +113,7 @@ def calibrate_motor(left_side):
send_linear_speed(0)
time.sleep(1)
- enable_pwm(False)
+ change_pwm(False)
oscillating = input_yes_no(
"Does the speed output shows stable and consistent oscillations? ")
@@ -131,16 +128,28 @@ def calibrate_motor(left_side):
rospy.loginfo("*---------------------*")
break
+def change_pwm(self, enable):
+ print("Waiting for service '/mccd/enable_pwm'")
+ rospy.wait_for_service('/mccd/enable_pwm')
+
+ try:
+ res = self.change_pwm_service(enable)
+ return res
+ except rospy.ServiceException as e:
+ print("Service call failed: %s" % e)
+
+
def calibration_node():
global enable_pwm, cmd_vel_pub
rospy.init_node('calibration')
+ time.sleep(5)
wait_for_mccd()
enable_pwm = rospy.ServiceProxy('/mccd/enable_pwm', SetBool)
- cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
+ cmd_vel_pub = rospy.Publisher("/cmd_vel", CmdVel, queue_size=1)
calibrate_motor(left_side=True)
calibrate_motor(left_side=False)