diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index b361a4a..bc3f0f3 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -7,25 +7,54 @@ * */ -#define USB_CON - #include #include +#include +#include /************* * Constants * *************/ /** - * @brief Define Pi for use in conversion + * @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 */ -const float PI_CONST = 3.14159265359; +#define R_WHEEL + +#if defined R_WHEEL +const char* VELOCITY_TOPIC = "right_vel"; +const char* ENCODER_TOPIC = "right_tick"; +#elif defined L_WHEEL +const char* VELOCITY_TOPIC = "left_vel"; +const char* ENCODER_TOPIC = "left_tick"; +#endif + + +/** + * @brief value for the desierd refresh rate of the encoder + * + * Variable is the number of Millisecond Delay between publishes + */ + const float REFRESH_RATE = 1000; /** * @brief Radians Per Second at full Throtle + * + * todo !!!!!!!STILL NEEDS TO BE UPDATED!!!!!!!!! + * */ const float MAX_RAD_SEC = 5; +/** + * @brief Pin the encoder is attached + */ +const int ENCODER_PIN = 2; + /** * @brief Pins used to control the Motor */ @@ -41,43 +70,101 @@ const int ENABLE_PIN = A3; const int FREQUENCY_DIVISOR = 8; /** - * @brief Place to Change which Wheel we want to Program + * @brief Float used to scale the Speed to + */ +float desired_speed = 0; + +/** + * @brief Boolean used to store the desired direction * - * 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 + * True is Forward, and False is Backwards */ -#define RIGHT_WHEEL +float desired_direction; -#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; /** - * @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; /************************ * Forward Declerations * ************************/ +void encoderCount(); +void updateEncoder(); +void set_pwm_frequency(int divisor); 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); /** * @brief ROS node handle */ -ros::NodeHandle nodeHandle; +ros::NodeHandle node_handle; + +/** + * @brief ROS message used for publishing the encoder data + */ +control::Encoder 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() +{ + //Fix the Motor Whine + //After testing on IGVC 8 gave the best results + set_pwm_frequency(FREQUENCY_DIVISOR); + + //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, RISING); +} + +void loop() +{ + + //update the current time for multitasking + current_mills = millis(); + + //Only update the Encoder date when the Refresh Rate says to + if(abs(current_mills-old_mills) >= REFRESH_RATE) + { + updateEncoder(); + old_mills = current_mills; + } + + //Only used to get the messages from ROS + node_handle.spinOnce(); +} + /** * @brief The callback function for velocity messages * @@ -88,12 +175,15 @@ 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); + 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 digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, speed); + analogWrite(FORWARD_PWM_PIN, desired_speed); analogWrite(REVERSE_PWM_PIN, 0); } else if(msg.data < 0) @@ -101,7 +191,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, desired_speed); } else { @@ -112,36 +202,44 @@ void velocityCallback(const std_msgs::Float64& msg) } } -void setup() +/** + * @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() { - //Fix the Motor Whine - //After testing on IGVC 8 gave the best results - set_pwm_frequency(FREQUENCY_DIVISOR); - - //setup pins - pinMode(FORWARD_PWM_PIN, OUTPUT); - pinMode(REVERSE_PWM_PIN, OUTPUT); - pinMode(ENABLE_PIN, OUTPUT); - - //Set Robot to break when starting - digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, 0); - analogWrite(REVERSE_PWM_PIN, 0); + //update the value of the message + encoderMessage.ticks = encoder_ticks; + encoderMessage.header.stamp = node_handle.now(); + + //publish message + encoderPub.publish(&encoderMessage); - //Setup ROS node and topics - nodeHandle.initNode(); - nodeHandle.subscribe(velocitySub); + //reset the count to 0 + encoder_ticks = 0; } -void loop() +/** + * @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 encoder_ticks + */ +void encoderCount() { - nodeHandle.spinOnce(); + if(desired_direction) + { + encoder_ticks++; + } + else + { + encoder_ticks--; + } } - - - - /** * @brief The Function will Scale the Input to the Expected Output * @@ -159,16 +257,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 @@ -189,53 +287,52 @@ 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; } - /** * @brief The Function will change frequency of Timer 0 * 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/CMakeLists.txt b/ros_ws/src/control/CMakeLists.txt index 27616ba..f044b4a 100644 --- a/ros_ws/src/control/CMakeLists.txt +++ b/ros_ws/src/control/CMakeLists.txt @@ -6,8 +6,18 @@ find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs + message_generation + nav_msgs + tf ) +################################################ +## 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 ## ################################### @@ -16,6 +26,7 @@ catkin_package( LIBRARIES control CATKIN_DEPENDS geometry_msgs roscpp std_msgs sensor_msgs DEPENDS system_lib + CATKIN_DEPENDS message_runtime ) ########### diff --git a/ros_ws/src/control/include/control/motor_controller.h b/ros_ws/src/control/include/control/motor_controller.h index a5f73ce..bf2f43e 100644 --- a/ros_ws/src/control/include/control/motor_controller.h +++ b/ros_ws/src/control/include/control/motor_controller.h @@ -10,8 +10,14 @@ #include #include #include +#include +#include +#include #include "constants.h" +/* Custom messages - This file should be Auto generated */ +#include "control/Encoder.h" + class MotorController { private: @@ -35,6 +41,11 @@ class MotorController */ double unscaled_max_speed; + /** + * @brief The encoder resolution in ticks/s + */ + double encoder_res; + /** * @brief The right velocity to be published */ @@ -45,11 +56,51 @@ 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 */ ros::Subscriber twist_sub; + /** + * @brief The joint state message to be published containing the left and right wheel positions + */ + sensor_msgs::JointState joint_state; + + /** + * @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 */ @@ -60,6 +111,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 */ @@ -75,6 +131,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 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 right wheel + * + * @param msg The message which is received from the right encoder publisher + */ + void rightEncoderCallback(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/launch/igvc.launch b/ros_ws/src/control/launch/igvc.launch index 61884b6..69dcf6f 100644 --- a/ros_ws/src/control/launch/igvc.launch +++ b/ros_ws/src/control/launch/igvc.launch @@ -4,5 +4,7 @@ - + + ["wheel_states"] + \ No newline at end of file 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..388ce62 100644 --- a/ros_ws/src/control/package.xml +++ b/ros_ws/src/control/package.xml @@ -18,9 +18,15 @@ roscpp std_msgs sensor_msgs + message_generation + nav_msgs + tf geometry_msgs roscpp std_msgs sensor_msgs + message_runtime + nav_msgs + tf - \ No newline at end of file + 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 0d1ff8d..d9999b9 100644 --- a/ros_ws/src/control/src/control/motor_controller.cpp +++ b/ros_ws/src/control/src/control/motor_controller.cpp @@ -13,6 +13,10 @@ 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); + 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)) @@ -35,6 +39,27 @@ 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"); + } + + //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); + 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); } @@ -45,6 +70,32 @@ void MotorController::twistCallback(const geometry_msgs::Twist::ConstPtr& msg) left_vel.data = getLeftVel(msg->linear.x, msg->angular.z); } +void MotorController::leftEncoderCallback(const control::Encoder::ConstPtr& msg) +{ + 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; + + 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; +} + +void MotorController::rightEncoderCallback(const control::Encoder::ConstPtr& msg) +{ + 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; + + 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); @@ -65,6 +116,22 @@ double MotorController::scale(const double val, const double pre_min, const doub 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); }