From 897e0fa3d21d098768a5ac55fbb636277b51df55 Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Sat, 26 May 2018 15:32:17 -0700 Subject: [PATCH] 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 | 41 ++--- .../tower_pan_tilt/tower_pan_tilt.ino | 145 ++++++++++++++++- .../ros_packages/rover_control/CMakeLists.txt | 1 + .../msg/TowerPanTiltControlMessage.msg | 4 + .../tower_pan_tilt_control/pan_tilt_tester.py | 26 +++ .../tower_pan_tilt_control.py | 152 ++++++++++++++++++ 6 files changed, 348 insertions(+), 21 deletions(-) create mode 100644 software/ros_packages/rover_control/msg/TowerPanTiltControlMessage.msg create mode 100755 software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py create mode 100755 software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_control.py diff --git a/software/firmware/tower/tower.ino b/software/firmware/tower/tower.ino index b170a39..b331f82 100644 --- a/software/firmware/tower/tower.ino +++ b/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 commsPort Serial2 -#define GPS_PORT_NAME "Serial3" +#define gpsPort Serial2 +#define commsPort 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(); // diff --git a/software/firmware/tower_pan_tilt/tower_pan_tilt.ino b/software/firmware/tower_pan_tilt/tower_pan_tilt.ino index 95c2b6e..843263c 100644 --- a/software/firmware/tower_pan_tilt/tower_pan_tilt.ino +++ b/software/firmware/tower_pan_tilt/tower_pan_tilt.ino @@ -1,9 +1,148 @@ +////////// Includes ////////// +#include +#include + +////////// 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; + } +} + diff --git a/software/ros_packages/rover_control/CMakeLists.txt b/software/ros_packages/rover_control/CMakeLists.txt index 0156d23..2c9d483 100644 --- a/software/ros_packages/rover_control/CMakeLists.txt +++ b/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 diff --git a/software/ros_packages/rover_control/msg/TowerPanTiltControlMessage.msg b/software/ros_packages/rover_control/msg/TowerPanTiltControlMessage.msg new file mode 100644 index 0000000..8d89609 --- /dev/null +++ b/software/ros_packages/rover_control/msg/TowerPanTiltControlMessage.msg @@ -0,0 +1,4 @@ +bool should_center + +int16 relative_pan_adjustment +int16 relative_tilt_adjustment \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py b/software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py new file mode 100755 index 0000000..507eb34 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_control.py b/software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_control.py new file mode 100755 index 0000000..4c74285 --- /dev/null +++ b/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() \ No newline at end of file