pidf almost working, D term needs tuning

lunabot_bringup/launch/robot.launch

@@ -23,7 +23,7 @@
         <arg name="frame_id" value="base_link" />
       </include>
 
-      <!--<node pkg="lunabot_control" type="differential_drive_controller.py" name="diff_drive_controller" output="screen"/>-->
+      <node pkg="lunabot_control" type="differential_drive_controller.py" name="diff_drive_controller" output="screen"/>
 
       <group ns="nav">
           <node pkg="lunabot_control" type="mpc_node" name="mpc_node" output="screen">

lunabot_config/config/real_robot.yml

@@ -1,9 +1,9 @@
 odom_topic: "/rtabmap/odom"
 cmd_vel_topic: "/cmd_vel"
+nav_goal_topic: "/goal"
 nav:
     map_topic: "/maps/costmap_node/global_costmap/costmap"
     global_path_topic: "/nav/global_path"
-    goal_topic: "/goal"
     occ_threshold: 80 # corresponds to possibly-in-collision
     bezier_step: 0.01
     lerp_step: 0.05

lunabot_control/scripts/differential_drive_controller.py

@@ -5,16 +5,6 @@
 
 from lunabot_msgs.msg import RobotEffort, RobotState
 
-
-def ang_delta(deg, prev_deg):
-    raw = deg - prev_deg
-    if raw == 0:
-        return 0
-    turn = min(raw % 360.0, -raw % 360.0)
-    dir_ = abs(raw) / raw if turn == raw else -abs(raw) / raw
-    return turn * dir_
-
-
 class DifferentialDriveController:
     def __init__(self):
         # ROS Publishers and subsribers to get / send data
@@ -24,28 +14,24 @@ def __init__(self):
         self._state_sub = rospy.Subscriber("state", RobotState, self._robot_state_cb)
 
         self.width = rospy.get_param("~width", 0.5588)
-        self.max_speed_percentage = rospy.get_param("~max_speed_percentage", 0.25)
+        self.max_speed_percentage = rospy.get_param("~max_speed_percentage", 1)
         self.hz = rospy.get_param("~hz", 20)
-        self.max_speed = rospy.get_param("~max_speed", 0.62)
-        self.p = rospy.get_param("~p", 0.00001)  # P gain for PD controller
-        self.d = rospy.get_param("~d", 0.000004)  # D gain for PD controller
+        self._max_speed = rospy.get_param("~max_speed", 4.3)
+        self.p = rospy.get_param("~p", 1.75)  # P gain for PD controller
+        self.d = rospy.get_param("~d", 0.01)  # D gain for PD controller
 
         self.lin = 0
         self.ang = 0
 
-        self.meters_per_tick = 0.1397  # TODO find
-        self.encoder_dt = 0.01  # Amount of time between readings
+        self._left_vel = 0
+        self._right_vel = 0
+        self._meters_per_rad = 0.1397
+        self._left_prev_error = 0
+        self._right_prev_error = 0
 
         # Variables for PIDF Velocity Control
 
         self.loop_dt = 1 / self.hz  # Amount of time between calls of this function
-        self.prev_left_vel_reading = 0
-        self.prev_right_vel_reading = 0
-        self.prev_left_reading = 0
-        self.left_reading = 0
-        self.prev_right_reading = 0
-        self.right_reading = 0
-
         rate = rospy.Rate(self.hz)
         rospy.on_shutdown(self.shutdown_hook)
 
@@ -58,59 +44,48 @@ def _vel_cb(self, vel_msg):
         self.ang = vel_msg.angular.z
 
     def _robot_state_cb(self, msg):
-        self.right_prev_reading = self.right_reading
-        self.right_reading = msg.drive_right_ang
-
-        self.left_prev_reading = self.left_reading
-        self.left_reading = msg.drive_left_ang
+        self._right_vel = msg.drive_right_vel
+        self._left_vel = msg.drive_left_vel
 
     def _loop(self):
         effort_msg = RobotEffort()
 
