Finished firmware for tower pan/tilt node. Also made the ROS node for it. Just needs to be installed on Rover.

software/firmware/tower/tower.ino

@@ -26,12 +26,12 @@
    temp (deg c)
 */
 
-Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR);
+Adafruit_BNO055 bno = Adafruit_BNO055(WIRE_BUS, -1, BNO055_ADDRESS_A, I2C_MASTER, I2C_PINS_29_30, I2C_PULLUP_INT, I2C_RATE_100, I2C_OP_MODE_ISR);
 
 
-#define gpsPort Serial3
+#define gpsPort Serial2
-#define commsPort Serial2
+#define commsPort Serial3
-#define GPS_PORT_NAME "Serial3"
+#define GPS_PORT_NAME "Serial2"
 
 const unsigned char ubxRate5Hz[] = { 0x06, 0x08, 0x06, 0x00, 200, 0x00, 0x01, 0x00, 0x01, 0x00 };
 const unsigned char ubxRate10Hz[] = { 0x06, 0x08, 0x06, 0x00, 100, 0x00, 0x01, 0x00, 0x01, 0x00 };
@@ -69,21 +69,26 @@ void sendUBX( const unsigned char *progmemBytes, size_t len )
 }
 
 void setup() {
+  Serial.begin(9600);
+  while (!Serial);
+  Serial.println("Booting up");
   // put your setup code here, to run once:
   commsPort.begin(115200);
-  commsPort.transmitterEnable(6);
+  commsPort.transmitterEnable(3);
 
   delay(250);
   gpsPort.begin(9600);
-  sendUBX( ubxRate5Hz, sizeof(ubxRate5Hz) );
+  //  sendUBX( ubxRate5Hz, sizeof(ubxRate5Hz) );
 
+  Serial.println("Setting up IMU");
   if (!bno.begin()) {
     /* There was a problem detecting the BNO055 ... check your connections */
-    commsPort.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
     while (1);
   }
+  Serial.println("IMU Online. Setting to external crystal.");
   bno.setExtCrystalUse(true);
-
+  Serial.println("IMU Configured.");
 
 
 }
