Started adding in xbox controller to gui. Thinking of scrapping spacenav mouse. It kinda sucks. Also, three axes moving on the arm! Time to make an action client for moveitgit add .git add .!

software/environment/rover/UDEV_rules/99-rover-usb-serial.rules

@@ -7,6 +7,8 @@
 #########################
 
 
+##### NOTE!!!! The IONIs cannot be on IRIS. The throughput is too high for it to handle with only a 12Mbit usb hub onboard #####
+
 # IRIS Board Mappings
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="00", SYMLINK+="rover/ttyREAR"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM2CZLZF", ENV{ID_USB_INTERFACE_NUM}=="01", SYMLINK+="rover/ttyLEFT"
@@ -23,6 +25,8 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="02", SYMLINK+="rover/ttyIRIS"
 SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{serial}=="NM34WE0R", ENV{ID_USB_INTERFACE_NUM}=="03", SYMLINK+="rover/ttyExtraUART"
 
+# External 485 adapter for the ARM
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyARM"
 
 ##### Mappings from OMSI / EXPO driving with individual adapters
 # SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyLEFT"

software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py

@@ -0,0 +1,168 @@
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import logging
+from inputs import devices, GamePad
+from time import time
+
+import rospy
+from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage
+
+#####################################
+# Global Variables
+#####################################
+GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360"
+
+DRIVE_COMMAND_HERTZ = 20
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class XBOXController(QtCore.QThread):
+    def __init__(self):
+        super(XBOXController, self).__init__()
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+        self.setup_controller_flag = True
+        self.data_acquisition_and_broadcast_flag = True
+        self.controller_acquired = False
+
+        # ########## Class Variables ##########
+        self.gamepad = None  # type: GamePad
+
+        self.controller_states = {
+            "x_axis": 512,
+            "y_axis": 512,
+            "z_axis": 128,
+            "throttle_axis": 128,
+
+            "hat_x_axis": 0,
+            "hat_y_axis": 0,
+
+            "trigger_pressed": 0,
+            "thumb_pressed": 0,
+            "three_pressed": 0,
+            "four_pressed": 0,
+            "five_pressed": 0,
+            "six_pressed": 0,
+
+            "seven_pressed": 0,
+            "eight_pressed": 0,
+            "nine_pressed": 0,
+            "ten_pressed": 0,
+            "eleven_pressed": 0,
+            "twelve_pressed": 0,
+        }
+
+        self.raw_mapping_to_class_mapping = {
+            "ABS_X": "x_axis",
+            "ABS_Y": "y_axis",
+            "ABS_RZ": "z_axis",
+            "ABS_THROTTLE": "throttle_axis",
+
+            "ABS_HAT0X": "hat_x_axis",
+            "ABS_HAT0Y": "hat_y_axis",
+
+            "BTN_TRIGGER": "trigger_pressed",
+            "BTN_THUMB": "thumb_pressed",
+            "BTN_THUMB2": "three_pressed",
+            "BTN_TOP": "four_pressed",
+            "BTN_TOP2": "five_pressed",
+            "BTN_PINKIE": "six_pressed",
+
+            "BTN_BASE": "seven_pressed",
+            "BTN_BASE2": "eight_pressed",
+            "BTN_BASE3": "nine_pressed",
+            "BTN_BASE4": "ten_pressed",
+            "BTN_BASE5": "eleven_pressed",
+            "BTN_BASE6": "twelve_pressed"
+        }
+
+        self.ready = False
+
+        self.start()
+
+    def run(self):
+
+        while self.run_thread_flag:
+            if self.setup_controller_flag:
+                self.controller_acquired = self.__setup_controller()
+                self.setup_controller_flag = False
+            if self.data_acquisition_and_broadcast_flag:
+                self.__get_controller_data()
+
+    def __setup_controller(self):
+        for device in devices.gamepads:
+            print device
+            if device.name == GAME_CONTROLLER_NAME:
+                self.gamepad = device
+
+                return True
+        return False
+
+    def __get_controller_data(self):
+        if self.controller_acquired:
+            events = self.gamepad.read()
+
+            for event in events:
+                print event.code
+                if event.code in self.raw_mapping_to_class_mapping:
+                    self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
+
+            self.ready = True
+
+
+#####################################
+# Controller Class Definition
+#####################################
+class ControllerControlSender(QtCore.QThread):
+
+    def __init__(self, shared_objects):
+        super(ControllerControlSender, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.video_coordinator = self.shared_objects["threaded_classes"]["Video Coordinator"]
+        self.right_screen = self.shared_objects["screens"]["right_screen"]
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = False
+
+        self.controller = XBOXController()
+
+        # ########## Class Variables ##########
+
+        self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
+
+    def run(self):
+        self.logger.debug("Starting Joystick Thread")
+
+        while self.run_thread_flag:
+            start_time = time()
+
+            time_diff = time() - start_time
+
+            self.msleep(max(int(self.wait_time - time_diff), 0))
+
+        self.logger.debug("Stopping Joystick Thread")
+
+    def connect_signals_and_slots(self):
+        pass
+
+    def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+        start_signal.connect(self.start)
+        signals_and_slots_signal.connect(self.connect_signals_and_slots)
+        kill_signal.connect(self.on_kill_threads_requested__slot)
+
+    def on_kill_threads_requested__slot(self):
+        self.run_thread_flag = False

software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py

@@ -193,6 +193,8 @@ class JoystickControlSender(QtCore.QThread):
         self.last_camera_toggle_time = time()
 
     def run(self):
+        self.logger.debug("Starting Joystick Thread")
+
         while self.run_thread_flag:
             start_time = time()
 
@@ -203,6 +205,8 @@ class JoystickControlSender(QtCore.QThread):
 
             self.msleep(max(int(self.wait_time - time_diff), 0))
 
+        self.logger.debug("Stopping Joystick Thread")
+
     def connect_signals_and_slots(self):
         self.set_speed_limit__signal.connect(self.speed_limit_progress_bar.setValue)
         self.set_left_drive_output__signal.connect(self.left_drive_progress_bar.setValue)

software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py

@@ -111,6 +111,7 @@ class SpaceNavControlSender(QtCore.QThread):
         self.current_control_mode = self.MINING_MODE
 
     def run(self):
+        self.logger.debug("Starting SpaceNav Mouse Thread")
         spnav.spnav_open()
 
         while self.run_thread_flag:
@@ -125,10 +126,13 @@ class SpaceNavControlSender(QtCore.QThread):
 
             # self.msleep(max(int((self.wait_time - time_diff) * 1000), 0))
 
+        self.logger.debug("Stopping SpaceNav Mouse Thread")
+
     def process_spnav_events(self):
         event = spnav.spnav_poll_event()
 
         if event:
+            # print event
             if event.ev_type == spnav.SPNAV_EVENT_MOTION:
                 self.spnav_states["linear_x"] = event.translation[0] / 350.0
                 self.spnav_states["linear_y"] = event.translation[2] / 350.0

software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py

@@ -33,6 +33,8 @@ class Mining(QtCore.QObject):
     lift_position_update_ready__signal = QtCore.pyqtSignal(int)
     tilt_position_update_ready__signal = QtCore.pyqtSignal(int)
 
+    weight_measurement_update_ready__signal = QtCore.pyqtSignal(int)
+
     def __init__(self, shared_objects):
         super(Mining, self).__init__()
 
@@ -81,6 +83,8 @@ class Mining(QtCore.QObject):
         self.tilt_position_update_ready__signal.connect(self.tilt_position_progress_bar.setValue)
         self.lift_position_update_ready__signal.connect(self.lift_position_progress_bar.setValue)
 
+        self.weight_measurement_update_ready__signal.connect(self.mining_qlcdnumber.display)
+
     def on_mining_set_cal_factor_clicked__slot(self):
         message = MiningControlMessage()
 
@@ -136,4 +140,4 @@ class Mining(QtCore.QObject):
         status = status  # type:MiningStatusMessage
         self.tilt_position_update_ready__signal.emit(status.tilt_position)
         self.lift_position_update_ready__signal.emit(status.lift_position)
-        self.mining_qlcdnumber.display(status.measured_weight)
+        self.weight_measurement_update_ready__signal.emit(status.measured_weight)

software/ros_packages/ground_station/src/ground_station.py

@@ -16,6 +16,7 @@ import Framework.LoggingSystems.Logger as Logger
 import Framework.VideoSystems.RoverVideoCoordinator as RoverVideoCoordinator
 import Framework.MapSystems.RoverMapCoordinator as RoverMapCoordinator
 import Framework.InputSystems.JoystickControlSender as JoystickControlSender
+import Framework.InputSystems.ControllerControlSender as ControllerControlSender
 import Framework.NavigationSystems.SpeedAndHeadingIndication as SpeedAndHeading
 import Framework.NavigationSystems.WaypointsCoordinator as WaypointsCoordinator
 import Framework.ArmSystems.ArmIndication as ArmIndication
@@ -108,6 +109,7 @@ class GroundStation(QtCore.QObject):
         self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
         self.__add_thread("Map Coordinator", RoverMapCoordinator.RoverMapCoordinator(self.shared_objects))
         self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects))
