current sensor conversions, refactoring, rosparams

lunabot_control/scripts/differential_drive_controller.py

@@ -16,31 +16,37 @@ def ang_delta(deg, prev_deg):
 
 
 class DifferentialDriveController:
-    width = 0.5588  # Robot width, TODO find
-    max_speed_percentage = 0.25  # Maximum speed we're allowing drive motors to spin
-    HZ = 20
-
     def __init__(self):
-
         # ROS Publishers and subsribers to get / send data
 
         self._vel_sub = rospy.Subscriber("/cmd_vel", Twist, self._vel_cb)
         self._effort_pub = rospy.Publisher("/effort", RobotEffort, queue_size=1)
         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.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.lin = 0
         self.ang = 0
 
+        self.meters_per_tick = 0.1397  # TODO find
+        self.encoder_dt = 0.01  # Amount of time between readings
+
         # 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)
+        rate = rospy.Rate(self.hz)
         rospy.on_shutdown(self.shutdown_hook)
 
         while not rospy.is_shutdown():
@@ -64,28 +70,20 @@ def _loop(self):
         left = self.lin - self.ang * self.width / 2
         right = self.lin + self.ang * self.width / 2
 
-        max_speed = 0.62  # TODO find max speed of motors in meters / second
-        p = 0.00001  # P gain for PD controller
-        d = 0.000004  # D gain for PD controller
-
         # Feed forward
-        left_percent_estimate = np.clip(left / max_speed, -1, 1)
-        right_percent_estimate = np.clip(right / max_speed, -1, 1)
-
-        meters_per_tick = 0.1397  # TODO find
-        encoder_dt = 0.01  # Amount of time between readings
-        loop_dt = 1 / self.HZ  # Amount of time between calls of this function
+        left_percent_estimate = np.clip(left / self.max_speed, -1, 1)
+        right_percent_estimate = np.clip(right / self.max_speed, -1, 1)
 
         # Measured Velocity in m / s
         left_vel_reading = (
             ang_delta(self.left_reading, self.prev_left_reading)
-            / encoder_dt
-            * meters_per_tick
-        ) * 3
+            / self.encoder_dt
+            * self.meters_per_tick
+        )
         right_vel_reading = (
             ang_delta(self.right_reading, self.prev_right_reading)
-            / encoder_dt
-            * meters_per_tick
+            / self.encoder_dt
+            * self.meters_per_tick
         )
 
         # Calculating error
@@ -99,13 +97,13 @@ def _loop(self):
         # Calculating motor velocities
         left = (
             left_percent_estimate
-            + left_error * p
-            + (left_error - left_prev_error) / loop_dt * d
+            + left_error * self.p
+            + (left_error - left_prev_error) / self.loop_dt * self.d
         )
         right = (
             right_percent_estimate
-            + right_error * p
-            + (right_error - right_prev_error) / loop_dt * d
+            + right_error * self.p
+            + (right_error - right_prev_error) / self.loop_dt * self.d
         )
 
         # Updating previous readings

lunabot_control/scripts/excavation_controller.py

@@ -27,17 +27,12 @@ def is_overflow(self):
 
 
 class ExcavationController:
-    max_exc_percent = 1
-    max_lead_screw_percent = 1
-
     FORWARD_EXCAVATE = -1
     DOWN_LEAD_SCREW = -0.2
     UP_LEAD_SCREW = 0.7
 
-    # valid_exc_range = (11000, 17400)
-    valid_exc_range = (6000, 17400)
-    # valid_lead_screw_range = (9000, 17000)
-    valid_lead_screw_range = (6000, 20000)
+    valid_exc_current_range = (6000, 17400)
+    valid_lead_screw_current_range = (6000, 20000)
 
     def __init__(self):
         self.exc_curr = None
@@ -55,8 +50,12 @@ def __init__(self):
         self._effort_pub = rospy.Publisher("/effort", RobotEffort, queue_size=1)
         self._state_sub = rospy.Subscriber("/state", RobotState, self._robot_state_cb)
 
+        self.max_exc_percent = rospy.get_param("~max_exc_percent", 1)
+        self.max_lead_screw_percent = rospy.get_param("~max_lead_screw_percent", 1)
+        self.hz = rospy.get_param("~hz", 50)
+
         rospy.on_shutdown(self.shutdown_hook)
