diff --git a/software/environment_reference/UDEV Rules/99-rover-usb-serial.rules b/software/environment_reference/UDEV Rules/99-rover-usb-serial.rules
index 03d4f75..7ef3f03 100644
--- a/software/environment_reference/UDEV Rules/99-rover-usb-serial.rules
+++ b/software/environment_reference/UDEV Rules/99-rover-usb-serial.rules
@@ -1,7 +1,25 @@
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyDEV0"
+# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyDEV0"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyDEV1"
+# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyDEV1"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyCompass"
+# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyCompass"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyGPS"
+# SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="FTU9EU0I", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyGPS"
+
+
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2NFZVA", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_0_0"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2NFZVA", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_0_1"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2NFZVA", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS_0_2"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2NFZVA", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_0_3"
+
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2RJ8G5", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_1_0"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2RJ8G5", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_1_1"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2RJ8G5", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS_1_2"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2RJ8G5", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_1_3"
+
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2UFVE1", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyIRIS_2_0"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2UFVE1", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyIRIS_2_1"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2UFVE1", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS_2_2"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2UFVE1", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyIRIS_2_3"
+
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyTEST"
\ No newline at end of file
diff --git a/software/rover/rover_drive/.idea/workspace.xml b/software/rover/rover_drive/.idea/workspace.xml
index d05528a..c38898b 100644
--- a/software/rover/rover_drive/.idea/workspace.xml
+++ b/software/rover/rover_drive/.idea/workspace.xml
@@ -13,7 +13,7 @@
-
+
@@ -27,8 +27,8 @@
-
-
+
+
@@ -37,8 +37,8 @@
-
-
+
+
@@ -46,11 +46,11 @@
-
+
-
-
+
+
@@ -58,31 +58,11 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
-
-
+
+
@@ -104,9 +84,10 @@
-
+
+
@@ -118,7 +99,7 @@
-
+
@@ -470,16 +451,16 @@
-
+
-
+
-
+
@@ -509,6 +490,246 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -523,7 +744,6 @@
-
@@ -542,34 +762,35 @@
+
+
+
+
+
+
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
-
-
+
+
@@ -578,19 +799,17 @@
-
-
+
+
-
+
-
-
-
-
-
+
+
+
diff --git a/software/rover/rover_drive/launch/example.launch b/software/rover/rover_drive/launch/example.launch
index c4cee6a..63ea410 100644
--- a/software/rover/rover_drive/launch/example.launch
+++ b/software/rover/rover_drive/launch/example.launch
@@ -1,8 +1,8 @@
-
-
+
+
diff --git a/software/rover/rover_drive/src/controllertest.py b/software/rover/rover_drive/src/controllertest.py
new file mode 100644
index 0000000..0341a8a
--- /dev/null
+++ b/software/rover/rover_drive/src/controllertest.py
@@ -0,0 +1,60 @@
+import rospy
+import time
+import atexit
+import signal
+
+import serial.rs485
+import minimalmodbus
+
+from rover_drive.msg import RoverMotorDrive
+
+signal.signal(signal.SIGINT, signal.SIG_DFL)
+
+rospy.init_node("drive_tester")
+
+pub = rospy.Publisher("/drive/motoroneandtwo", RoverMotorDrive, queue_size=1)
+
+
+def make_instr(port_name, baud):
+ instr = minimalmodbus.Instrument(port_name, 1)
+ instr.serial = serial.rs485.RS485(port_name, baudrate=baud, timeout=0.05)
+ instr.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=0,
+ delay_before_tx=0)
+ return instr
+
+sbus = make_instr("/dev/rover/ttyIRIS_2_2", 2000000)
+
+max = 1811
+mid = 990
+min = 172
+
+scalar = 75
+
+drive = RoverMotorDrive()
+while not rospy.is_shutdown():
+ left, right = sbus.read_registers(0, 2)
+ # print left
+ drive.first_motor_direction = 1 if left > mid else 0
+ drive.first_motor_speed = abs(left - mid ) * scalar
+ # print drive.first_motor_speed
+ pub.publish(drive)
+ # try:
+ # drive = RoverMotorDrive()
+ #
+ # last_dir = not last_dir
+ # for i in range(0, 65535, speed_step):
+ # drive.first_motor_direction = last_dir
+ # drive.first_motor_speed = i
+ # pub.publish(drive)
+ # time.sleep(sleep_time)
+ # print i
+ # for i in range(65535, 0, -speed_step):
+ # drive.first_motor_direction = last_dir
+ # drive.first_motor_speed = i
+ # pub.publish(drive)
+ # time.sleep(sleep_time)
+ # print i
+ # except KeyboardInterrupt:
+ # exit()
+
+
diff --git a/software/rover/rover_drive/src/rover_drive_node.py b/software/rover/rover_drive/src/rover_drive_node.py
index 8b430ea..8d59e85 100755
--- a/software/rover/rover_drive/src/rover_drive_node.py
+++ b/software/rover/rover_drive/src/rover_drive_node.py
@@ -61,6 +61,7 @@ class RoverDrive(object):
self.first_motor = minimalmodbus.Instrument(self.port, self.first_motor_modbus_id)
self.second_motor = minimalmodbus.Instrument(self.port, self.second_motor_modbus_id)
+ print self.port
self.motors_subscriber = rospy.Subscriber(MOTOR_TOPIC, RoverMotorDrive, self.__motor_message_callback)
@@ -91,11 +92,14 @@ class RoverDrive(object):
serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0, delay_before_rx=0, delay_before_tx=0)
def __motor_message_callback(self, data):
- first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE) # Makes a copy
- first_motor_register_data[MOTOR_DRIVER_REGISTER_MAP["DIRECTION"]] = int(data.first_motor_direction)
- first_motor_register_data[MOTOR_DRIVER_REGISTER_MAP["SPEED"]] = data.first_motor_speed
- print first_motor_register_data
- self.first_motor.write_registers(0, first_motor_register_data)
+ try:
+ first_motor_register_data = list(MOTOR_DRIVER_DEFAULT_MESSAGE) # Makes a copy
+ first_motor_register_data[MOTOR_DRIVER_REGISTER_MAP["DIRECTION"]] = int(data.first_motor_direction)
+ first_motor_register_data[MOTOR_DRIVER_REGISTER_MAP["SPEED"]] = data.first_motor_speed
+ # print first_motor_register_data
+ self.first_motor.write_registers(0, first_motor_register_data)
+ except:
+ print "Drive exception"
#####################################
# Main Definition
diff --git a/software/rover/rover_drive/src/rover_drive_node_tester.py b/software/rover/rover_drive/src/rover_drive_node_tester.py
index 33ef88a..d2a9318 100644
--- a/software/rover/rover_drive/src/rover_drive_node_tester.py
+++ b/software/rover/rover_drive/src/rover_drive_node_tester.py
@@ -1,31 +1,50 @@
import rospy
import time
+import atexit
+import signal
from rover_drive.msg import RoverMotorDrive
+signal.signal(signal.SIGINT, signal.SIG_DFL)
+
rospy.init_node("drive_tester")
-pub = rospy.Publisher("/drive/motoroneandtwo", RoverMotorDrive, queue_size=10)
+pub = rospy.Publisher("/drive/motoroneandtwo", RoverMotorDrive, queue_size=1)
-speed_step = 500
+speed_step = 1000
last_dir = 0
-sleep_time = 0.01
+sleep_time = 0.1
+
+
+def stop_motors():
+ global pub
+
+ drive.first_motor_direction = last_dir
+ drive.first_motor_speed = 0
+ pub.publish(drive)
+
+atexit.register(stop_motors)
while not rospy.is_shutdown():
- drive = RoverMotorDrive()
+ try :
+ drive = RoverMotorDrive()
+
+ last_dir = not last_dir
+ for i in range(0, 65535, speed_step):
+ drive.first_motor_direction = last_dir
+ drive.first_motor_speed = i
+ pub.publish(drive)
+ time.sleep(sleep_time)
+ print i
+ for i in range(65535, 0, -speed_step):
+ drive.first_motor_direction = last_dir
+ drive.first_motor_speed = i
+ pub.publish(drive)
+ time.sleep(sleep_time)
+ print i
+ except KeyboardInterrupt:
+ exit()
+
- last_dir = not last_dir
- for i in range(0, 65535, speed_step):
- drive.first_motor_direction = last_dir
- drive.first_motor_speed = i
- pub.publish(drive)
- time.sleep(sleep_time)
- print i
- for i in range(65535, 0, -speed_step):
- drive.first_motor_direction = last_dir
- drive.first_motor_speed = i
- pub.publish(drive)
- time.sleep(sleep_time)
- print i