@@ -113,34 +118,34 @@ void loop() {
   commsPort.print(",");
   commsPort.print("oy:");
   commsPort.print(quat.y(), float_decimal_places);
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("oz:");
   commsPort.print(quat.z(), float_decimal_places);
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("ow:");
   commsPort.print(quat.w(), float_decimal_places);
 
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("lax:");
   commsPort.print(linear_accel.x(), float_decimal_places);
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("lay:");
   commsPort.print(linear_accel.y(), float_decimal_places);
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("laz:");
   commsPort.print(linear_accel.z(), float_decimal_places);
 
-  
+
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("avx:");
   commsPort.print(angular_vel.x(), float_decimal_places);
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("avy:");
   commsPort.print(angular_vel.y(), float_decimal_places);
-    commsPort.print(",");
+  commsPort.print(",");
   commsPort.print("avz:");
   commsPort.print(angular_vel.z(), float_decimal_places);
-  
+
   commsPort.println();
 
   //

software/firmware/tower_pan_tilt/tower_pan_tilt.ino

@@ -1,9 +1,148 @@
+////////// Includes //////////
+#include <ModbusRtu.h>
+#include <Servo.h>
+
+////////// Hardware / Data Enumerations //////////
+enum HARDWARE {
+  RS485_EN = 2,
+  RS485_RX = 7,
+  RS485_TX = 8,
+
+  SERVO_PAN = 5,
+  SERVO_TILT = 4,
+
+  LED_RED = 1,
+  LED_GREEN = 32,
+  LED_BLUE = 6,
+
+  LED_BLUE_EXTRA = 13
+};
+
+enum MODBUS_REGISTERS {
+  CENTER_ALL = 0,           // Input/Output
+  PAN_ADJUST_POSITIVE = 1,  // Input/Output
+  PAN_ADJUST_NEGATIVE = 2,  // Input/Output
+  TILT_ADJUST_POSITIVE = 3, // Input/Output
+  TILT_ADJUST_NEGATIVE = 4, // Input/Output
+};
+
+////////// Global Variables //////////
+const uint8_t node_id = 1;
+const uint8_t mobus_serial_port_number = 3;
+
+uint16_t modbus_data[] = {0, 0, 0, 0, 0};
+uint8_t num_modbus_registers = 0;
+
+int8_t poll_state = 0;
+bool communication_good = false;
+uint8_t message_count = 0;
+
+// Pan/tilt hard limits
+const int pan_min = 1415;
+const int pan_center = 1538;
+const int pan_max = 1665;
+
+const int tilt_min = 0;
+const int tilt_center = 1900;
+const int tilt_max = 2400;
+
+// Pan/tilt positions
+int pan_position = pan_center;
+int tilt_position = tilt_center;
+
+////////// Class Instantiations //////////
+Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
+
+Servo pan_servo;
+Servo tilt_servo;
+
 void setup() {
-  // put your setup code here, to run once:
+  setup_hardware();
+
+  num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
+
+  slave.begin(115200);
+  slave.setTimeOut(150);
 
 }
 
 void loop() {
-  // put your main code here, to run repeatedly:
+  poll_modbus();
-
+  set_leds();
+  set_pan_tilt_adjustments();
 }
+
+void setup_hardware() {
+  // Setup pins as inputs / outputs
+  pinMode(HARDWARE::RS485_EN, OUTPUT);
+
+  pinMode(HARDWARE::SERVO_PAN, OUTPUT);
+  pinMode(HARDWARE::SERVO_TILT, OUTPUT);
+
+  pan_servo.attach(HARDWARE::SERVO_PAN);
+  tilt_servo.attach(HARDWARE::SERVO_TILT);
+
+  pan_servo.write(pan_center);
+  tilt_servo.write(tilt_center);
+
+  pinMode(HARDWARE::LED_RED, OUTPUT);
+  pinMode(HARDWARE::LED_GREEN, OUTPUT);
+  pinMode(HARDWARE::LED_BLUE, OUTPUT);
+
+  pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
+
+  // Set default pin states
+  digitalWrite(HARDWARE::LED_RED, LOW);
+  digitalWrite(HARDWARE::LED_GREEN, HIGH);
+  digitalWrite(HARDWARE::LED_BLUE, HIGH);
+
+  digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
+}
+
+void poll_modbus() {
+  poll_state = slave.poll(modbus_data, num_modbus_registers);
+  communication_good = !slave.getTimeOutState();
+}
+
+void set_leds() {
+  if (poll_state > 4) {
+    message_count++;
+    if (message_count > 2) {
+      digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA));
+      message_count = 0;
+    }
+
+    digitalWrite(HARDWARE::LED_GREEN, LOW);
+    digitalWrite(HARDWARE::LED_RED, HIGH);
+  } else if (!communication_good) {
+    digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
+    digitalWrite(HARDWARE::LED_GREEN, HIGH);
+    digitalWrite(HARDWARE::LED_RED, LOW);
+  }
+}
+
+void set_pan_tilt_adjustments() {
+  if (communication_good) {
+    if (modbus_data[MODBUS_REGISTERS::CENTER_ALL]) {
+      pan_servo.write(constrain(pan_position, pan_min, pan_max));
+      tilt_servo.write(constrain(tilt_position, tilt_min, tilt_max));
+
+      pan_position = pan_center;
+      tilt_position = tilt_center;
+
+      modbus_data[MODBUS_REGISTERS::CENTER_ALL] = 0;
+    }
+
+    pan_position = pan_position - modbus_data[MODBUS_REGISTERS::PAN_ADJUST_POSITIVE] + modbus_data[MODBUS_REGISTERS::PAN_ADJUST_NEGATIVE];
+    tilt_position = tilt_position + modbus_data[MODBUS_REGISTERS::TILT_ADJUST_POSITIVE] - modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE];
+
+    pan_servo.write(constrain(pan_position, pan_min, pan_max));
+    tilt_servo.write(constrain(tilt_position, tilt_min, tilt_max));
+
+    modbus_data[MODBUS_REGISTERS::PAN_ADJUST_POSITIVE] = 0;
+    modbus_data[MODBUS_REGISTERS::PAN_ADJUST_NEGATIVE] = 0;
+    modbus_data[MODBUS_REGISTERS::TILT_ADJUST_POSITIVE] = 0;
+    modbus_data[MODBUS_REGISTERS::TILT_ADJUST_NEGATIVE] = 0;
+  }
+}
+

software/ros_packages/rover_control/CMakeLists.txt

@@ -54,6 +54,7 @@ find_package(catkin REQUIRED COMPONENTS
    DriveControlMessage.msg
    DriveStatusMessage.msg
    IrisStatusMessage.msg
+   TowerPanTiltControlMessage.msg
  )
 
 ## Generate services in the 'srv' folder

software/ros_packages/rover_control/msg/TowerPanTiltControlMessage.msg

@@ -0,0 +1,4 @@
+bool should_center
+
+int16 relative_pan_adjustment
+int16 relative_tilt_adjustment

software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+import rospy
+import time
+
+from rover_control.msg import TowerPanTiltControlMessage
+
+DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "tower/control"
+
+rospy.init_node("tower_pan_tilt_tester")
+
+publisher = rospy.Publisher(DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC, TowerPanTiltControlMessage, queue_size=1)
+
+time.sleep(2)
+
+message = TowerPanTiltControlMessage()
+message.should_center = 1
+
+publisher.publish(message)
+
+time.sleep(1)
+
+message = TowerPanTiltControlMessage()
+message.relative_pan_adjustment = -100
+message.relative_tilt_adjustment = -500
+
+publisher.publish(message)

