caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Corwin Perren
2018-05-26 15:32:17 -07:00
parent fea235fc67
commit 897e0fa3d2
6 changed files with 348 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
software/firmware/tower/tower.ino
View File

@@ -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,31 +118,31 @@ 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);

145
software/firmware/tower_pan_tilt/tower_pan_tilt.ino
View File

@@ -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;
}
}

1
software/ros_packages/rover_control/CMakeLists.txt
View File

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

4
software/ros_packages/rover_control/msg/TowerPanTiltControlMessage.msg Normal file
View File

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

26
software/ros_packages/rover_control/src/tower_pan_tilt_control/pan_tilt_tester.py Executable file
View File

@@ -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)

152
software/ros_packages/rover_control/src/tower_pan_tilt_control/tower_pan_tilt_control.py Executable file
View File

@@ -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()