Wrote the base of rover_control. Currently drives rover using FrSky. Needs tuning.

software/rover/rover_control/src/control_coordinator/control_coordinator.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+
+from time import time, sleep
+
+# Custom Imports
+from rover_control.msg import DriveCommandMessage, DriveControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "control_coordinator"
+
+DEFAULT_IRIS_DRIVE_COMMAND_TOPIC = "command_control/iris_drive"
+DEFAULT_REAR_BOGIE_TOPIC = "drive_control/rear"
+DEFAULT_LEFT_BOGIE_TOPIC = "drive_control/left"
+DEFAULT_RIGHT_BOGIE_TOPIC = "drive_control/right"
+
+UINT16_MAX = 65535
+
+
+#####################################
+# ControlCoordinator Class Definition
+#####################################
+class ControlCoordinator(object):
+    def __init__(self):
+        rospy.init_node(NODE_NAME)
+
+        self.iris_topic = rospy.get_param("~iris_drive_command_topic", DEFAULT_IRIS_DRIVE_COMMAND_TOPIC)
+        self.rear_bogie_topic = rospy.get_param("~rear_bogie_control_topic", DEFAULT_REAR_BOGIE_TOPIC)
+        self.left_bogie_topic = rospy.get_param("~left_bogie_control_topic", DEFAULT_LEFT_BOGIE_TOPIC)
+        self.right_bogie_topic = rospy.get_param("~right_bogie_control_topic", DEFAULT_RIGHT_BOGIE_TOPIC)
+
+        self.iris_drive_command_subscriber = \
+            rospy.Subscriber(self.iris_topic, DriveCommandMessage, self.iris_drive_command_callback)
+
+        self.rear_bogie_publisher = rospy.Publisher(self.rear_bogie_topic, DriveControlMessage, queue_size=1)
+        self.left_bogie_publisher = rospy.Publisher(self.left_bogie_topic, DriveControlMessage, queue_size=1)
+        self.right_bogie_publisher = rospy.Publisher(self.right_bogie_topic, DriveControlMessage, queue_size=1)
+
+        self.run()
+
+    def run(self):
+        while not rospy.is_shutdown():
+            sleep(0.25)
+
+    def iris_drive_command_callback(self, drive_command):
+        rear_drive = DriveControlMessage()
+        left_drive = DriveControlMessage()
+        right_drive = DriveControlMessage()
+
+        left = drive_command.drive_twist.linear.x - drive_command.drive_twist.angular.z
+        right = drive_command.drive_twist.linear.x + drive_command.drive_twist.angular.z
+
+        left_direction = 1 if left >= 0 else 0
+        rear_drive.first_motor_direction = left_direction
+        left_drive.first_motor_direction = left_direction
+        left_drive.second_motor_direction = left_direction
+
+        right_direction = 1 if right >= 0 else 0
+        rear_drive.second_motor_direction = right_direction
+        right_drive.first_motor_direction = right_direction
+        right_drive.second_motor_direction = right_direction
+
+        left_speed = abs(left * UINT16_MAX)
+        right_speed = abs(right * UINT16_MAX)
+
+        rear_drive.first_motor_speed = left_speed
+        left_drive.first_motor_speed = left_speed
+        left_drive.second_motor_speed = left_speed
+
+        rear_drive.second_motor_speed = right_speed
+        right_drive.first_motor_speed = right_speed
+        right_drive.second_motor_speed = right_speed
+
+        self.rear_bogie_publisher.publish(rear_drive)
+        self.left_bogie_publisher.publish(left_drive)
+        self.right_bogie_publisher.publish(right_drive)
+
+if __name__ == "__main__":
+    ControlCoordinator()

software/rover/rover_control/src/drive_control/drive_control.py

