From 5bb1abe09988af6834ec41b266faf2b07debb13b Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Sun, 1 Nov 2015 23:44:38 -0600 Subject: [PATCH 01/13] Add code to use encoders Needs to be tested on working robot, to verify the speed is calculated correctlly, and acurattly --- ros_ws/src/arudino/motor/motor.ino | 190 +++++++++++++++++++++-------- 1 file changed, 142 insertions(+), 48 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index 72da2f7..875f620 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -16,16 +16,54 @@ * Constants * *************/ +/** + * @brief Place to Change which Wheel we want to Program + * + * In order to correctly inturpret the Wheel Velocity's, + * we need to define which wheel we are uploading code to. + * This will be used to insure that the proper wheel gets the + * Proper commands + */ +#define LEFT_WHEEL + +#if defined RIGHT_WHEEL +const char* VELOCITY_TOPIC = "right_vel"; +#elif defined LEFT_WHEEL +const char* VELOCITY_TOPIC = "left_vel"; +#endif + /** * @brief Define Pi for use in conversion */ const float PI_CONST = 3.14159265359; + +/** + * @brief value for the desierd refresh rate of the encoder + * + * Set to 30Hz + */ + const float refresh_rate = 33.3; + +/** + * @brief Define constants for the Encoder Calculation + */ +const float WHEEL_RADIUS = 8.5; //Wheel Radius in inches +const float WHEEL_CIRCUMFRANCE = (WHEEL_RADIUS*PI_CONST*2); +const int GEAR_RATIO = 2; +const int TICKS_PER_ROTATION = 200/GEAR_RATIO; /** * @brief Radians Per Second at full Throtle */ const float MAX_RAD_SEC = 5; +/** + * @brief Pin the encoder is attached + */ +const int ENCODER_PIN = 2; + + + /** * @brief Pins used to control the Motor */ @@ -33,31 +71,36 @@ const int FORWARD_PWM_PIN = 5; const int REVERSE_PWM_PIN = 6; const int ENABLE_PIN = A3; - /** - * @brief Place to Change which Wheel we want to Program - * - * In order to correctly inturpret the Wheel Velocity's, - * we need to define which wheel we are uploading code to. - * This will be used to insure that the proper wheel gets the - * Proper commands + * @brief Float used to scale the Speed to */ -#define RIGHT_WHEEL +float desiered_speed = 0; -#if defined RIGHT_WHEEL -const char* VELOCITY_TOPIC = "right_vel"; -#elif defined LEFT_WHEEL -const char* VELOCITY_TOPIC = "left_vel"; -#endif +/** + * @brief Values to be updated when the inturrupt is triggered for encoder + */ +volatile unsigned int encoder_ticks = 0; +volatile int encoder_pos_last = LOW; +volatile int encoder_pos_current = LOW; /** - * @brief Float used to scale the Speed to + * @brief Values used to keep track of current time for multitasking */ -float speed = 0; + int current_mills = 0; + int old_mills = 0; + + /** + * @brief Values used to calculate speed from the encoder + */ + int ticks_per_cycle = 0; + int ticks_per_cycle_old = 0; + float current_speed = 0; /************************ * Forward Declerations * ************************/ +void encoder_count(); +void calculate_speed(); void velocityCallback(const std_msgs::Float64& msg); float fscale( float originalMin, float originalMax, float newBegin, float newEnd, float inputValue, float curve); @@ -65,7 +108,7 @@ float fscale( float originalMin, float originalMax, float newBegin, /** * @brief ROS node handle */ -ros::NodeHandle nodeHandle; +ros::NodeHandle node_handle; /** * @brief ROS Subscriber for the Velocity Message @@ -82,12 +125,12 @@ ros::Subscriber velocitySub(VELOCITY_TOPIC, &velocityCallback */ void velocityCallback(const std_msgs::Float64& msg) { - speed = fscale(0, MAX_RAD_SEC, 0, 255, abs(msg.data), 0); + desiered_speed = fscale(0, MAX_RAD_SEC, 0, 255, abs(msg.data), 0); if(msg.data > 0) { //Go Forward digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, speed); + analogWrite(FORWARD_PWM_PIN, desiered_speed); analogWrite(REVERSE_PWM_PIN, 0); } else if(msg.data < 0) @@ -95,7 +138,7 @@ void velocityCallback(const std_msgs::Float64& msg) //Go in Reverse digitalWrite(ENABLE_PIN, HIGH); analogWrite(FORWARD_PWM_PIN, 0); - analogWrite(REVERSE_PWM_PIN, speed); + analogWrite(REVERSE_PWM_PIN, desiered_speed); } else { @@ -112,6 +155,7 @@ void setup() pinMode(FORWARD_PWM_PIN, OUTPUT); pinMode(REVERSE_PWM_PIN, OUTPUT); pinMode(ENABLE_PIN, OUTPUT); + pinMode(ENCODER_PIN, INPUT); //Set Robot to break when starting digitalWrite(ENABLE_PIN, HIGH); @@ -119,18 +163,68 @@ void setup() analogWrite(REVERSE_PWM_PIN, 0); //Setup ROS node and topics - nodeHandle.initNode(); - nodeHandle.subscribe(velocitySub); + node_handle.initNode(); + node_handle.subscribe(velocitySub); + + //Set the Inturupt on Pin 2 + attachInterrupt(0, encoder_count, CHANGE); } void loop() { - nodeHandle.spinOnce(); -} - + node_handle.spinOnce(); + //update the current time for multitasking + current_mills = millis(); + + //Get the speed of wheel at a rate of 30Htz + if(current_mills-old_mills >= refresh_rate) + { + calculate_speed(); + } +} +/** + * @brief The Function will update the encoder + * + * This Function is called whenever there is a change + * to the value of pin 2, it is part of the attachInterrupt routine + * It updates the value of + */ +void encoder_count() +{ + encoder_pos_current = digitalRead(ENCODER_PIN); + + if ((encoder_pos_last == LOW) && + (encoder_pos_current == HIGH)) + { + encoder_ticks++; + } + + encoder_pos_last = encoder_pos_current; +} +/** + * @brief The Function will calculate the current speed + * + * This function is called once durring each refresh rate + * It calculates the current speed based on the encoder readings + */ +void calculate_speed() +{ + ticks_per_cycle = encoder_ticks; + + //calculates speed in (inces per .03 seconds) + //************************************************* + //TEMPORARY --WILL UPDATE WITH PROPER UNITS LATER-- + //************************************************* + current_speed = ((ticks_per_cycle-ticks_per_cycle_old) / + (TICKS_PER_ROTATION*WHEEL_CIRCUMFRANCE)); + + //Update the following values so that the next cycle works correctlly + ticks_per_cycle_old = ticks_per_cycle; + old_mills = current_mills; +} /** * @brief The Function will Scale the Input to the Expected Output @@ -149,16 +243,16 @@ void loop() * curve - excepts a value -10 through 10, 0 being linear * scaling the values with a curve */ -float fscale( float originalMin, float originalMax, float newBegin, - float newEnd, float inputValue, float curve) +float fscale( float original_min, float original_max, float new_begin, + float new_end, float input_value, float curve) { - float OriginalRange = 0; - float NewRange = 0; - float zeroRefCurVal = 0; - float normalizedCurVal = 0; - float rangedValue = 0; - boolean invFlag = 0; + float original_range = 0; + float new_range = 0; + float zero_ref_cur_val = 0; + float normalized_cur_val = 0; + float ranged_value = 0; + boolean inv_flag = 0; // condition curve parameter @@ -179,48 +273,48 @@ float fscale( float originalMin, float originalMax, float newBegin, curve = pow(10, curve); // Check for out of range inputValues - if (inputValue < originalMin) + if (input_value < original_min) { - inputValue = originalMin; + input_value = original_min; } - if (inputValue > originalMax) + if (input_value > original_max) { - inputValue = originalMax; + input_value = original_max; } // Zero Refference the values - OriginalRange = originalMax - originalMin; + original_range = original_max - original_min; - if (newEnd > newBegin) + if (new_end > new_begin) { - NewRange = newEnd - newBegin; + new_range = new_end - new_begin; } else { - NewRange = newBegin - newEnd; - invFlag = 1; + new_range = new_begin - new_end; + inv_flag = 1; } - zeroRefCurVal = inputValue - originalMin; + zero_ref_cur_val = input_value - original_min; // normalize to 0 - 1 float - normalizedCurVal = zeroRefCurVal / OriginalRange; + normalized_cur_val = zero_ref_cur_val / original_range; // Check for originalMin > originalMax - the math for all other cases // i.e. negative numbers seems to work out fine - if (originalMin > originalMax ) + if (original_min > original_max ) { return 0; } - if (invFlag == 0) + if (inv_flag == 0) { - rangedValue = (pow(normalizedCurVal, curve) * NewRange) + newBegin; + ranged_value = (pow(normalized_cur_val, curve) * new_range) + new_begin; } else // invert the ranges { - rangedValue = newBegin - (pow(normalizedCurVal, curve) * NewRange); + ranged_value = new_begin - (pow(normalized_cur_val, curve) * new_range); } - return rangedValue; + return ranged_value; } From d9373d9914c86fff72936a7bb4f133eea8a48966 Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Mon, 2 Nov 2015 11:20:12 -0600 Subject: [PATCH 02/13] Fix spelling mistakes Changed the desiered_speed variable to desired_speed --- ros_ws/src/arudino/motor/motor.ino | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index 875f620..68b244a 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -74,7 +74,7 @@ const int ENABLE_PIN = A3; /** * @brief Float used to scale the Speed to */ -float desiered_speed = 0; +float desired_speed = 0; /** * @brief Values to be updated when the inturrupt is triggered for encoder @@ -125,12 +125,12 @@ ros::Subscriber velocitySub(VELOCITY_TOPIC, &velocityCallback */ void velocityCallback(const std_msgs::Float64& msg) { - desiered_speed = fscale(0, MAX_RAD_SEC, 0, 255, abs(msg.data), 0); + desired_speed = fscale(0, MAX_RAD_SEC, 0, 255, abs(msg.data), 0); if(msg.data > 0) { //Go Forward digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, desiered_speed); + analogWrite(FORWARD_PWM_PIN, desired_speed); analogWrite(REVERSE_PWM_PIN, 0); } else if(msg.data < 0) @@ -138,7 +138,7 @@ void velocityCallback(const std_msgs::Float64& msg) //Go in Reverse digitalWrite(ENABLE_PIN, HIGH); analogWrite(FORWARD_PWM_PIN, 0); - analogWrite(REVERSE_PWM_PIN, desiered_speed); + analogWrite(REVERSE_PWM_PIN, desired_speed); } else { From 2955a66a0737cdb36a1f1fbd3a11e7a6505b681b Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Sun, 8 Nov 2015 13:29:02 -0600 Subject: [PATCH 03/13] Add custom msg to send encoder data back to ROS Added code to Arduino to publish encoder ticks Created a custom msg including Header, and Uint8 --- ros_ws/src/arudino/motor/motor.ino | 103 ++++++++++++++++++----------- ros_ws/src/control/CMakeLists.txt | 10 +++ ros_ws/src/control/msg/Encoder.msg | 2 + ros_ws/src/control/package.xml | 4 +- 4 files changed, 79 insertions(+), 40 deletions(-) create mode 100644 ros_ws/src/control/msg/Encoder.msg diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index 68b244a..bcacab4 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -11,6 +11,7 @@ #include #include +#include /************* * Constants * @@ -28,8 +29,10 @@ #if defined RIGHT_WHEEL const char* VELOCITY_TOPIC = "right_vel"; +const char* ENCODER_TOPIC = "right_tick"; #elif defined LEFT_WHEEL const char* VELOCITY_TOPIC = "left_vel"; +const char* ENCODER_TOPIC = "left_tick"; #endif /** @@ -42,7 +45,7 @@ const float PI_CONST = 3.14159265359; * * Set to 30Hz */ - const float refresh_rate = 33.3; + const float REFRESH_RATE = 33.3; /** * @brief Define constants for the Encoder Calculation @@ -99,10 +102,11 @@ volatile int encoder_pos_current = LOW; /************************ * Forward Declerations * ************************/ -void encoder_count(); -void calculate_speed(); +void encoderCount(); +void calculateSpeed(); +void updateEncoder(); void velocityCallback(const std_msgs::Float64& msg); -float fscale( float originalMin, float originalMax, float newBegin, +float fScale( float originalMin, float originalMax, float newBegin, float newEnd, float inputValue, float curve); /** @@ -110,11 +114,58 @@ float fscale( float originalMin, float originalMax, float newBegin, */ ros::NodeHandle node_handle; +/** + * @brief ROS message used for publishing the encoder data + */ + std_msgs::UInt8 encoderMessage; + /** * @brief ROS Subscriber for the Velocity Message */ ros::Subscriber velocitySub(VELOCITY_TOPIC, &velocityCallback); +/** + * @brief ROS Publisher for the Encoder Message + */ +ros::Publisher encoderPub(ENCODER_TOPIC, &encoderMessage); + +void setup() +{ + //setup pins + pinMode(FORWARD_PWM_PIN, OUTPUT); + pinMode(REVERSE_PWM_PIN, OUTPUT); + pinMode(ENABLE_PIN, OUTPUT); + pinMode(ENCODER_PIN, INPUT); + + //Set Robot to break when starting + digitalWrite(ENABLE_PIN, HIGH); + analogWrite(FORWARD_PWM_PIN, 0); + analogWrite(REVERSE_PWM_PIN, 0); + + //Setup ROS node and topics + node_handle.initNode(); + node_handle.subscribe(velocitySub); + node_handle.advertise(encoderPub); + + //Set the Inturupt on Pin 2 + attachInterrupt(0, encoderCount, CHANGE); +} + +void loop() +{ + node_handle.spinOnce(); + updateEncoder(); + + //update the current time for multitasking + current_mills = millis(); + + //Get the speed of wheel at a rate of 30Htz + if(current_mills-old_mills >= REFRESH_RATE) + { + calculateSpeed(); + } +} + /** * @brief The callback function for velocity messages * @@ -125,7 +176,7 @@ ros::Subscriber velocitySub(VELOCITY_TOPIC, &velocityCallback */ void velocityCallback(const std_msgs::Float64& msg) { - desired_speed = fscale(0, MAX_RAD_SEC, 0, 255, abs(msg.data), 0); + desired_speed = fScale(0, MAX_RAD_SEC, 0, 255, abs(msg.data), 0); if(msg.data > 0) { //Go Forward @@ -149,39 +200,13 @@ void velocityCallback(const std_msgs::Float64& msg) } } -void setup() +void updateEncoder() { - //setup pins - pinMode(FORWARD_PWM_PIN, OUTPUT); - pinMode(REVERSE_PWM_PIN, OUTPUT); - pinMode(ENABLE_PIN, OUTPUT); - pinMode(ENCODER_PIN, INPUT); - - //Set Robot to break when starting - digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, 0); - analogWrite(REVERSE_PWM_PIN, 0); - - //Setup ROS node and topics - node_handle.initNode(); - node_handle.subscribe(velocitySub); - - //Set the Inturupt on Pin 2 - attachInterrupt(0, encoder_count, CHANGE); -} - -void loop() -{ - node_handle.spinOnce(); - - //update the current time for multitasking - current_mills = millis(); + //update the value of the message + encoderMessage.data = encoder_ticks; - //Get the speed of wheel at a rate of 30Htz - if(current_mills-old_mills >= refresh_rate) - { - calculate_speed(); - } + //publish message + encoderPub.publish(&encoderMessage); } /** @@ -191,7 +216,7 @@ void loop() * to the value of pin 2, it is part of the attachInterrupt routine * It updates the value of */ -void encoder_count() +void encoderCount() { encoder_pos_current = digitalRead(ENCODER_PIN); @@ -210,7 +235,7 @@ void encoder_count() * This function is called once durring each refresh rate * It calculates the current speed based on the encoder readings */ -void calculate_speed() +void calculateSpeed() { ticks_per_cycle = encoder_ticks; @@ -243,7 +268,7 @@ void calculate_speed() * curve - excepts a value -10 through 10, 0 being linear * scaling the values with a curve */ -float fscale( float original_min, float original_max, float new_begin, +float fScale( float original_min, float original_max, float new_begin, float new_end, float input_value, float curve) { diff --git a/ros_ws/src/control/CMakeLists.txt b/ros_ws/src/control/CMakeLists.txt index 7450770..87abee0 100644 --- a/ros_ws/src/control/CMakeLists.txt +++ b/ros_ws/src/control/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs + message_generation ) ################################### @@ -16,8 +17,17 @@ catkin_package( LIBRARIES control CATKIN_DEPENDS geometry_msgs roscpp std_msgs sensor_msgs DEPENDS system_lib + CATKIN_DEPENDS message_runtime ) +################################################ +## Declare ROS messages, services and actions ## +################################################ +## Generate message in the 'msg' folder +add_message_files(FILES Encoder.msg) + + + ########### ## Build ## ########### diff --git a/ros_ws/src/control/msg/Encoder.msg b/ros_ws/src/control/msg/Encoder.msg new file mode 100644 index 0000000..af1b739 --- /dev/null +++ b/ros_ws/src/control/msg/Encoder.msg @@ -0,0 +1,2 @@ +Header header +uint8 ticks diff --git a/ros_ws/src/control/package.xml b/ros_ws/src/control/package.xml index f1af59c..87a9313 100644 --- a/ros_ws/src/control/package.xml +++ b/ros_ws/src/control/package.xml @@ -18,9 +18,11 @@ roscpp std_msgs sensor_msgs + message_generation geometry_msgs roscpp std_msgs sensor_msgs + message_runtime - \ No newline at end of file + From 64e74689364792c72de4e746e62a5a1341d78532 Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Fri, 13 Nov 2015 12:56:20 -0600 Subject: [PATCH 04/13] Update comments to be more accurate --- ros_ws/src/arudino/motor/motor.ino | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index bcacab4..a779194 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -65,8 +65,6 @@ const float MAX_RAD_SEC = 5; */ const int ENCODER_PIN = 2; - - /** * @brief Pins used to control the Motor */ @@ -117,7 +115,7 @@ ros::NodeHandle node_handle; /** * @brief ROS message used for publishing the encoder data */ - std_msgs::UInt8 encoderMessage; +std_msgs::UInt8 encoderMessage; /** * @brief ROS Subscriber for the Velocity Message @@ -154,6 +152,7 @@ void setup() void loop() { node_handle.spinOnce(); + updateEncoder(); //update the current time for multitasking @@ -200,6 +199,12 @@ void velocityCallback(const std_msgs::Float64& msg) } } +/** + * @brief The Function will send the updated encoder value to ROS + * + * This Function is used to update the main ROS program with + * the current number of ticks per timestamp + */ void updateEncoder() { //update the value of the message @@ -214,7 +219,7 @@ void updateEncoder() * * This Function is called whenever there is a change * to the value of pin 2, it is part of the attachInterrupt routine - * It updates the value of + * It updates the value of encoder_ticks */ void encoderCount() { From 27b992ff19e86180d63ef85798c5417e455a4ddf Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Sun, 15 Nov 2015 10:27:14 -0600 Subject: [PATCH 05/13] Fix issue with Arduino not seeing header Allowed the Arduino to see the Ros Custom Header, will require remaking the rosserial_python libraries after making, so that the arduino code can actully compile --- ros_ws/src/arudino/motor/motor.ino | 6 ++++-- ros_ws/src/control/CMakeLists.txt | 15 +++++++-------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index a779194..1d3e432 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -12,6 +12,7 @@ #include #include #include +#include /************* * Constants * @@ -115,7 +116,7 @@ ros::NodeHandle node_handle; /** * @brief ROS message used for publishing the encoder data */ -std_msgs::UInt8 encoderMessage; +control::Encoder encoderMessage; /** * @brief ROS Subscriber for the Velocity Message @@ -208,7 +209,8 @@ void velocityCallback(const std_msgs::Float64& msg) void updateEncoder() { //update the value of the message - encoderMessage.data = encoder_ticks; + encoderMessage.ticks = encoder_ticks; + encoderMessage.header.stamp = node_handle.now(); //publish message encoderPub.publish(&encoderMessage); diff --git a/ros_ws/src/control/CMakeLists.txt b/ros_ws/src/control/CMakeLists.txt index 87abee0..b217d0f 100644 --- a/ros_ws/src/control/CMakeLists.txt +++ b/ros_ws/src/control/CMakeLists.txt @@ -9,6 +9,13 @@ find_package(catkin REQUIRED COMPONENTS message_generation ) +################################################ +## Declare ROS messages, services and actions ## +################################################ +## Generate message in the 'msg' folder +add_message_files(FILES Encoder.msg) +generate_messages(DEPENDENCIES std_msgs) + ################################### ## catkin specific configuration ## ################################### @@ -20,14 +27,6 @@ catkin_package( CATKIN_DEPENDS message_runtime ) -################################################ -## Declare ROS messages, services and actions ## -################################################ -## Generate message in the 'msg' folder -add_message_files(FILES Encoder.msg) - - - ########### ## Build ## ########### From ff81dbe70e3942a78489f8e4ec3daf7593872322 Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Wed, 20 Jan 2016 22:42:36 -0600 Subject: [PATCH 06/13] Add shell encoder callbacks The encoder callbacks will process data retrieved from the encoders. The callbacks are currently empty. --- ros_ws/src/common/constants.h | 8 +++ .../include/control/motor_controller.h | 51 +++++++++++++++++++ .../control/src/control/motor_controller.cpp | 13 +++++ 3 files changed, 72 insertions(+) diff --git a/ros_ws/src/common/constants.h b/ros_ws/src/common/constants.h index 11b615a..d956eeb 100644 --- a/ros_ws/src/common/constants.h +++ b/ros_ws/src/common/constants.h @@ -33,6 +33,14 @@ const std::string RIGHT_VEL_TOPIC = "right_vel"; * @brief The topic that the left wheel angular velocity will be published on */ const std::string LEFT_VEL_TOPIC = "left_vel"; +/** + * @brief The topic that the left encoder will publish on + */ +const std::string LEFT_ENCODER_TOPIC = "left_tick"; +/** + * @brief The topic that the right encoder will publish on + */ +const std::string RIGHT_ENCODER_TOPIC = "right_tick"; //Node name constants /** diff --git a/ros_ws/src/control/include/control/motor_controller.h b/ros_ws/src/control/include/control/motor_controller.h index a5f73ce..3c76e50 100644 --- a/ros_ws/src/control/include/control/motor_controller.h +++ b/ros_ws/src/control/include/control/motor_controller.h @@ -12,6 +12,9 @@ #include #include "constants.h" +/* Custom messages - This file should be Auto generated */ +#include "control/Encoder.h" + class MotorController { private: @@ -35,6 +38,16 @@ class MotorController */ double unscaled_max_speed; + /** + * @brief The previous left encoder time read + */ + ros::Time left_prev_time; + + /** + * @brief The previous right encoder time read + */ + ros::Time right_prev_time; + /** * @brief The right velocity to be published */ @@ -50,6 +63,26 @@ class MotorController */ ros::Subscriber twist_sub; + /** + * @brief The number of ticks read from the left encoder in a time interval + */ + control::Encoder left_ticks; + + /** + * @brief The number of ticks read from the right encoder in a time interval + */ + control::Encoder right_ticks; + + /** + * @brief The subscriber for the left encoder + */ + ros::Subscriber left_encoder; + + /** + * @brief The subscriber for the right encoder + */ + ros::Subscriber right_encoder; + /** * @brief The right wheel velocity publisher */ @@ -75,6 +108,24 @@ class MotorController */ void twistCallback(const geometry_msgs::Twist::ConstPtr& msg); + /** + * @brief The callback function for the right encoder message + * + * This function accepts Encoder messages and records the number of ticks in a time interval for the right wheel + * + * @param msg The message which is received from the right encoder publisher + */ + void rightEncoderCallback(const control::Encoder::ConstPtr& msg); + + /** + * @brief The callback function for the right encoder message + * + * This function accepts Encoder messages and records the number of ticks in a time interval for the left wheel + * + * @param msg The message which is received from the right encoder publisher + */ + void leftEncoderCallback(const control::Encoder::ConstPtr& msg); + /** * @brief Calculates the right wheel angular velocity based on a differential drive model * @param lin_vel The robot's linear velocity diff --git a/ros_ws/src/control/src/control/motor_controller.cpp b/ros_ws/src/control/src/control/motor_controller.cpp index 0d1ff8d..c14637e 100644 --- a/ros_ws/src/control/src/control/motor_controller.cpp +++ b/ros_ws/src/control/src/control/motor_controller.cpp @@ -13,6 +13,9 @@ MotorController::MotorController() twist_sub = nh.subscribe(CONTROL_TOPIC, 1, &MotorController::twistCallback, this); right_vel_pub = nh.advertise(RIGHT_VEL_TOPIC, 1); left_vel_pub = nh.advertise(LEFT_VEL_TOPIC, 1); + left_encoder = nh.subscribe(LEFT_ENCODER_TOPIC, 5, &MotorController::leftEncoderCallback, this); + right_encoder = nh.subscribe(RIGHT_ENCODER_TOPIC, 5, &MotorController::rightEncoderCallback, + this); //Check that the necessary parameters exist and set member variables appropriately if(!nh.getParam("/robot_base", robot_base)) @@ -45,6 +48,16 @@ void MotorController::twistCallback(const geometry_msgs::Twist::ConstPtr& msg) left_vel.data = getLeftVel(msg->linear.x, msg->angular.z); } +void MotorController::rightEncoderCallback(const control::Encoder::ConstPtr& msg) +{ + +} + +void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) +{ + +} + double MotorController::getRightVel(const double lin_vel, const double ang_vel) { double pre_scaled = (2 * lin_vel + ang_vel * robot_base) / (2 * wheel_rad); From 12443b2db1f5d6b73c8236910c5d60ad6b7d02a2 Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Thu, 21 Jan 2016 13:18:19 -0600 Subject: [PATCH 07/13] Add odometry update from encoders The robots odometry should now be updated from the encoders. --- ros_ws/src/control/CMakeLists.txt | 2 + .../include/control/motor_controller.h | 11 +++++- ros_ws/src/control/package.xml | 4 ++ ros_ws/src/control/param/igvc.yaml | 1 + .../control/src/control/motor_controller.cpp | 37 +++++++++++++++++++ 5 files changed, 53 insertions(+), 2 deletions(-) diff --git a/ros_ws/src/control/CMakeLists.txt b/ros_ws/src/control/CMakeLists.txt index fb2d6a0..f044b4a 100644 --- a/ros_ws/src/control/CMakeLists.txt +++ b/ros_ws/src/control/CMakeLists.txt @@ -7,6 +7,8 @@ find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs message_generation + nav_msgs + tf ) ################################################ diff --git a/ros_ws/src/control/include/control/motor_controller.h b/ros_ws/src/control/include/control/motor_controller.h index 3c76e50..2b49110 100644 --- a/ros_ws/src/control/include/control/motor_controller.h +++ b/ros_ws/src/control/include/control/motor_controller.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include #include "constants.h" /* Custom messages - This file should be Auto generated */ @@ -38,6 +40,11 @@ class MotorController */ double unscaled_max_speed; + /** + * @brief The encoder resolution in ticks/s + */ + double encoder_res; + /** * @brief The previous left encoder time read */ @@ -66,12 +73,12 @@ class MotorController /** * @brief The number of ticks read from the left encoder in a time interval */ - control::Encoder left_ticks; + double left_measured_dist; /** * @brief The number of ticks read from the right encoder in a time interval */ - control::Encoder right_ticks; + double right_measured_dist; /** * @brief The subscriber for the left encoder diff --git a/ros_ws/src/control/package.xml b/ros_ws/src/control/package.xml index 87a9313..388ce62 100644 --- a/ros_ws/src/control/package.xml +++ b/ros_ws/src/control/package.xml @@ -19,10 +19,14 @@ std_msgs sensor_msgs message_generation + nav_msgs + tf geometry_msgs roscpp std_msgs sensor_msgs message_runtime + nav_msgs + tf diff --git a/ros_ws/src/control/param/igvc.yaml b/ros_ws/src/control/param/igvc.yaml index bb7d289..457f6df 100644 --- a/ros_ws/src/control/param/igvc.yaml +++ b/ros_ws/src/control/param/igvc.yaml @@ -3,4 +3,5 @@ wheel_rad: 0.19 max_speed: 5.0 max_lin_vel: 1.0 max_ang_vel: 1.0 +encoder_res: 100 diff --git a/ros_ws/src/control/src/control/motor_controller.cpp b/ros_ws/src/control/src/control/motor_controller.cpp index c14637e..fa0d7f5 100644 --- a/ros_ws/src/control/src/control/motor_controller.cpp +++ b/ros_ws/src/control/src/control/motor_controller.cpp @@ -38,6 +38,10 @@ MotorController::MotorController() { ROS_ERROR("max_ang_vel is not defined on the parameter server"); } + if(!nh.getParam("/encoder_res", encoder_res)) + { + ROS_ERROR("encoder_res is not defined on the parameter server"); + } unscaled_max_speed = (2 * max_lin_vel + max_ang_vel * robot_base) / (2 * wheel_rad); } @@ -50,12 +54,23 @@ void MotorController::twistCallback(const geometry_msgs::Twist::ConstPtr& msg) void MotorController::rightEncoderCallback(const control::Encoder::ConstPtr& msg) { + double dt = (msg->header.stamp.toSec() - right_prev_time.toSec()); + + //Calculate wheel velocity in m/s + right_measured_dist = (static_cast(msg->ticks) * wheel_rad * 2 * M_PI)/encoder_res; + //Set the previous time for next calculation + right_prev_time = msg->header.stamp; } void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) { + double dt = (msg->header.stamp.toSec() - right_prev_time.toSec()); + //Calculate wheel velocity in m/s + left_measured_dist = (static_cast(msg->ticks) * wheel_rad * 2 * M_PI)/encoder_res; + + left_prev_time = msg->header.stamp; } double MotorController::getRightVel(const double lin_vel, const double ang_vel) @@ -78,6 +93,28 @@ double MotorController::scale(const double val, const double pre_min, const doub void MotorController::update() { + //Calculate robot linear and angular velocities + tf::TransformBroadcaster odom_broadcaster; + geometry_msgs::TransformStamped odom_trans; + geometry_msgs::Quaternion rot; + double trans, theta; + + theta = (right_measured_dist - left_measured_dist) / robot_base; + tf::quaternionTFToMsg(tf::createQuaternionFromYaw(theta), rot); + trans = (right_measured_dist + left_measured_dist) / 2; + + //Build odometry header message + odom_trans.header.stamp = ros::Time::now(); + odom_trans.child_frame_id = "odom"; + odom_trans.child_frame_id = "base_link"; + + odom_trans.transform.translation.x = trans * cos(theta); + odom_trans.transform.translation.y = trans * sin(theta); + odom_trans.transform.translation.z = 0; + odom_trans.transform.rotation = rot; + + odom_broadcaster.sendTransform(odom_trans); + right_vel_pub.publish(right_vel); left_vel_pub.publish(left_vel); } From ca128df56605bf2f3794711b3ec2e88cb5abc139 Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Thu, 21 Jan 2016 14:02:51 -0600 Subject: [PATCH 08/13] Fix recomended changes to attempt to fix encoders --- ros_ws/src/arudino/motor/motor.ino | 34 +++++++++++++++++++----------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index 1d3e432..f7a2e35 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -78,12 +78,17 @@ const int ENABLE_PIN = A3; */ float desired_speed = 0; +/** + * @brief Boolean used to store the desired direction + * + * True is Forward, and False is Backwards + */ +float desired_direction; + /** * @brief Values to be updated when the inturrupt is triggered for encoder */ volatile unsigned int encoder_ticks = 0; -volatile int encoder_pos_last = LOW; -volatile int encoder_pos_current = LOW; /** * @brief Values used to keep track of current time for multitasking @@ -147,13 +152,11 @@ void setup() node_handle.advertise(encoderPub); //Set the Inturupt on Pin 2 - attachInterrupt(0, encoderCount, CHANGE); + attachInterrupt(0, encoderCount, RISING); } void loop() { - node_handle.spinOnce(); - updateEncoder(); //update the current time for multitasking @@ -164,6 +167,8 @@ void loop() { calculateSpeed(); } + + node_handle.spinOnce(); } /** @@ -177,6 +182,9 @@ void loop() void velocityCallback(const std_msgs::Float64& msg) { desired_speed = fScale(0, MAX_RAD_SEC, 0, 255, abs(msg.data), 0); + //If msg.data is positive, the set to TRUE + desired_direction = (msg.data > 0); + if(msg.data > 0) { //Go Forward @@ -214,6 +222,9 @@ void updateEncoder() //publish message encoderPub.publish(&encoderMessage); + + //reset the count to 0 + encoder_ticks = 0; } /** @@ -225,15 +236,14 @@ void updateEncoder() */ void encoderCount() { - encoder_pos_current = digitalRead(ENCODER_PIN); - - if ((encoder_pos_last == LOW) && - (encoder_pos_current == HIGH)) - { + if(desired_direction) + { encoder_ticks++; } - - encoder_pos_last = encoder_pos_current; + else + { + encoder_ticks--; + } } /** From 8793545b22c843593b6812524129bfdec82e70af Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Thu, 21 Jan 2016 16:36:02 -0600 Subject: [PATCH 09/13] Fix error i made while merging --- ros_ws/src/arudino/motor/motor.ino | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index 42649f6..134eef8 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -26,7 +26,7 @@ * This will be used to insure that the proper wheel gets the * Proper commands */ -#define LEFT_WHEEL +#define RIGHT_WHEEL #if defined RIGHT_WHEEL const char* VELOCITY_TOPIC = "right_vel"; @@ -91,12 +91,6 @@ float desired_speed = 0; * True is Forward, and False is Backwards */ float desired_direction; -/** - * @brief Place to Change which Wheel we want to Program - * - * True is Forward, and False is Backwards - */ -float desired_direction; /** * @brief Values to be updated when the inturrupt is triggered for encoder @@ -122,6 +116,7 @@ volatile unsigned int encoder_ticks = 0; void encoderCount(); void calculateSpeed(); void updateEncoder(); +void set_pwm_frequency(int divisor); void velocityCallback(const std_msgs::Float64& msg); float fScale( float originalMin, float originalMax, float newBegin, float newEnd, float inputValue, float curve); From e4de6af17821d9a5cec024e3a9f3e8bd8d655619 Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Sun, 24 Jan 2016 13:59:13 -0600 Subject: [PATCH 10/13] Add joint publisher The code now handles the encoder readings using joints. It will update the wheel joints using a joint_state_publisher. --- .../include/control/motor_controller.h | 26 +++------ ros_ws/src/control/launch/igvc.launch | 4 +- .../control/src/control/motor_controller.cpp | 55 ++++++++----------- 3 files changed, 34 insertions(+), 51 deletions(-) diff --git a/ros_ws/src/control/include/control/motor_controller.h b/ros_ws/src/control/include/control/motor_controller.h index 2b49110..b7a6894 100644 --- a/ros_ws/src/control/include/control/motor_controller.h +++ b/ros_ws/src/control/include/control/motor_controller.h @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include "constants.h" @@ -45,16 +45,6 @@ class MotorController */ double encoder_res; - /** - * @brief The previous left encoder time read - */ - ros::Time left_prev_time; - - /** - * @brief The previous right encoder time read - */ - ros::Time right_prev_time; - /** * @brief The right velocity to be published */ @@ -71,14 +61,9 @@ class MotorController ros::Subscriber twist_sub; /** - * @brief The number of ticks read from the left encoder in a time interval + * @brief The joint state message to be published containing the left and right wheel positions */ - double left_measured_dist; - - /** - * @brief The number of ticks read from the right encoder in a time interval - */ - double right_measured_dist; + sensor_msgs::JointState joint_state; /** * @brief The subscriber for the left encoder @@ -100,6 +85,11 @@ class MotorController */ ros::Publisher left_vel_pub; + /** + * @brief The joint state publisher for the left and right wheel positions + */ + ros::Publisher joint_state_pub; + /** * @brief The ros node handle */ diff --git a/ros_ws/src/control/launch/igvc.launch b/ros_ws/src/control/launch/igvc.launch index 61884b6..48d674b 100644 --- a/ros_ws/src/control/launch/igvc.launch +++ b/ros_ws/src/control/launch/igvc.launch @@ -4,5 +4,7 @@ - + + ["wheel_state"] + \ No newline at end of file diff --git a/ros_ws/src/control/src/control/motor_controller.cpp b/ros_ws/src/control/src/control/motor_controller.cpp index fa0d7f5..befac6d 100644 --- a/ros_ws/src/control/src/control/motor_controller.cpp +++ b/ros_ws/src/control/src/control/motor_controller.cpp @@ -16,6 +16,7 @@ MotorController::MotorController() left_encoder = nh.subscribe(LEFT_ENCODER_TOPIC, 5, &MotorController::leftEncoderCallback, this); right_encoder = nh.subscribe(RIGHT_ENCODER_TOPIC, 5, &MotorController::rightEncoderCallback, this); + joint_state_pub = nh.advertise("wheel_states", 1); //Check that the necessary parameters exist and set member variables appropriately if(!nh.getParam("/robot_base", robot_base)) @@ -42,6 +43,14 @@ MotorController::MotorController() { ROS_ERROR("encoder_res is not defined on the parameter server"); } + + //Setup the joint state message + joint_state.name.resize(2); + joint_state.position.resize(2); + joint_state.name[0] = "left_wheel_joint"; + joint_state.name[1] = "right_wheel_joint"; + joint_state.position[0] = 0; + joint_state.position[1] = 0; unscaled_max_speed = (2 * max_lin_vel + max_ang_vel * robot_base) / (2 * wheel_rad); } @@ -54,23 +63,24 @@ void MotorController::twistCallback(const geometry_msgs::Twist::ConstPtr& msg) void MotorController::rightEncoderCallback(const control::Encoder::ConstPtr& msg) { - double dt = (msg->header.stamp.toSec() - right_prev_time.toSec()); - - //Calculate wheel velocity in m/s - right_measured_dist = (static_cast(msg->ticks) * wheel_rad * 2 * M_PI)/encoder_res; + //Calculate rotational position of the wheel + joint_state.position[1] += ((2 * M_PI / encoder_res) * msg->ticks) - M_PI; - //Set the previous time for next calculation - right_prev_time = msg->header.stamp; + if(joint_state.position[1] > M_PI) + joint_state.position[1] -= 2*M_PI; + else if(joint_state.position[1] < -M_PI) + joint_state.position[1] += 2*M_PI; } void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) { - double dt = (msg->header.stamp.toSec() - right_prev_time.toSec()); + //Calculate rotational position of the wheel + joint_state.position[0] += ((2 * M_PI / encoder_res) * msg->ticks) - M_PI; - //Calculate wheel velocity in m/s - left_measured_dist = (static_cast(msg->ticks) * wheel_rad * 2 * M_PI)/encoder_res; - - left_prev_time = msg->header.stamp; + if(joint_state.position[0] > M_PI) + joint_state.position[0] -= 2*M_PI; + else if(joint_state.position[0] < -M_PI) + joint_state.position[0] += 2*M_PI; } double MotorController::getRightVel(const double lin_vel, const double ang_vel) @@ -93,27 +103,8 @@ double MotorController::scale(const double val, const double pre_min, const doub void MotorController::update() { - //Calculate robot linear and angular velocities - tf::TransformBroadcaster odom_broadcaster; - geometry_msgs::TransformStamped odom_trans; - geometry_msgs::Quaternion rot; - double trans, theta; - - theta = (right_measured_dist - left_measured_dist) / robot_base; - tf::quaternionTFToMsg(tf::createQuaternionFromYaw(theta), rot); - trans = (right_measured_dist + left_measured_dist) / 2; - - //Build odometry header message - odom_trans.header.stamp = ros::Time::now(); - odom_trans.child_frame_id = "odom"; - odom_trans.child_frame_id = "base_link"; - - odom_trans.transform.translation.x = trans * cos(theta); - odom_trans.transform.translation.y = trans * sin(theta); - odom_trans.transform.translation.z = 0; - odom_trans.transform.rotation = rot; - - odom_broadcaster.sendTransform(odom_trans); + joint_state.header.stamp = ros::Time::now(); + joint_state_pub.publish(joint_state); right_vel_pub.publish(right_vel); left_vel_pub.publish(left_vel); From b537c38c1cfd73a8574e55fc6184cc1176bb9b2c Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Sun, 24 Jan 2016 14:10:50 -0600 Subject: [PATCH 11/13] Update code to remove unecassarry functions --- ros_ws/src/arudino/motor/motor.ino | 72 +++++++----------------------- 1 file changed, 15 insertions(+), 57 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index 134eef8..fcf2632 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -7,8 +7,6 @@ * */ -#define USB_CON - #include #include #include @@ -26,38 +24,28 @@ * This will be used to insure that the proper wheel gets the * Proper commands */ -#define RIGHT_WHEEL +#define R_WHEEL -#if defined RIGHT_WHEEL +#if defined R_WHEEL const char* VELOCITY_TOPIC = "right_vel"; const char* ENCODER_TOPIC = "right_tick"; -#elif defined LEFT_WHEEL +#elif defined L_WHEEL const char* VELOCITY_TOPIC = "left_vel"; const char* ENCODER_TOPIC = "left_tick"; #endif -/** - * @brief Define Pi for use in conversion - */ -const float PI_CONST = 3.14159265359; - /** * @brief value for the desierd refresh rate of the encoder * - * Set to 30Hz - */ - const float REFRESH_RATE = 33.3; - -/** - * @brief Define constants for the Encoder Calculation + * Variable is the number of Millisecond Delay between publishes */ -const float WHEEL_RADIUS = 8.5; //Wheel Radius in inches -const float WHEEL_CIRCUMFRANCE = (WHEEL_RADIUS*PI_CONST*2); -const int GEAR_RATIO = 2; -const int TICKS_PER_ROTATION = 200/GEAR_RATIO; + const float REFRESH_RATE = 1000; /** * @brief Radians Per Second at full Throtle + * + * todo !!!!!!!STILL NEEDS TO BE UPDATED!!!!!!!!! + * */ const float MAX_RAD_SEC = 5; @@ -69,8 +57,8 @@ const int ENCODER_PIN = 2; /** * @brief Pins used to control the Motor */ +const int REVERSE_PWM_PIN = 4; const int FORWARD_PWM_PIN = 5; -const int REVERSE_PWM_PIN = 6; const int ENABLE_PIN = A3; /** @@ -103,18 +91,10 @@ volatile unsigned int encoder_ticks = 0; int current_mills = 0; int old_mills = 0; - /** - * @brief Values used to calculate speed from the encoder - */ - int ticks_per_cycle = 0; - int ticks_per_cycle_old = 0; - float current_speed = 0; - /************************ * Forward Declerations * ************************/ void encoderCount(); -void calculateSpeed(); void updateEncoder(); void set_pwm_frequency(int divisor); void velocityCallback(const std_msgs::Float64& msg); @@ -169,17 +149,17 @@ void setup() void loop() { - updateEncoder(); - //update the current time for multitasking current_mills = millis(); - //Get the speed of wheel at a rate of 30Htz - if(current_mills-old_mills >= REFRESH_RATE) + //Only update the Encoder date when the Refresh Rate says to + if(abs(current_mills-old_mills) >= REFRESH_RATE) { - calculateSpeed(); + updateEncoder(); + old_mills = current_mills; } - + + //Only used to get the messages from ROS node_handle.spinOnce(); } @@ -258,28 +238,6 @@ void encoderCount() } } -/** - * @brief The Function will calculate the current speed - * - * This function is called once durring each refresh rate - * It calculates the current speed based on the encoder readings - */ -void calculateSpeed() -{ - ticks_per_cycle = encoder_ticks; - - //calculates speed in (inces per .03 seconds) - //************************************************* - //TEMPORARY --WILL UPDATE WITH PROPER UNITS LATER-- - //************************************************* - current_speed = ((ticks_per_cycle-ticks_per_cycle_old) / - (TICKS_PER_ROTATION*WHEEL_CIRCUMFRANCE)); - - //Update the following values so that the next cycle works correctlly - ticks_per_cycle_old = ticks_per_cycle; - old_mills = current_mills; -} - /** * @brief The Function will Scale the Input to the Expected Output * From 76822eaf264cbdfb1a19dae02ee8e019d012e71f Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Sun, 24 Jan 2016 15:26:39 -0600 Subject: [PATCH 12/13] Add odometry publishing The odometry transform is now published based on encoder ticks. --- .../include/control/motor_controller.h | 44 +++++++++++++---- ros_ws/src/control/launch/igvc.launch | 2 +- .../control/src/control/motor_controller.cpp | 48 ++++++++++++++----- 3 files changed, 73 insertions(+), 21 deletions(-) diff --git a/ros_ws/src/control/include/control/motor_controller.h b/ros_ws/src/control/include/control/motor_controller.h index b7a6894..bf2f43e 100644 --- a/ros_ws/src/control/include/control/motor_controller.h +++ b/ros_ws/src/control/include/control/motor_controller.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include "constants.h" @@ -55,6 +56,31 @@ class MotorController */ std_msgs::Float64 left_vel; + /** + * @brief The linear displacement of the left wheel since the last message + */ + double displacement_left; + + /** + * @brief The linear displacement of the right wheel since the last message + */ + double displacement_right; + + /** + * @brief The previous angular displacement of the robot + */ + double prev_theta; + + /** + * @brief The tf transform to be published specifying odometry + */ + geometry_msgs::TransformStamped odom_trans; + + /** + * @brief The tf broadcaster for the odom transform + */ + tf::TransformBroadcaster odom_broadcaster; + /** * @brief The twist message subscriber */ @@ -106,22 +132,22 @@ class MotorController void twistCallback(const geometry_msgs::Twist::ConstPtr& msg); /** - * @brief The callback function for the right encoder message - * - * This function accepts Encoder messages and records the number of ticks in a time interval for the right wheel - * - * @param msg The message which is received from the right encoder publisher - */ - void rightEncoderCallback(const control::Encoder::ConstPtr& msg); + * @brief The callback function for the right encoder message + * + * This function accepts Encoder messages and records the number of ticks in a time interval for the left wheel + * + * @param msg The message which is received from the right encoder publisher + */ + void leftEncoderCallback(const control::Encoder::ConstPtr& msg); /** * @brief The callback function for the right encoder message * - * This function accepts Encoder messages and records the number of ticks in a time interval for the left wheel + * This function accepts Encoder messages and records the number of ticks in a time interval for the right wheel * * @param msg The message which is received from the right encoder publisher */ - void leftEncoderCallback(const control::Encoder::ConstPtr& msg); + void rightEncoderCallback(const control::Encoder::ConstPtr& msg); /** * @brief Calculates the right wheel angular velocity based on a differential drive model diff --git a/ros_ws/src/control/launch/igvc.launch b/ros_ws/src/control/launch/igvc.launch index 48d674b..69dcf6f 100644 --- a/ros_ws/src/control/launch/igvc.launch +++ b/ros_ws/src/control/launch/igvc.launch @@ -5,6 +5,6 @@ - ["wheel_state"] + ["wheel_states"] \ No newline at end of file diff --git a/ros_ws/src/control/src/control/motor_controller.cpp b/ros_ws/src/control/src/control/motor_controller.cpp index befac6d..e8becae 100644 --- a/ros_ws/src/control/src/control/motor_controller.cpp +++ b/ros_ws/src/control/src/control/motor_controller.cpp @@ -44,6 +44,15 @@ MotorController::MotorController() ROS_ERROR("encoder_res is not defined on the parameter server"); } + //Setup odom tf link + odom_trans.header.stamp = ros::Time::now(); + odom_trans.header.frame_id = "odom"; + odom_trans.child_frame_id = "base_link"; + odom_trans.transform.translation.x = 0; + odom_trans.transform.translation.y = 0; + odom_trans.transform.translation.z = 0; + prev_theta = 0; + //Setup the joint state message joint_state.name.resize(2); joint_state.position.resize(2); @@ -61,19 +70,10 @@ void MotorController::twistCallback(const geometry_msgs::Twist::ConstPtr& msg) left_vel.data = getLeftVel(msg->linear.x, msg->angular.z); } -void MotorController::rightEncoderCallback(const control::Encoder::ConstPtr& msg) -{ - //Calculate rotational position of the wheel - joint_state.position[1] += ((2 * M_PI / encoder_res) * msg->ticks) - M_PI; - - if(joint_state.position[1] > M_PI) - joint_state.position[1] -= 2*M_PI; - else if(joint_state.position[1] < -M_PI) - joint_state.position[1] += 2*M_PI; -} - void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) { + displacement_left = ((M_PI * robot_base) / encoder_res) * msg->ticks; + //Calculate rotational position of the wheel joint_state.position[0] += ((2 * M_PI / encoder_res) * msg->ticks) - M_PI; @@ -83,6 +83,19 @@ void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) joint_state.position[0] += 2*M_PI; } +void MotorController::rightEncoderCallback(const control::Encoder::ConstPtr& msg) +{ + displacement_right = ((M_PI * robot_base) / encoder_res) * msg->ticks; + + //Calculate rotational position of the wheel + joint_state.position[1] += ((2 * M_PI / encoder_res) * msg->ticks) - M_PI; + + if(joint_state.position[1] > M_PI) + joint_state.position[1] -= 2*M_PI; + else if(joint_state.position[1] < -M_PI) + joint_state.position[1] += 2*M_PI; +} + double MotorController::getRightVel(const double lin_vel, const double ang_vel) { double pre_scaled = (2 * lin_vel + ang_vel * robot_base) / (2 * wheel_rad); @@ -106,6 +119,19 @@ void MotorController::update() joint_state.header.stamp = ros::Time::now(); joint_state_pub.publish(joint_state); + odom_trans.header.stamp = ros::Time::now(); + + double displacement = (displacement_right + displacement_left) / 2; + double theta = (displacement_right - displacement_left) / robot_base; + + odom_trans.transform.translation.x += displacement * cos(theta); + odom_trans.transform.translation.y += displacement * sin(theta); + odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(prev_theta + theta); + + odom_broadcaster.sendTransform(odom_trans); + + prev_theta += theta; + right_vel_pub.publish(right_vel); left_vel_pub.publish(left_vel); } From 11e5f019cad69255eb3a3e83f717e3b325c974db Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Sun, 31 Jan 2016 13:47:46 -0600 Subject: [PATCH 13/13] Fix conversion to linear displacement The conversion to linear displacement used the robot radius rather than the wheel radius. --- ros_ws/src/control/src/control/motor_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_ws/src/control/src/control/motor_controller.cpp b/ros_ws/src/control/src/control/motor_controller.cpp index e8becae..d9999b9 100644 --- a/ros_ws/src/control/src/control/motor_controller.cpp +++ b/ros_ws/src/control/src/control/motor_controller.cpp @@ -72,7 +72,7 @@ void MotorController::twistCallback(const geometry_msgs::Twist::ConstPtr& msg) void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) { - displacement_left = ((M_PI * robot_base) / encoder_res) * msg->ticks; + displacement_left = ((2 * M_PI * wheel_rad) / encoder_res) * msg->ticks; //Calculate rotational position of the wheel joint_state.position[0] += ((2 * M_PI / encoder_res) * msg->ticks) - M_PI; @@ -85,7 +85,7 @@ void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) void MotorController::rightEncoderCallback(const control::Encoder::ConstPtr& msg) { - displacement_right = ((M_PI * robot_base) / encoder_res) * msg->ticks; + displacement_right = ((2 * M_PI * wheel_rad) / encoder_res) * msg->ticks; //Calculate rotational position of the wheel joint_state.position[1] += ((2 * M_PI / encoder_res) * msg->ticks) - M_PI;