+        self.__add_thread("Controller Sender", ControllerControlSender.ControllerControlSender(self.shared_objects))
         self.__add_thread("Speed and Heading", SpeedAndHeading.SpeedAndHeadingIndication(self.shared_objects))
         self.__add_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects))
         self.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects))

software/ros_packages/rover_arm/src/rover_arm.cpp

@@ -1,5 +1,6 @@
 #include <string>
 #include <iostream>
+#include <unistd.h>
 
 #include <ros/ros.h>
 #include <ros/console.h>
@@ -13,8 +14,7 @@ int device_address = 1;
 
 ////////// Global Variables //////////
 // ROS Parameter Defaults
-const string default_port = "/dev/ttyUSB0";
+const string default_port = "/dev/rover/ttyARM";
-
 
 // Axis Defaults
 const smint32 axis1_max_count = 100000;
@@ -47,6 +47,8 @@ public:
         if(arm_bus_handle < 0){
             ROS_ERROR("Could not connect to arm");
         }
+
+        ROS_INFO("Connected to smBus");
     }
 
     void run(){
@@ -57,18 +59,40 @@ public:
         while(ros::ok()){
             smSetParameter(arm_bus_handle, device_address, SMP_FAULTS, 0);
             smSetParameter(arm_bus_handle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
+            smSetParameter(arm_bus_handle, 2, SMP_FAULTS, 0);
+            smSetParameter(arm_bus_handle, 2, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
+            smSetParameter(arm_bus_handle, 3, SMP_FAULTS, 0);
+            smSetParameter(arm_bus_handle, 3, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
 
             smint32 status = 0;
-            while(!(status & STAT_TARGET_REACHED)){
+            smint32 status1 = 0;
-                smRead1Parameter(arm_bus_handle, device_address, SMP_STATUS, &status);
+            smint32 status2 = 0;
+
+            smint32 position = 0;
+            smint32 position1 = 0;
+            smint32 position2 = 0;
+
+            smint32 read_bus_voltage;
+
+            while(!(status & STAT_TARGET_REACHED) || !(status1 & STAT_TARGET_REACHED) || !(status2 & STAT_TARGET_REACHED)){
+                smRead2Parameters(arm_bus_handle, device_address, SMP_STATUS, &status, SMP_ACTUAL_POSITION_FB, &position);
+                smRead2Parameters(arm_bus_handle, 2, SMP_STATUS, &status1, SMP_ACTUAL_POSITION_FB, &position1);
+                smRead2Parameters(arm_bus_handle, 3, SMP_STATUS, &status2, SMP_ACTUAL_POSITION_FB, &position2);
+                smRead1Parameter(arm_bus_handle, device_address, SMP_ACTUAL_BUS_VOLTAGE, &read_bus_voltage);
+
+                ROS_INFO("Dir: %u\tVoltage: %ld\tAxis 1: %ld\tAxis 2: %ld\tAxis 3: %ld", dir, read_bus_voltage, position, position1, position2);
             }
 
             dir = !dir;
 
             if(dir){
                 smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0);
+                smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 0);
+                smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 0);
             }else{
-                smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 330000);
+                smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 50000);
+                smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 300000);
+                smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 250000);
             }
 
             ros::spinOnce();

software/ros_packages/rover_arm/src/simplemotion/busdevice.c

@@ -63,7 +63,7 @@ smbusdevicehandle smBDOpen( const char *devicename )
 	//all handles in use
 	if(handle>=SM_MAX_BUSES) return -1;
 
-        if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0) //use rs232 lib
+        if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0 || strncmp(devicename,"/dev/rover/tty",8) == 0)
 	{
             BusDevice[handle].comPort=OpenComport( devicename, SM_BAUDRATE );
                 if( BusDevice[handle].comPort == -1 )

software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch

@@ -155,7 +155,7 @@
                 {name: "/rover_status/battery_status", compress: false, rate: 1.0},
                 {name: "/rover_control/tower/status/co2", compress: false, rate: 1.0},
                 {name: "/rover_odometry/imu/data", compress: false, rate: 10.0},
-                {name: "/rover_control/mining/status", compress: false, rate: 2.0},
+                {name: "/rover_control/mining/status", compress: false, rate: 5.0},
                 ]
             </rosparam>
         </node>