diff --git a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules index 418b174..901ff80 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/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" diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py new file mode 100644 index 0000000..c3c7705 --- /dev/null +++ b/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 diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py index 7070752..99b3bee 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/JoystickControlSender.py +++ b/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) diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py index 43fd862..9eca7ca 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/SpaceNavControlSender.py +++ b/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 diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py index 87f8aec..2967871 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py +++ b/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) diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 7be7e2d..b1facf9 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/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)) diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index cf134c2..2848d46 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -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)){ - smRead1Parameter(arm_bus_handle, device_address, SMP_STATUS, &status); + smint32 status1 = 0; + 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(); diff --git a/software/ros_packages/rover_arm/src/simplemotion/busdevice.c b/software/ros_packages/rover_arm/src/simplemotion/busdevice.c index 9bf82c8..5b43ddc 100644 --- a/software/ros_packages/rover_arm/src/simplemotion/busdevice.c +++ b/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 ) diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 88e9805..3ce6974 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/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}, ]