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 7ef3f03..b2abd8a 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -22,4 +22,6 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria 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 +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="AH05K2Y8", SYMLINK+="rover/ttyLEFT" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="A906H89E", SYMLINK+="rover/ttyRIGHT" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", ATTRS{serial}=="A906H7P0", SYMLINK+="rover/ttyREAR" \ No newline at end of file diff --git a/software/firmware/motor_driver/motor_driver.ino b/software/firmware/motor_driver/motor_driver.ino index dc504d8..44a2933 100644 --- a/software/firmware/motor_driver/motor_driver.ino +++ b/software/firmware/motor_driver/motor_driver.ino @@ -53,7 +53,7 @@ void setup() { setup_hardware(); num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); - slave.begin(2000000); // baud-rate at 19200 + slave.begin(115200); // baud-rate at 19200 slave.setTimeOut(150); Serial.begin(9600); diff --git a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py index 00a339b..2a44149 100644 --- a/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py +++ b/software/ros_packages/ground_station/src/Framework/StatusSystems/StatusCore.py @@ -67,9 +67,6 @@ class SensorCore(QtCore.QThread): self.main_cam = self.screen_main_window.main_cam # type: QtWidgets.QLabel self.chassis_cam = self.screen_main_window.chassis_cam # type: QtWidgets.QLabel self.under_cam = self.screen_main_window.under_cam # type: QtWidgets.QLabel - self.bogie_right = self.screen_main_window.right_bogie # type: QtWidgets.QLabel - self.bogie_left = self.screen_main_window.left_bogie # type: QtWidgets.QLabel - self.bogie_rear = self.screen_main_window.rear_bogie # type: QtWidgets.QLabel self.clock = self.screen_main_window.clock_qlcdnumber # type: QtWidgets.QLCDNumber self.cpu = self.screen_main_window.cpu # type: QtWidgets.QLabel self.ram = self.screen_main_window.ram # type: QtWidgets.QLabel @@ -78,7 +75,6 @@ class SensorCore(QtCore.QThread): # ########## subscriptions pulling data from system_statuses_node.py ########## self.camera_status = rospy.Subscriber(CAMERA_TOPIC_NAME, CameraStatuses, self.__camera_callback) - self.bogie_status = rospy.Subscriber(BOGIE_TOPIC_NAME, BogieStatuses, self.__bogie_callback) self.frsky_status = rospy.Subscriber(FRSKY_TOPIC_NAME, FrSkyStatus, self.__frsky_callback) self.gps_status = rospy.Subscriber(GPS_TOPIC_NAME, GPSInfo, self.__gps_callback) self.jetson_status = rospy.Subscriber(JETSON_TOPIC_NAME, JetsonInfo, self.__jetson_callback) @@ -140,32 +136,6 @@ class SensorCore(QtCore.QThread): else: self.frsky_stylesheet_change_ready__signal.emit("background-color: darkgreen;") - def __bogie_callback(self, data): - self.bogie_msg.bogie_connection_1 = data.bogie_connection_1 - self.bogie_msg.bogie_connection_2 = data.bogie_connection_2 - self.bogie_msg.bogie_connection_3 = data.bogie_connection_3 - - if data.bogie_connection_1 is False: - # self.bogie_right.setStyleSheet("background-color: darkred;") - self.bogie_connection_1_stylesheet_change_ready__signal.emit("background-color: darkred;") - else: - # self.bogie_right.setStyleSheet("background-color: darkgreen;") - self.bogie_connection_1_stylesheet_change_ready__signal.emit("background-color: darkgreen;") - - if data.bogie_connection_2 is False: - # self.bogie_left.setStyleSheet("background-color: darkred;") - self.bogie_connection_2_stylesheet_change_ready__signal.emit("background-color: darkred;") - else: - # self.bogie_left.setStyleSheet("background-color: darkgreen;") - self.bogie_connection_2_stylesheet_change_ready__signal.emit("background-color: darkgreen;") - - if data.bogie_connection_3 is False: - # self.bogie_rear.setStyleSheet("background-color: darkred;") - self.bogie_connection_3_stylesheet_change_ready__signal.emit("background-color: darkred;") - else: - # self.bogie_rear.setStyleSheet("background-color: darkgreen;") - self.bogie_connection_3_stylesheet_change_ready__signal.emit("background-color: darkgreen;") - def __jetson_callback(self, data): self.jetson_cpu_update_ready__signal.emit("TX2 CPU\n" + str(data.jetson_CPU) + "%") @@ -238,9 +208,6 @@ class SensorCore(QtCore.QThread): self.jetson_emmc_stylesheet_change_ready__signal.connect(self.emmc.setStyleSheet) self.jetson_gpu_temp_update_ready__signal.connect(self.gpu_temp.setText) self.jetson_gpu_temp_stylesheet_change_ready__signal.connect(self.gpu_temp.setStyleSheet) - self.bogie_connection_1_stylesheet_change_ready__signal.connect(self.bogie_right.setStyleSheet) - self.bogie_connection_2_stylesheet_change_ready__signal.connect(self.bogie_left.setStyleSheet) - self.bogie_connection_3_stylesheet_change_ready__signal.connect(self.bogie_rear.setStyleSheet) self.camera_zed_stylesheet_change_ready__signal.connect(self.zed.setStyleSheet) self.camera_under_stylesheet_change_ready__signal.connect(self.under_cam.setStyleSheet) self.camera_chassis_stylesheet_change_ready__signal.connect(self.chassis_cam.setStyleSheet) diff --git a/software/ros_packages/ground_station/src/Resources/Images/osurclogo.png b/software/ros_packages/ground_station/src/Resources/Images/osurclogo.png index e5ea847..7107d7f 100644 Binary files a/software/ros_packages/ground_station/src/Resources/Images/osurclogo.png and b/software/ros_packages/ground_station/src/Resources/Images/osurclogo.png differ diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 38057f2..cbcd109 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -277,7 +277,7 @@ - + @@ -322,7 +322,7 @@ Connected - + @@ -395,7 +395,7 @@ Connected - background-color:darkred; + background-color:darkgreen; QFrame::NoFrame @@ -443,7 +443,7 @@ Connected - background-color:darkred; + background-color:darkgreen; FrSky @@ -485,7 +485,7 @@ Connected false - background-color:darkred; + background-color:darkgreen; 3D Nav Mouse @@ -524,7 +524,7 @@ Connected - background-color:darkred; + background-color:darkgreen; QFrame::NoFrame @@ -538,7 +538,7 @@ Connected - + @@ -611,7 +611,7 @@ Connected false - background-color:darkred; + background-color:darkgreen; GPS @@ -623,7 +623,7 @@ Fix - + 0 @@ -656,8 +656,8 @@ Fix background-color:darkred; - Right Bogie -Connected + Middle Left Wheel +Disconnected Qt::AlignCenter @@ -665,7 +665,7 @@ Connected - + 0 @@ -692,13 +692,13 @@ Connected - background-color:darkred; + background-color:darkgreen; QFrame::NoFrame - Rear Bogie + Rear Left Wheel Connected @@ -707,7 +707,7 @@ Connected - + 0 @@ -734,13 +734,13 @@ Connected - background-color:darkred; + background-color:darkgreen; QFrame::NoFrame - Left Bogie + Front Left Wheel Connected @@ -748,7 +748,7 @@ Connected - + @@ -782,7 +782,8 @@ Connected QFrame::NoFrame - ZED Connected + ZED +Disconnected Qt::AlignCenter @@ -815,6 +816,174 @@ Connected + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + background-color:darkgreen; + + + QFrame::NoFrame + + + Front Right Wheel +Connected + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + background-color:darkred; + + + QFrame::NoFrame + + + Battery Voltage +N/A + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + background-color:darkred; + + + QFrame::NoFrame + + + Middle Right Wheel +Disconnected + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + background-color:darkgreen; + + + QFrame::NoFrame + + + Rear Right Wheel +Connected + + + Qt::AlignCenter + + + diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index 7c23cb1..cf134c2 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -11,20 +11,84 @@ using namespace std; int device_address = 1; -int main(int argc, char** argv){ - smbus busHandle = smOpenBus("/dev/ttyUSB0"); +////////// Global Variables ////////// +// ROS Parameter Defaults +const string default_port = "/dev/ttyUSB0"; - if (busHandle >= 0) { - cout << "Successfully connected bus" << endl; -// deviceAddress=ui->deviceAddress->value(); - } else - cout << "Couldn't connect to bus"; - while(1){ - smSetParameter(busHandle, device_address, SMP_FAULTS, 0); - smSetParameter(busHandle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); - smSetParameter(busHandle, device_address, SMP_ABSOLUTE_SETPOINT, 1000); - cout << "Test" << endl; +// Axis Defaults +const smint32 axis1_max_count = 100000; +float axis1_max_degrees = 180.0; + +const smint32 axis2_max_count = 100000; + +const smint32 axis3_max_count = 100000; + +const smint32 axis4_max_count = 100000; + +const smint32 axis5_max_count = 320000; + +const smint32 axis6_max_count = 100000; + + +class RoverArm{ +public: + RoverArm(int argc, char** argv){ + ros::init(argc, argv, "rover_arm"); + + node_handle = new ros::NodeHandle("~"); + + node_handle->param("port", arm_port, default_port); + + + // Connect to arm, exit if failed + arm_bus_handle = smOpenBus(arm_port.c_str()); + + if(arm_bus_handle < 0){ + ROS_ERROR("Could not connect to arm"); + } } + void run(){ + char dir = 0; + + printf("OK?: %d", ros::ok()); + + while(ros::ok()){ + smSetParameter(arm_bus_handle, device_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smint32 status = 0; + while(!(status & STAT_TARGET_REACHED)){ + smRead1Parameter(arm_bus_handle, device_address, SMP_STATUS, &status); + } + + dir = !dir; + + if(dir){ + smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0); + }else{ + smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 330000); + } + + ros::spinOnce(); + } + + ROS_ERROR("Shutting down."); + } +private: + ros::NodeHandle *node_handle; + + string arm_port; + smbus arm_bus_handle; + + + +}; + + + +int main(int argc, char** argv){ + RoverArm rover_arm(argc, argv); + rover_arm.run(); } \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py index 89ea46b..96c99de 100755 --- a/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py +++ b/software/ros_packages/rover_control/src/control_coordinators/drive_coordinator.py @@ -91,10 +91,10 @@ class DriveCoordinator(object): sleep(max(self.wait_time - time_diff, 0)) def process_drive_commands(self): - if not self.drive_command_data["iris"]["message"].ignore_drive_control: - self.send_drive_control_command(self.drive_command_data["iris"]) - else: - self.send_drive_control_command(self.drive_command_data["ground_station"]) + # if not self.drive_command_data["iris"]["message"].ignore_drive_control: + # self.send_drive_control_command(self.drive_command_data["iris"]) + # else: + self.send_drive_control_command(self.drive_command_data["ground_station"]) def send_drive_control_command(self, drive_command_data): diff --git a/software/ros_packages/rover_control/src/drive_control/drive_control.py b/software/ros_packages/rover_control/src/drive_control/drive_control.py index 0da5859..6d45284 100755 --- a/software/ros_packages/rover_control/src/drive_control/drive_control.py +++ b/software/ros_packages/rover_control/src/drive_control/drive_control.py @@ -68,6 +68,11 @@ class DriveControl(object): self.port = rospy.get_param("~port", DEFAULT_PORT) self.baud = rospy.get_param("~baud", DEFAULT_BAUD) + print self.port + + self.first_motor_id = rospy.get_param("~first_motor_id", FIRST_MOTOR_ID) + self.second_motor_id = rospy.get_param("~second_motor_id", SECOND_MOTOR_ID) + self.first_motor_inverted = rospy.get_param("~invert_first_motor", DEFAULT_INVERT) self.second_motor_inverted = rospy.get_param("~invert_second_motor", DEFAULT_INVERT) @@ -117,6 +122,7 @@ class DriveControl(object): print "Error occurred:", error if (time() - self.bogie_last_seen) > BOGIE_LAST_SEEN_TIMEOUT: + print "Bogie not seen for", BOGIE_LAST_SEEN_TIMEOUT, "seconds. Exiting." return # Exit so respawn can take over time_diff = time() - start_time @@ -124,8 +130,8 @@ class DriveControl(object): sleep(max(self.wait_time - time_diff, 0)) def connect_to_bogie(self): - self.first_motor = minimalmodbus.Instrument(self.port, FIRST_MOTOR_ID) - self.second_motor = minimalmodbus.Instrument(self.port, SECOND_MOTOR_ID) + self.first_motor = minimalmodbus.Instrument(self.port, int(self.first_motor_id)) + self.second_motor = minimalmodbus.Instrument(self.port, int(self.second_motor_id)) self.__setup_minimalmodbus_for_485() def send_drive_control_message(self): diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index f5042ed..37dbc7f 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -1,18 +1,20 @@ - - - - + + + + - + + + - + @@ -20,9 +22,11 @@ - + + +