From 5bb1abe09988af6834ec41b266faf2b07debb13b Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Sun, 1 Nov 2015 23:44:38 -0600 Subject: [PATCH 01/12] 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/12] 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/12] 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/12] 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/12] 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 ca128df56605bf2f3794711b3ec2e88cb5abc139 Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Thu, 21 Jan 2016 14:02:51 -0600 Subject: [PATCH 06/12] 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 07/12] 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 b537c38c1cfd73a8574e55fc6184cc1176bb9b2c Mon Sep 17 00:00:00 2001 From: Ryan Loffelman Date: Sun, 24 Jan 2016 14:10:50 -0600 Subject: [PATCH 08/12] 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 360edf98da04b4bd99b86cfdb18ac1666c1954c1 Mon Sep 17 00:00:00 2001 From: Ryan Loeffelman Date: Sun, 31 Jan 2016 12:20:42 -0600 Subject: [PATCH 09/12] Update to attempt to fix encoder code --- ros_ws/src/arudino/motor/motor.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros_ws/src/arudino/motor/motor.ino b/ros_ws/src/arudino/motor/motor.ino index fcf2632..8f05767 100644 --- a/ros_ws/src/arudino/motor/motor.ino +++ b/ros_ws/src/arudino/motor/motor.ino @@ -57,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; /** @@ -153,7 +153,7 @@ void loop() current_mills = millis(); //Only update the Encoder date when the Refresh Rate says to - if(abs(current_mills-old_mills) >= REFRESH_RATE) + if(current_mills-old_mills >= REFRESH_RATE) { updateEncoder(); old_mills = current_mills; From 73bb8b469d2afa8a6c63b550a75a85c8cd4bce26 Mon Sep 17 00:00:00 2001 From: RyanLoeffelman Date: Sun, 31 Jan 2016 14:34:28 -0600 Subject: [PATCH 10/12] Renamed the misspelled Arduino Folder --- ros_ws/src/arudino/imu/imu.ino | 179 -------------- ros_ws/src/arudino/motor/motor.ino | 370 ----------------------------- 2 files changed, 549 deletions(-) delete mode 100644 ros_ws/src/arudino/imu/imu.ino delete mode 100644 ros_ws/src/arudino/motor/motor.ino diff --git a/ros_ws/src/arudino/imu/imu.ino b/ros_ws/src/arudino/imu/imu.ino deleted file mode 100644 index 85389a3..0000000 --- a/ros_ws/src/arudino/imu/imu.ino +++ /dev/null @@ -1,179 +0,0 @@ -#include - -#define GYROADDR 0x68 -#define COMPASSADDR 0x1e -#define ACCELADDR 0x53 - -union XYZBuffer { - struct { - short x,y,z; - } value; - byte buff[6]; -}; - -void changeEndian(union XYZBuffer *xyz) { - for (int i=0;i<6;i+=2) { - byte t=xyz->buff[i]; - xyz->buff[i]=xyz->buff[i+1]; - xyz->buff[i+1]=t; - } -} - -// Generically useful reading into a union type -void readXYZ(int device,union XYZBuffer *xyz) { - Wire.requestFrom(device, 6); - long start=millis(); - while (!Wire.available() && (millis()-start)<100); - if (millis()-start<100) { - for (int i=0;i<6;i++) - xyz->buff[i]=Wire.read(); - } -} - -void setupAccel(int device) { - // Check ID to see if we are communicating - Wire.beginTransmission(device); - Wire.write(0x00); // One Reading - Wire.endTransmission(); - Wire.requestFrom(device,1); - while (!Wire.available()); - byte ch=Wire.read(); - Serial.print("Accel id is 0x"); - Serial.println(ch,HEX); - // Should output E5 - - // https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf - // Page 16 - Wire.beginTransmission(device); - Wire.write(0x2d); - Wire.write(0x08); - Wire.endTransmission(); - Wire.beginTransmission(device); - Wire.write(0x38); - Wire.write(0x84); - Wire.endTransmission(); - -} -void readAccel(int device,union XYZBuffer *xyz) { - Wire.beginTransmission(device); - Wire.write(0x32); // One Reading - Wire.endTransmission(); - readXYZ(device,xyz); -} - -void setupCompass(int device) { - // Check ID to see if we are communicating - Serial.print("Compass id is "); - Wire.beginTransmission(device); - Wire.write(10); // One Reading - Wire.endTransmission(); - Wire.requestFrom(device,2); - while (!Wire.available()); - char ch=Wire.read(); - Serial.print(ch); - ch=Wire.read(); - Serial.println(ch); - // Should output H4 - -// Page 18 -// at http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf - Wire.beginTransmission(device); - Wire.write(0x00); Wire.write(0x70); - Wire.endTransmission(); - Wire.beginTransmission(device); - Wire.write(0x01); Wire.write(0xA0); - Wire.endTransmission(); - Wire.beginTransmission(device); - Wire.write(0x02); Wire.write(0x00); // Reading - Wire.endTransmission(); - delay(6); -} -void readCompass(int device,union XYZBuffer *xyz) { - readXYZ(device,xyz); - changeEndian(xyz); - Wire.beginTransmission(device); - Wire.write(0x03); - Wire.endTransmission(); -} - -void setupGyro(int device) { - // Check ID to see if we are communicating - Wire.beginTransmission(device); - Wire.write(0x00); // One Reading - Wire.endTransmission(); - Wire.requestFrom(device,1); - while (!Wire.available()); - byte ch=Wire.read(); - Serial.print("Gyro id is 0x"); - Serial.println(ch,HEX); - // Should output 69 -} -void readGyro(int device,union XYZBuffer *xyz) { - // https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf - // page 20 - Wire.beginTransmission(device); - Wire.write(0x1d); - Wire.endTransmission(); - readXYZ(device,xyz); - changeEndian(xyz); -} - -void pad(int width,int number) { - int n=abs(number); - int w=width; - if (number<0) w--; - while (n>0) { - w--; - n/=10; - } - if (number==0) w--; - for (int i=0;i -#include -#include -#include - -/************* - * 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 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 - */ -const int FORWARD_PWM_PIN = 5; -const int REVERSE_PWM_PIN = 6; -const int ENABLE_PIN = A3; - -/** - * @brief Const used to change the Motor Whine Divisor - * - * This is set to 8 for IGVC Robot - */ -const int FREQUENCY_DIVISOR = 8; - -/** - * @brief Float used to scale the Speed to - */ -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; - -/** - * @brief Values used to keep track of current time for multitasking - */ - 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 newEnd, float inputValue, float curve); - -/** - * @brief ROS node handle - */ -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(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 - * - * This function accepts velocity messages and convertes them into wheel speeds - * This will convert a float into an intger(0-255) - * - * @param msg The message witch is recived from the velocity publisher - */ -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 - digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, desired_speed); - analogWrite(REVERSE_PWM_PIN, 0); - } - else if(msg.data < 0) - { - //Go in Reverse - digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, 0); - analogWrite(REVERSE_PWM_PIN, desired_speed); - } - else - { - //active break - digitalWrite(ENABLE_PIN, HIGH); - analogWrite(FORWARD_PWM_PIN, 0); - analogWrite(REVERSE_PWM_PIN, 0); - } -} - -/** - * @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 - encoderMessage.ticks = encoder_ticks; - encoderMessage.header.stamp = node_handle.now(); - - //publish message - encoderPub.publish(&encoderMessage); - - //reset the count to 0 - encoder_ticks = 0; -} - -/** - * @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() -{ - if(desired_direction) - { - encoder_ticks++; - } - else - { - encoder_ticks--; - } -} - -/** - * @brief The Function will Scale the Input to the Expected Output - * - * This function accepts the input value, and input range, - * It will the scale the value to the expected output inside - * the given output range - * This Function will also allow the input to be scaled - * with a curve, It is not onyl linear. - * - * @param originalMin - minimum input expected by function - * originalMax - maximum input expected by function - * newBegin - minimum input returned by the function - * newEnd - maximum input returned by the function - * inputValue - the value you want scaled - * 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 new_end, float input_value, float curve) -{ - - 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 - // limit range - if (curve > 10) - { - curve = 10; - } - if (curve < -10) - { - curve = -10; - } - - // - invert and scale - this seems more intuitive - - // postive numbers give more weight to high end on output - curve = (curve * -.1) ; - // convert linear scale into lograthimic exponent for other pow function - curve = pow(10, curve); - - // Check for out of range inputValues - if (input_value < original_min) - { - input_value = original_min; - } - if (input_value > original_max) - { - input_value = original_max; - } - - // Zero Refference the values - original_range = original_max - original_min; - - if (new_end > new_begin) - { - new_range = new_end - new_begin; - } - else - { - new_range = new_begin - new_end; - inv_flag = 1; - } - - zero_ref_cur_val = input_value - original_min; - - // normalize to 0 - 1 float - 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 (original_min > original_max ) - { - return 0; - } - - if (inv_flag == 0) - { - ranged_value = (pow(normalized_cur_val, curve) * new_range) + new_begin; - } - else // invert the ranges - { - ranged_value = new_begin - (pow(normalized_cur_val, curve) * new_range); - } - - return ranged_value; -} - -/** - * @brief The Function will change frequency of Timer 0 - * - * This function accepts as input a divisor that will change - * the frequency of the Timer that controlls the PWM signal - * - * @param divisor -what we divide the timer frequency by - */ -void set_pwm_frequency(int divisor) -{ - byte mode; - switch(divisor) - { - case 1: - mode = 0x01; - break; - case 8: - mode = 0x02; - break; - case 64: - mode = 0x03; - break; - case 256: - mode = 0x04; - break; - case 1024: - mode = 0x05; - break; - default: - return; - } - - //set mode of timer 0 - TCCR0B = TCCR0B & 0b11111000 | mode; - - return; -} From 2d2bcea29b5715563581171d08f3daf908f583d2 Mon Sep 17 00:00:00 2001 From: RyanLoeffelman Date: Thu, 18 Feb 2016 17:02:51 -0600 Subject: [PATCH 11/12] Renamed that arduino folder --- ros_ws/src/arduino/motor/motor.ino | 370 +++++++++++++++++++++++++++++ 1 file changed, 370 insertions(+) create mode 100644 ros_ws/src/arduino/motor/motor.ino diff --git a/ros_ws/src/arduino/motor/motor.ino b/ros_ws/src/arduino/motor/motor.ino new file mode 100644 index 0000000..8f05767 --- /dev/null +++ b/ros_ws/src/arduino/motor/motor.ino @@ -0,0 +1,370 @@ +/** + * @file motor.ino + * @brief Controls the Motors + * + * Code to be put on the Arduino based Motor Controllers + * Converts Angular Velocity (Rad/Sec) to float (0-255) + * + */ + +#include +#include +#include +#include + +/************* + * 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 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 + */ +const int FORWARD_PWM_PIN = 5; +const int REVERSE_PWM_PIN = 6; +const int ENABLE_PIN = A3; + +/** + * @brief Const used to change the Motor Whine Divisor + * + * This is set to 8 for IGVC Robot + */ +const int FREQUENCY_DIVISOR = 8; + +/** + * @brief Float used to scale the Speed to + */ +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; + +/** + * @brief Values used to keep track of current time for multitasking + */ + 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 newEnd, float inputValue, float curve); + +/** + * @brief ROS node handle + */ +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(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 + * + * This function accepts velocity messages and convertes them into wheel speeds + * This will convert a float into an intger(0-255) + * + * @param msg The message witch is recived from the velocity publisher + */ +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 + digitalWrite(ENABLE_PIN, HIGH); + analogWrite(FORWARD_PWM_PIN, desired_speed); + analogWrite(REVERSE_PWM_PIN, 0); + } + else if(msg.data < 0) + { + //Go in Reverse + digitalWrite(ENABLE_PIN, HIGH); + analogWrite(FORWARD_PWM_PIN, 0); + analogWrite(REVERSE_PWM_PIN, desired_speed); + } + else + { + //active break + digitalWrite(ENABLE_PIN, HIGH); + analogWrite(FORWARD_PWM_PIN, 0); + analogWrite(REVERSE_PWM_PIN, 0); + } +} + +/** + * @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 + encoderMessage.ticks = encoder_ticks; + encoderMessage.header.stamp = node_handle.now(); + + //publish message + encoderPub.publish(&encoderMessage); + + //reset the count to 0 + encoder_ticks = 0; +} + +/** + * @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() +{ + if(desired_direction) + { + encoder_ticks++; + } + else + { + encoder_ticks--; + } +} + +/** + * @brief The Function will Scale the Input to the Expected Output + * + * This function accepts the input value, and input range, + * It will the scale the value to the expected output inside + * the given output range + * This Function will also allow the input to be scaled + * with a curve, It is not onyl linear. + * + * @param originalMin - minimum input expected by function + * originalMax - maximum input expected by function + * newBegin - minimum input returned by the function + * newEnd - maximum input returned by the function + * inputValue - the value you want scaled + * 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 new_end, float input_value, float curve) +{ + + 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 + // limit range + if (curve > 10) + { + curve = 10; + } + if (curve < -10) + { + curve = -10; + } + + // - invert and scale - this seems more intuitive - + // postive numbers give more weight to high end on output + curve = (curve * -.1) ; + // convert linear scale into lograthimic exponent for other pow function + curve = pow(10, curve); + + // Check for out of range inputValues + if (input_value < original_min) + { + input_value = original_min; + } + if (input_value > original_max) + { + input_value = original_max; + } + + // Zero Refference the values + original_range = original_max - original_min; + + if (new_end > new_begin) + { + new_range = new_end - new_begin; + } + else + { + new_range = new_begin - new_end; + inv_flag = 1; + } + + zero_ref_cur_val = input_value - original_min; + + // normalize to 0 - 1 float + 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 (original_min > original_max ) + { + return 0; + } + + if (inv_flag == 0) + { + ranged_value = (pow(normalized_cur_val, curve) * new_range) + new_begin; + } + else // invert the ranges + { + ranged_value = new_begin - (pow(normalized_cur_val, curve) * new_range); + } + + return ranged_value; +} + +/** + * @brief The Function will change frequency of Timer 0 + * + * This function accepts as input a divisor that will change + * the frequency of the Timer that controlls the PWM signal + * + * @param divisor -what we divide the timer frequency by + */ +void set_pwm_frequency(int divisor) +{ + byte mode; + switch(divisor) + { + case 1: + mode = 0x01; + break; + case 8: + mode = 0x02; + break; + case 64: + mode = 0x03; + break; + case 256: + mode = 0x04; + break; + case 1024: + mode = 0x05; + break; + default: + return; + } + + //set mode of timer 0 + TCCR0B = TCCR0B & 0b11111000 | mode; + + return; +} From 24a7f7c75144dbbd725348dc9ad5332ecddbaefd Mon Sep 17 00:00:00 2001 From: RyanLoeffelman Date: Thu, 25 Feb 2016 14:31:45 -0600 Subject: [PATCH 12/12] Update the Arudino Timing Functions to hopefully fix sync issues --- ros_ws/src/arduino/motor/motor.ino | 63 ++++++++++++------------------ 1 file changed, 24 insertions(+), 39 deletions(-) diff --git a/ros_ws/src/arduino/motor/motor.ino b/ros_ws/src/arduino/motor/motor.ino index 8f05767..2260c7f 100644 --- a/ros_ws/src/arduino/motor/motor.ino +++ b/ros_ws/src/arduino/motor/motor.ino @@ -39,8 +39,23 @@ const char* ENCODER_TOPIC = "left_tick"; * * Variable is the number of Millisecond Delay between publishes */ - const float REFRESH_RATE = 1000; + const float REFRESH_RATE_DESIRED = 1000; +/****************************************************************** + * Adjust for the changing of the arudino timer funciton to * + * remove motor whine. This will only work with a divisor of 8! * + * If Divisor changes, please look into how this effects arduino * + * Timing Functons. * + * * + * For Referance please see below website * + * http://playground.arduino.cc/Main/TimerPWMCheatsheet * + * ****************************************************************/ + //const float REFRESH_RATE = (REFRESH_RATE_DESIRED * 8); + + //Replace this line with the line above it if motor whine + // is still an issue + const float REFRESH_RATE = (REFRESH_RATE_DESIRED); + /** * @brief Radians Per Second at full Throtle * @@ -61,13 +76,6 @@ const int FORWARD_PWM_PIN = 5; const int REVERSE_PWM_PIN = 6; const int ENABLE_PIN = A3; -/** - * @brief Const used to change the Motor Whine Divisor - * - * This is set to 8 for IGVC Robot - */ -const int FREQUENCY_DIVISOR = 8; - /** * @brief Float used to scale the Speed to */ @@ -96,7 +104,7 @@ volatile unsigned int encoder_ticks = 0; ************************/ void encoderCount(); void updateEncoder(); -void set_pwm_frequency(int divisor); +void set_pwm_frequency(); void velocityCallback(const std_msgs::Float64& msg); float fScale( float originalMin, float originalMax, float newBegin, float newEnd, float inputValue, float curve); @@ -124,8 +132,7 @@ 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); + //set_pwm_frequency(); //setup pins pinMode(FORWARD_PWM_PIN, OUTPUT); @@ -152,7 +159,7 @@ void loop() //update the current time for multitasking current_mills = millis(); - //Only update the Encoder date when the Refresh Rate says to + //Only update the Encoder data when the Refresh Rate says to if(current_mills-old_mills >= REFRESH_RATE) { updateEncoder(); @@ -334,37 +341,15 @@ float fScale( float original_min, float original_max, float new_begin, /** * @brief The Function will change frequency of Timer 0 * - * This function accepts as input a divisor that will change - * the frequency of the Timer that controlls the PWM signal - * - * @param divisor -what we divide the timer frequency by + * This function will change the frequency of the Timer + * that controlls the PWM signal. This will alos effect + * any other functions on Timer 0 */ -void set_pwm_frequency(int divisor) +void set_pwm_frequency() { - byte mode; - switch(divisor) - { - case 1: - mode = 0x01; - break; - case 8: - mode = 0x02; - break; - case 64: - mode = 0x03; - break; - case 256: - mode = 0x04; - break; - case 1024: - mode = 0x05; - break; - default: - return; - } //set mode of timer 0 - TCCR0B = TCCR0B & 0b11111000 | mode; + TCCR0B = TCCR0B & 0b11111000 | 0x02; return; }