Odometry

ros_ws/src/arudino/motor/motor.ino

@@ -7,25 +7,54 @@
  * 
  */
 
-#define USB_CON
-
 #include <ros.h>
 #include <std_msgs/Float64.h>
+#include <control/Encoder.h>
+#include <ros/time.h>
 
 /*************
  * 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<std_msgs::Float64> 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,20 +175,23 @@ ros::Subscriber<std_msgs::Float64> 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)
     {
         //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
  * 

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
 /**