@@ -0,0 +1,114 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+
+from time import time, sleep
+
+import serial.rs485
+import minimalmodbus
+
+# Custom Imports
+from rover_control.msg import DriveControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "drive_control"
+
+DEFAULT_PORT = "/dev/rover/ttyBogie"
+DEFAULT_BAUD = 115200
+
+DEFAULT_INVERT = False
+
+DEFAULT_DRIVE_CONTROL_TOPIC = "drive_control/rear"
+
+FIRST_MOTOR_ID = 1
+SECOND_MOTOR_ID = 2
+
+COMMUNICATIONS_TIMEOUT = 0.15  # Seconds
+
+RX_DELAY = 0.01
+TX_DELAY = 0.01
+
+MODBUS_REGISTERS = {
+    "DIRECTION": 0,
+    "SPEED": 1,
+    "SLEEP": 2,
+
+    "CURRENT": 3,
+    "FAULT": 4,
+
+    "TEMPERATURE": 5
+}
+
+MOTOR_DRIVER_DEFAULT_MESSAGE = [
+    1,  # Forwards
+    0,  # 0 Speed
+    1  # Not in sleep mode
+]
+
+
+#####################################
+# DriveControl Class Definition
+#####################################
+class DriveControl(object):
+    def __init__(self):
+        rospy.init_node(NODE_NAME)
+
+        self.port = rospy.get_param("~port", DEFAULT_PORT)
+        self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
+
+        self.first_motor_inverted = rospy.get_param("~invert_first_motor", DEFAULT_INVERT)
+        self.second_motor_inverted = rospy.get_param("~invert_second_motor", DEFAULT_INVERT)
+
+        self.drive_control_subscriber_topic = rospy.get_param("~drive_control_topic", DEFAULT_DRIVE_CONTROL_TOPIC)
+
+        self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID)
+        self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID)
+        self.__setup_minimalmodbus_for_485()
+
+        self.drive_control_subscriber = \
+            rospy.Subscriber(self.drive_control_subscriber_topic, DriveControlMessage, self.drive_control_callback)
+
+        self.run()
+
+    def __setup_minimalmodbus_for_485(self):
+        self.first_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.first_motor.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                        delay_before_rx=RX_DELAY,
+                                                                        delay_before_tx=TX_DELAY)
+
+        self.second_motor.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.second_motor.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                         delay_before_rx=RX_DELAY,
+                                                                         delay_before_tx=TX_DELAY)
+
+    def run(self):
+        while not rospy.is_shutdown():
+            sleep(0.25)
+
+    def drive_control_callback(self, drive_control):
+        try:
+            first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
+            first_direction = \
+                not drive_control.first_motor_direction if self.first_motor_inverted else drive_control.first_motor_direction
+            first_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = first_direction
+            first_motor_register_data[MODBUS_REGISTERS["SPEED"]] = drive_control.first_motor_speed
+
+            second_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE)
+            second_direction = not drive_control.second_motor_direction if self.second_motor_inverted else drive_control.second_motor_direction
+            second_motor_register_data[MODBUS_REGISTERS["DIRECTION"]] = second_direction
+            second_motor_register_data[MODBUS_REGISTERS["SPEED"]] = drive_control.second_motor_speed
+
+            self.first_motor.write_registers(0, first_motor_register_data)
+            self.second_motor.write_registers(0, second_motor_register_data)
+
+        except Exception, error:
+            print "Error occurred:", error
+
+
+if __name__ == "__main__":
+    DriveControl()

software/rover/rover_control/src/iris_controller/iris_controller.py

