Some new drive code. Firmware too, but that's not up yet.

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

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

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