-        left = self.lin - self.ang * self.width / 2
-        right = self.lin + self.ang * self.width / 2
+        left_percent_estimate = np.clip(self._left_vel / self._max_speed, -1, 1)
+        right_percent_estimate = np.clip(self._right_vel / self._max_speed, -1, 1)
 
-        # Feed forward
-        left_percent_estimate = np.clip(left / self.max_speed, -1, 1)
-        right_percent_estimate = np.clip(right / self.max_speed, -1, 1)
+        left_set = self.lin - self.ang * self.width / 2
+        right_set = self.lin + self.ang * self.width / 2
 
         # Measured Velocity in m / s
-        left_vel_reading = (
-            ang_delta(self.left_reading, self.prev_left_reading)
-            / self.encoder_dt
-            * self.meters_per_tick
-        )
-        right_vel_reading = (
-            ang_delta(self.right_reading, self.prev_right_reading)
-            / self.encoder_dt
-            * self.meters_per_tick
+        left_measured = (self._left_vel * self._meters_per_rad)
+        right_measured = (
+            self._right_vel * self._meters_per_rad
         )
 
         # Calculating error
-        left_error = left - left_vel_reading
-        right_error = right - right_vel_reading
-
-        # Calculating previous error
-        left_prev_error = left - self.prev_left_vel_reading
-        right_prev_error = right - self.prev_right_vel_reading
+        left_error = left_set - left_measured 
+        right_error = right_set - right_measured 
 
         # Calculating motor velocities
         left = (
-            left_percent_estimate
+           left_percent_estimate
             + left_error * self.p
-            + (left_error - left_prev_error) / self.loop_dt * self.d
+            + (left_error - self._left_prev_error) / self.loop_dt * self.d
         )
         right = (
-            right_percent_estimate
+           right_percent_estimate 
             + right_error * self.p
-            + (right_error - right_prev_error) / self.loop_dt * self.d
+            + (right_error - self._right_prev_error) / self.loop_dt * self.d
         )
 
-        # Updating previous readings
-        self.prev_left_vel_reading = left_vel_reading
-        self.prev_right_vel_reading = right_vel_reading
+        # Calculating previous error
+        self._left_prev_error = left_error
+        self._right_prev_error = right_error
+
         effort_msg.left_drive = self.constrain(left)
         effort_msg.right_drive = self.constrain(right)
+        print("left", left_error)
+        print("right", right_error)
         self._effort_pub.publish(effort_msg)
 
     def constrain(self, val):

lunabot_embedded/include/lunabot_embedded/sensor_proc.h

@@ -32,6 +32,7 @@ float u_mod(float n, float base) {
   return (m >= 0) ? m : m + base;
 }
 
+// TODO: Fix cyclic spikes in delta
 float deg_angle_delta(float deg, float prev_deg) {
   float raw = deg - prev_deg;
   if (raw == 0) {

lunabot_embedded/src/teensy_driver_node.cpp

@@ -23,6 +23,9 @@ using namespace std;
 #define BUF_SIZE 64
 #define MAX_ANGLE_DELTA_DEG 15
 
+// Left encoder reports half the speed due to hardware error
+#define LEFT_ENCODER_MULTIPLIER 2
+
 #define LEAKY_INTEGRATOR_ALPHA 0.6
 
 uint8_t buf[BUF_SIZE];
@@ -67,7 +70,7 @@ void recv(ros::Publisher &pub) {
   prev_time = ros::Time::now().toSec();
 
   float left_vel, right_vel;
-  left_vel = deg_angle_delta(state.drive_left_ang, prev_state.drive_left_ang) / dt;
+  left_vel = LEFT_ENCODER_MULTIPLIER * deg_angle_delta(state.drive_left_ang, prev_state.drive_left_ang) / dt;
   right_vel = deg_angle_delta(state.drive_right_ang, prev_state.drive_right_ang) / dt;
 
   left_buffer.push(left_vel);