@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+
+from time import time, sleep
+
+import serial.rs485
+import minimalmodbus
+
+# Custom Imports
+from rover_control.msg import DriveCommandMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "iris_controller"
+
+DEFAULT_PORT = "/dev/rover/ttyIRIS"
+DEFAULT_BAUD = 115200
+
+DEFAULT_DRIVE_COMMAND_TOPIC = "command_control/iris_drive"
+
+DEFAULT_HERTZ = 10
+COMMUNICATIONS_TIMEOUT = 0.15  # Seconds
+
+MODBUS_ID = 1
+
+RX_DELAY = 0.01
+TX_DELAY = 0.01
+
+SBUS_VALUES = {
+    "SBUS_MAX": 1811,
+    "SBUS_MID": 991,
+    "SBUS_MIN": 172,
+    "SBUS_RANGE": 820.0,
+
+    "SBUS_DEADZONE": 5
+}
+
+MODBUS_REGISTERS = {
+    "LEFT_STICK_Y_AXIS": 0,
+    "RIGHT_STICK_Y_AXIS": 1,
+    "RIGHT_STICK_X_AXIS": 2,
+    "LEFT_STICK_X_AXIS": 3,
+    "LEFT_POT": 4,
+    "S1_POT": 5,
+    "S2_POT": 6,
+    "RIGHT_POT": 7,
+    "SA_SWITCH": 8,
+    "SB_SWITCH": 9,
+    "SC_SWITCH": 10,
+    "SD_SWITCH": 11,
+    "SE_SWITCH": 12,
+    "SF_SWITCH": 13,
+    "SG_SWITCH": 14,
+    "SH_SWITCH": 15,
+
+    "VOLTAGE_24V": 16,
+    "VOLTAGE_5V": 17,
+    "USB_VOLTAGE_5V": 18,
+    "VOLTAGE_3V3": 19
+}
+
+REGISTER_STATE_MAPPING = {
+    "IGNORE_CONTROL": "SF_SWITCH",
+    "DRIVE_VS_ARM": "SE_SWITCH"
+}
+
+
+#####################################
+# IrisController Class Definition
+#####################################
+class IrisController(object):
+    def __init__(self):
+        rospy.init_node(NODE_NAME)
+
+        self.port = rospy.get_param("~port", DEFAULT_PORT)
+        self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
+
+        self.drive_command_publisher_topic = rospy.get_param("~drive_command_topic", DEFAULT_DRIVE_COMMAND_TOPIC)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.iris = minimalmodbus.Instrument(self.port, MODBUS_ID)
+        self.__setup_minimalmodbus_for_485()
+
+        self.drive_command_publisher = rospy.Publisher(self.drive_command_publisher_topic, DriveCommandMessage,
+                                                       queue_size=1)
+
+        self.registers = []
+
+        self.run()
+
+    def __setup_minimalmodbus_for_485(self):
+        self.iris.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.iris.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                 delay_before_rx=RX_DELAY, delay_before_tx=TX_DELAY)
+
+    def run(self):
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            try:
+                self.read_registers()
+                self.broadcast_drive_if_current_mode()
+                self.broadcast_arm_if_current_mode()
+
+            except Exception, error:
+                print "Error occurred:", error
+
+            time_diff = time() - start_time
+
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def read_registers(self):
+        self.registers = self.iris.read_registers(0, len(MODBUS_REGISTERS))
+
+    def broadcast_drive_if_current_mode(self):
+        if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] < SBUS_VALUES["SBUS_MID"]:
+            command = DriveCommandMessage()
+
+            left_y_axis = self.registers[MODBUS_REGISTERS["LEFT_STICK_Y_AXIS"]]
+            right_y_axis = self.registers[MODBUS_REGISTERS["RIGHT_STICK_Y_AXIS"]]
+
+            if left_y_axis == 0 and right_y_axis == 0:
+                command.ignore_drive_control = True
+                command.drive_twist.linear.x = 0
+                command.drive_twist.angular.z = 0
+            else:
+
+                left = (left_y_axis - SBUS_VALUES["SBUS_MID"]) / SBUS_VALUES[
+                    "SBUS_RANGE"]
+
+                right = (right_y_axis - SBUS_VALUES["SBUS_MID"]) / SBUS_VALUES[
+                    "SBUS_RANGE"]
+
+                command.ignore_drive_control = \
+                    self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["IGNORE_CONTROL"]]] > SBUS_VALUES["SBUS_MID"]
+                command.drive_twist.linear.x = (left + right) / 2.0
+                command.drive_twist.angular.z = (right - left) / 2.0
+
+            self.drive_command_publisher.publish(command)
+
+    def broadcast_arm_if_current_mode(self):
+        if self.registers[MODBUS_REGISTERS[REGISTER_STATE_MAPPING["DRIVE_VS_ARM"]]] > \
+                        SBUS_VALUES["SBUS_MIN"] + SBUS_VALUES["SBUS_DEADZONE"]:
+            print "Arm"
+
+
+if __name__ == "__main__":
+    IrisController()