-        self.rate = rospy.Rate(50)
+        self.rate = rospy.Rate(self.hz)
 
         while not rospy.is_shutdown():
             if self.exc_curr is not None and self.lead_screw_curr is not None:
@@ -96,15 +95,15 @@ def _robot_state_cb(self, msg):
 
     def is_exc_stuck(self):
         return (
-            self.exc_curr < self.valid_exc_range[0]
-            or self.exc_curr > self.valid_exc_range[1]
+            self.exc_curr < self.valid_exc_current_range[0]
+            or self.exc_curr > self.valid_exc_current_range[1]
         )
 
     def lead_screw_max_retract(self):
-        return self.lead_screw_curr < self.valid_lead_screw_range[0]
+        return self.lead_screw_curr < self.valid_lead_screw_current_range[0]
 
     def lead_screw_max_extend(self):
-        return self.lead_screw_curr > self.valid_lead_screw_range[1]
+        return self.lead_screw_curr > self.valid_lead_screw_current_range[1]
 
     def is_lead_screw_invalid(self):
         return self.lead_screw_max_retract() or self.lead_screw_max_extend()

lunabot_embedded/CMakeLists.txt

@@ -115,7 +115,7 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-  #include
+  include
   ${catkin_INCLUDE_DIRS}
 )
 

lunabot_embedded/include/lunabot_embedded/utils.h

@@ -0,0 +1,59 @@
+#include <algorithm>
+#include <math.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <sys/ioctl.h>
+#include <termios.h>
+
+using namespace std;
+
+float to_rad(float deg) { return deg * M_PI / 180.0; }
+float u_mod(float n, float base) {
+    float m = fmod(n, base);
+    return (m >= 0) ? m : m + base;
+}
+float ang_delta(float deg, float prev_deg) {
+    float raw = deg - prev_deg;
+    if (raw == 0) {
+        return 0;
+    }
+    float turn = min(u_mod(raw, 360.), u_mod(-raw, 360.));
+
+    int dir = turn == raw ? abs(raw) / raw : -abs(raw) / raw;
+
+    return turn * dir;
+}
+
+struct GearAngle {
+  public:
+    GearAngle(float gear_ratio) : gear_ratio_(gear_ratio) { cycle_ = 0; }
+    float get_rad_from_raw(float deg, float prev_deg) {
+        float raw = deg - prev_deg;
+        float turn = min(u_mod(raw, 360.), u_mod(-raw, 360.));
+        int dir = turn == raw ? abs(raw) / raw : -abs(raw) / raw;
+        if (abs(raw) != abs(turn)) { // overflow occured!
+            if (raw > 0) {
+                cycle_++;
+            } else {
+                cycle_--;
+            }
+        }
+        return (2 * M_PI * cycle_ + to_rad(deg)) * gear_ratio_;
+    }
+
+  private:
+    int cycle_;
+    float gear_ratio_;
+};
+
+float adc_to_current_ACS711_31A(float adc_value, float adc_fsr = 4.096,
+                                float vcc = 3.3) {
+    float vout = adc_value / pow(2, 16 - 1) * adc_fsr;
+    return 73.3 * (vout / vcc) - 36.7;
+}
+
+float adc_to_current_ACS711_15A(float adc_value, float adc_fsr = 4.096,
+                                float vcc = 3.3) {
+    float vout = adc_value / pow(2, 16 - 1) * adc_fsr;
+    return 36.7 * (vout / vcc) - 18.3;
+}

lunabot_embedded/src/sensor_proc.cpp

@@ -0,0 +1 @@
+

lunabot_embedded/src/teensy_driver_node.cpp

@@ -6,6 +6,7 @@
 #include <sys/ioctl.h>
 #include <termios.h>
 
+#include <lunabot_embedded/utils.h>
 #include <lunabot_msgs/RobotEffort.h>
 #include <lunabot_msgs/RobotState.h>
 
