added basic IMU magnetometer reader

Software/real_time/ROS_RoboBuggy/src/robobuggy/include/transistor/controller/Controller.h

@@ -33,6 +33,7 @@ class Controller
 
     int get_closest_waypoint_index();
     double pure_pursuit_controller();
+    double stanley_controller();
     bool get_deploy_brake_value();
     double normalize_angle_rad(double radians);
     double get_distance_from_pose(robobuggy::Pose pose_msg);

Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/imu_magnetometer_reader.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import json
+import time
+from robobuggy.msg import IMU
+
+def magnetometer_callback(data):
+    global x
+    global y
+    global z
+    x = data.X_Mag
+    y = data.Y_Mag
+    z = data.Z_Mag
+
+    rospy.loginfo("orientation: %f", math.degrees(math.atan2(y,x)))
+
+    #plot on Tkinter window
+    pass
+
+def start_subscriber_spin():
+    rospy.init_node("IMU_Plotter", anonymous=True)
+
+    rospy.Subscriber("IMU", IMU, magnetometer_callback)
+
+    #tkinter graphics window
+    rospy.spin()
+    pass
+
+if __name__ == "__main__":
+    start_subscriber_spin()

Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/controller/Controller.cpp

@@ -123,6 +123,14 @@ double Controller::pure_pursuit_controller()
     return commanded_angle;
 }
 
+double Controller::stanley_controller()
+{
+    // first get the closest waypoint to us
+    // get the track heading by getting the heading between it and the next waypoint
+
+    // calculate cross track error
+}
+
 bool Controller::get_deploy_brake_value()
 {
     return false;