software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_control.py

@@ -0,0 +1,152 @@
+#!/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 TowerPanTiltControlMessage
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "drive_control"
+
+DEFAULT_PORT = "/dev/ttyUSB0"
+DEFAULT_BAUD = 115200
+
+DEFAULT_INVERT = False
+
+DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC = "tower/control"
+
+NODE_ID = 1
+
+COMMUNICATIONS_TIMEOUT = 0.01  # Seconds
+
+RX_DELAY = 0.01
+TX_DELAY = 0.01
+
+DEFAULT_HERTZ = 20
+
+MODBUS_REGISTERS = {
+    "CENTER_ALL": 0,
+
+    "PAN_ADJUST_POSITIVE": 1,
+    "PAN_ADJUST_NEGATIVE": 2,
+    "TILT_ADJUST_POSITIVE": 3,
+    "TILT_ADJUST_NEGATIVE": 4
+}
+
+PAN_TILT_CONTROL_DEFAULT_MESSAGE = [
+    0,  # No centering
+    0,  # No pan positive adjustment
+    0,  # No pan negative adjustment
+    0,  # No tilt positive adjustment
+    0   # No tilt negative adjustement
+]
+
+PAN_TILT_LAST_SEEN_TIMEOUT = 2  # seconds
+
+
+#####################################
+# DriveControl Class Definition
+#####################################
+class TowerPanTiltControl(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.node_id = rospy.get_param("~node_id", NODE_ID)
+
+        self.pan_tilt_control_subscriber_topic = rospy.get_param("~pan_tilt_control_topic", DEFAULT_TOWER_PAN_TILT_CONTROL_TOPIC)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.pan_tilt_node = None
+
+        self.connect_to_pan_tilt()
+
+        self.pan_tilt_control_subscriber = \
+            rospy.Subscriber(self.pan_tilt_control_subscriber_topic, TowerPanTiltControlMessage, self.pan_tilt_control_callback)
+
+        self.pan_tilt_control_message = None
+        self.new_control_message = False
+
+        self.pan_tilt_last_seen = time()
+
+        self.run()
+
+    def __setup_minimalmodbus_for_485(self):
+        self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.pan_tilt_node.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):
+        self.send_startup_centering_command()
+
+        while not rospy.is_shutdown():
+            start_time = time()
+
+            try:
+                self.send_pan_tilt_control_message()
+                self.pan_tilt_last_seen = time()
+
+            except Exception, error:
+                print "Error occurred:", error
+
+            if (time() - self.pan_tilt_last_seen) > PAN_TILT_LAST_SEEN_TIMEOUT:
+                print "Tower pan/tilt not seen for", PAN_TILT_LAST_SEEN_TIMEOUT, "seconds. Exiting."
+                return  # Exit so respawn can take over
+
+            time_diff = time() - start_time
+
+            sleep(max(self.wait_time - time_diff, 0))
+
+    def connect_to_pan_tilt(self):
+        self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.node_id))
+        self.__setup_minimalmodbus_for_485()
+
+    def send_startup_centering_command(self):
+        registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
+        registers[MODBUS_REGISTERS["CENTER_ALL"]] = 1
+        self.pan_tilt_node.write_registers(0, registers)
+
+    def send_pan_tilt_control_message(self):
+        if self.new_control_message:
+            pan_tilt_control_message = self.pan_tilt_control_message  # type: TowerPanTiltControlMessage
+
+            registers = list(PAN_TILT_CONTROL_DEFAULT_MESSAGE)
+            registers[MODBUS_REGISTERS["CENTER_ALL"]] = int(pan_tilt_control_message.should_center)
+
+            if pan_tilt_control_message.relative_pan_adjustment >= 0:
+                registers[MODBUS_REGISTERS["PAN_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_pan_adjustment
+            else:
+                registers[MODBUS_REGISTERS["PAN_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_pan_adjustment
+
+            if pan_tilt_control_message.relative_tilt_adjustment >= 0:
+                registers[MODBUS_REGISTERS["TILT_ADJUST_POSITIVE"]] = pan_tilt_control_message.relative_tilt_adjustment
+            else:
+                registers[MODBUS_REGISTERS["TILT_ADJUST_NEGATIVE"]] = -pan_tilt_control_message.relative_tilt_adjustment
+
+            self.pan_tilt_node.write_registers(0, registers)
+
+            self.new_control_message = False
+        else:
+            self.pan_tilt_node.write_registers(0, PAN_TILT_CONTROL_DEFAULT_MESSAGE)
+
+    def pan_tilt_control_callback(self, pan_tilt_control):
+        self.pan_tilt_control_message = pan_tilt_control
+        self.new_control_message = True
+
+
+if __name__ == "__main__":
+    TowerPanTiltControl()