@@ -19,53 +20,11 @@ extern "C" {
 using namespace std;
 
 #define BUF_SIZE 64
-
 #define DEP_GEAR_RATIO (1. / 7.125)
-
 #define MAX_DIFF 15
 
-float to_rad(float deg) { return deg * M_PI / 180.0; }
-float u_mod(float n, float base) {
-    float m = fmod(n, base);
-    return (m >= 0) ? m : m + base;
-}
-float ang_delta(float deg, float prev_deg) {
-    float raw = deg - prev_deg;
-    if (raw == 0) {
-        return 0;
-    }
-    float turn = min(u_mod(raw, 360.), u_mod(-raw, 360.));
-
-    int dir = turn == raw ? abs(raw) / raw : -abs(raw) / raw;
-
-    return turn * dir;
-}
-
-struct GearAngle {
-  public:
-    GearAngle(float gear_ratio) : gear_ratio_(gear_ratio) { cycle_ = 0; }
-    float get_rad_from_raw(float deg, float prev_deg) {
-        float raw = deg - prev_deg;
-        float turn = min(u_mod(raw, 360.), u_mod(-raw, 360.));
-        int dir = turn == raw ? abs(raw) / raw : -abs(raw) / raw;
-        if (abs(raw) != abs(turn)) { // overflow occured!
-            if (raw > 0) {
-                cycle_++;
-            } else {
-                cycle_--;
-            }
-        }
-        return (2 * M_PI * cycle_ + to_rad(deg)) * gear_ratio_;
-    }
-
-  private:
-    int cycle_;
-    float gear_ratio_;
-};
-
 uint8_t buf[BUF_SIZE];
 
-GearAngle dep_gear(DEP_GEAR_RATIO);
 RobotState prev_state = RobotState_init_zero;
 RobotState state = RobotState_init_zero;
 RobotEffort effort = RobotEffort_init_zero;
@@ -80,12 +39,15 @@ void recv(ros::Publisher &pub) {
     /* Now we are ready to decode the message. */
     pb_decode(&stream, RobotState_fields, &state);
     lunabot_msgs::RobotState state_msg;
-    state_msg.act_right_curr = state.act_right_curr;
-    state_msg.drive_right_curr = state.drive_right_curr;
-    state_msg.drive_left_curr = state.drive_left_curr;
-    state_msg.lead_screw_curr = state.lead_screw_curr;
-    state_msg.dep_curr = state.dep_curr;
-    state_msg.exc_curr = state.exc_curr;
+    state_msg.act_right_curr = adc_to_current_ACS711_15A(state.act_right_curr);
+    state_msg.drive_right_curr =
+        adc_to_current_ACS711_15A(state.drive_right_curr);
+    state_msg.drive_left_curr =
+        adc_to_current_ACS711_15A(state.drive_left_curr);
+    state_msg.lead_screw_curr =
+        adc_to_current_ACS711_15A(state.lead_screw_curr);
+    state_msg.dep_curr = adc_to_current_ACS711_15A(state.dep_curr);
+    state_msg.exc_curr = adc_to_current_ACS711_31A(state.exc_curr);
     state_msg.act_ang = state.act_ang;
     float drive_left_ang_diff =
         ang_delta(state.drive_left_ang, last_drive_left);
@@ -99,9 +61,9 @@ void recv(ros::Publisher &pub) {
 
     state_msg.drive_right_ang = to_rad(state.drive_right_ang);
     state_msg.lead_screw_ang = state.lead_screw_ang;
-    state_msg.dep_ang =
-        dep_gear.get_rad_from_raw(state.dep_ang, prev_state.dep_ang);
-    // state_msg.dep_ang = state.dep_ang;
+    // state_msg.dep_ang =
+    // dep_gear.get_rad_from_raw(state.dep_ang, prev_state.dep_ang);
+    state_msg.dep_ang = state.dep_ang;
 
     prev_state = state;
     pub.publish(state_msg);

lunabot_sim/dummy_bot_description/src/gazebo_ros_skid_steer_drive.cpp

@@ -47,7 +47,7 @@
 #include <algorithm>
 #include <assert.h>
 
-#include <gazebo_plugins/gazebo_ros_skid_steer_drive.h>
+#include <dummy_bot_description/gazebo_ros_skid_steer_drive.h>
 
 #ifdef ENABLE_PROFILER
 #include <ignition/common/Profiler.hh>