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 8bce37c..fe172ed 100644 --- a/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules +++ b/software/environment/rover/UDEV_rules/99-rover-usb-serial.rules @@ -29,6 +29,6 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6011", ATTRS{seria 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" +SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="DA01OM1N", SYMLINK+="rover/ttyScale" # 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/freq_testing/freq_testing.ino b/software/firmware/freq_testing/freq_testing.ino index ecef981..5fc0008 100644 --- a/software/firmware/freq_testing/freq_testing.ino +++ b/software/firmware/freq_testing/freq_testing.ino @@ -9,7 +9,7 @@ void setup(){ void loop(){ digitalWrite(pin,LOW); - delay(mills); + delay(mills / 2.0); digitalWrite(pin, HIGH); - delay(mills); + delay(mills / 2.0); } diff --git a/software/firmware/mining/mining.ino b/software/firmware/mining/mining.ino index a0c03f0..562721e 100644 --- a/software/firmware/mining/mining.ino +++ b/software/firmware/mining/mining.ino @@ -41,6 +41,13 @@ enum MOTORS { TILT = 1, }; +// void shoot(); +// void slowZoomIn(); +// void slowZoomOut(); +// void fullZoomIn(); +// void fullZoomOut(); +// void focus(); + enum MODBUS_REGISTERS { // Inputs SET_POSITION_LIFT_POSITIVE = 0, @@ -53,10 +60,24 @@ enum MODBUS_REGISTERS { TARE = 7, CALIBRATION_FACTOR = 8, + CHANGE_VIEW_MODE = 9, + ZOOM_IN = 10, + ZOOM_OUT = 11, + FULL_ZOOM_IN = 12, + FULL_ZOOM_OUT = 13, + FOCUS = 14, + SHOOT = 15, + // Outputs - CURRENT_POSITION_LIFT = 9, - CURRENT_POSITION_TILT = 10, - MEASURED_WEIGHT = 11 + CURRENT_POSITION_LIFT = 16, + CURRENT_POSITION_TILT = 17, + MEASURED_WEIGHT = 18 +}; + +enum CAMERA_VIEW_MODES { + NO_CHANGE = 0, + LCD = 1, + NETWORK = 2 }; ////////// Global Variables ////////// @@ -71,7 +92,7 @@ float last_calibration_factor = -120000; //for the scale // modbus stuff const uint8_t node_id = 2; const uint8_t mobus_serial_port_number = 2; -uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 972, 0, 0, 0}; +uint16_t modbus_data[] = {0, 0, 0, 0, 9999, 9999, 0, 0, 895, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; uint8_t num_modbus_registers = 0; int8_t poll_state = 0; bool communication_good = false; @@ -89,7 +110,7 @@ Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN); void setup() { Serial.begin(9600); // debug - // while(!Serial); + while(!Serial); setup_hardware(); num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); @@ -103,6 +124,7 @@ void loop() { set_motors(); set_scale(); poll_scale(); + apply_camera_commands(); } void setup_hardware() { @@ -139,7 +161,7 @@ void setup_hardware() { digitalWrite(HARDWARE::MOTOR_LIFT_EN, HIGH); digitalWrite(HARDWARE::MOTOR_TILT_EN, HIGH); - digitalWrite(HARDWARE::CAMERA_VIDEO_EN, HIGH); + digitalWrite(HARDWARE::CAMERA_VIDEO_EN, LOW); // Change motor PWM frequency so it's not in the audible range analogWriteFrequency(HARDWARE::MOTOR_LIFT_PWM, 25000); @@ -152,7 +174,7 @@ void setup_hardware() { current_position_tilt = analogRead(HARDWARE::MOTOR_TILT_FB); // setup scale - scale.set_scale(last_calibration_factor); + scale.set_scale(); scale.tare(); //Reset the scale to 0 } @@ -233,7 +255,7 @@ void set_motors() { } void set_scale(){ - float cal_factor = modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR] * -100; + float cal_factor = modbus_data[MODBUS_REGISTERS::CALIBRATION_FACTOR] * 100; if(cal_factor != last_calibration_factor){ scale.set_scale(cal_factor); last_calibration_factor = cal_factor; @@ -247,12 +269,57 @@ void set_scale(){ void poll_scale(){ if(modbus_data[MODBUS_REGISTERS::MEASURE] == 1){ - // Serial.println(scale.get_units()*-1000); + Serial.println(scale.get_units()*-1000); modbus_data[MODBUS_REGISTERS::MEASURED_WEIGHT] = scale.get_units()*-1000; modbus_data[MODBUS_REGISTERS::MEASURE] = 0; } } +void apply_camera_commands(){ + ///// Camea video output control ///// + switch (modbus_data[MODBUS_REGISTERS::CHANGE_VIEW_MODE]) { + case CAMERA_VIEW_MODES::LCD: + digitalWrite(HARDWARE::CAMERA_VIDEO_EN, HIGH); + break; + case CAMERA_VIEW_MODES::NETWORK: + digitalWrite(HARDWARE::CAMERA_VIDEO_EN, LOW); + break; + default: + break; + } + modbus_data[MODBUS_REGISTERS::CHANGE_VIEW_MODE] = CAMERA_VIEW_MODES::NO_CHANGE; + + ////// Zoom controls, process all, even if conflicting /////// + if(modbus_data[MODBUS_REGISTERS::ZOOM_IN]){ + Serial.println("ZOOM IN"); + modbus_data[MODBUS_REGISTERS::ZOOM_IN] = 0; + }else if(modbus_data[MODBUS_REGISTERS::ZOOM_OUT]){ + Serial.println("ZOOM OUT"); + modbus_data[MODBUS_REGISTERS::ZOOM_OUT] = 0; + }else if(modbus_data[MODBUS_REGISTERS::FULL_ZOOM_IN]){ + Serial.println("FULL ZOOM IN"); + modbus_data[MODBUS_REGISTERS::FULL_ZOOM_IN] = 0; + } + + if(modbus_data[MODBUS_REGISTERS::FULL_ZOOM_OUT]){ + Serial.println("FULL ZOOM OUT"); + modbus_data[MODBUS_REGISTERS::FULL_ZOOM_OUT] = 0; + } + + ////// Focus and shoot controls /////// + if(modbus_data[MODBUS_REGISTERS::FOCUS]){ + Serial.println("FOCUS"); + modbus_data[MODBUS_REGISTERS::FOCUS] = 0; + } + + if(modbus_data[MODBUS_REGISTERS::SHOOT]){ + Serial.println("SHOOT"); + modbus_data[MODBUS_REGISTERS::SHOOT] = 0; + } + ///// camera process here + +} + //---Set Motor Output---// /* Inputs: motor number, direction, pwm value diff --git a/software/firmware/scale_tester/scale_tester.ino b/software/firmware/scale_tester/scale_tester.ino new file mode 100644 index 0000000..d0db55e --- /dev/null +++ b/software/firmware/scale_tester/scale_tester.ino @@ -0,0 +1,67 @@ +/* + Example using the SparkFun HX711 breakout board with a scale + By: Nathan Seidle + SparkFun Electronics + Date: November 19th, 2014 + License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license). + + This is the calibration sketch. Use it to determine the calibration_factor that the main example uses. It also + outputs the zero_factor useful for projects that have a permanent mass on the scale in between power cycles. + + Setup your scale and start the sketch WITHOUT a weight on the scale + Once readings are displayed place the weight on the scale + Press +/- or a/z to adjust the calibration_factor until the output readings match the known weight + Use this calibration_factor on the example sketch + + This example assumes pounds (lbs). If you prefer kilograms, change the Serial.print(" lbs"); line to kg. The + calibration factor will be significantly different but it will be linearly related to lbs (1 lbs = 0.453592 kg). + + Your calibration factor may be very positive or very negative. It all depends on the setup of your scale system + and the direction the sensors deflect from zero state + This example code uses bogde's excellent library: https://github.com/bogde/HX711 + bogde's library is released under a GNU GENERAL PUBLIC LICENSE + Arduino pin 2 -> HX711 CLK + 3 -> DOUT + 5V -> VCC + GND -> GND + + Most any pin on the Arduino Uno will be compatible with DOUT/CLK. + + The HX711 board can be powered from 2.7V to 5V so the Arduino 5V power should be fine. + +*/ +#include "HX711.h" +#define DOUT 8 +#define CLK 7 +HX711 scale(DOUT, CLK); +float calibration_factor = 1314 00; //-7050 worked for my 440lb max scale setup +void setup() { + Serial.begin(9600); + Serial.println("HX711 calibration sketch"); + Serial.println("Remove all weight from scale"); + Serial.println("After readings begin, place known weight on scale"); + Serial.println("Press + or a to increase calibration factor"); + Serial.println("Press - or z to decrease calibration factor"); + scale.set_scale(); + scale.tare(); //Reset the scale to 0 + long zero_factor = scale.read_average(); //Get a baseline reading + Serial.print("Zero factor: "); //This can be used to remove the need to tare the scale. Useful in permanent scale projects. + Serial.println(zero_factor); +} +void loop() { + scale.set_scale(calibration_factor); //Adjust to this calibration factor + Serial.print("Reading: "); + Serial.print(scale.get_units(), 4); + Serial.print(" Kg"); //Change this to kg and re-adjust the calibration factor if you follow SI units like a sane person + Serial.print(" calibration_factor: "); + Serial.print(calibration_factor); + Serial.println(); + if(Serial.available()) + { + char temp = Serial.read(); + if(temp == '+' || temp == 'a') + calibration_factor += 100; + else if(temp == '-' || temp == 'z') + calibration_factor -= 100; + } +} diff --git a/software/ros_packages/__init__.py b/software/ros_packages/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py b/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py index 7b60e41..920e02d 100644 --- a/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py +++ b/software/ros_packages/ground_station/src/Framework/ArmSystems/ArmIndication.py @@ -63,6 +63,14 @@ FAULT_TO_STRING = { 15: "HOST COMM ERROR" } +GRIPPER_MODES = { + 0: "No Change", + 1: "Normal", + 2: "Pinch", + 3: "Wide ", + 4: "Scissor" +} + ##################################### # Controller Class Definition @@ -106,6 +114,7 @@ class ArmIndication(QtCore.QObject): thumb_current_updated__signal = QtCore.pyqtSignal(int) middlefinger_current_updated__signal = QtCore.pyqtSignal(int) + gripper_reported_mode_updated__signal = QtCore.pyqtSignal(str) gripper_reported_setpoint_updated__signal = QtCore.pyqtSignal(int) def __init__(self, shared_objects): @@ -155,6 +164,7 @@ class ArmIndication(QtCore.QObject): self.arm_controls_wrist_roll_status_label = self.right_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel self.arm_controls_wrist_roll_faults_label = self.right_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel + self.gripper_reported_mode_label = self.right_screen.gripper_reported_mode_label # type:QtWidgets.QLabel self.gripper_reported_setpoint_lcd_number = self.right_screen.gripper_reported_setpoint_lcd_number # type: QtWidgets.QLCDNumber # ########## Get the settings instance ########## @@ -209,6 +219,7 @@ class ArmIndication(QtCore.QObject): self.thumb_current_updated__signal.connect(self.thumb_current_lcd_number.display) self.middlefinger_current_updated__signal.connect(self.middlefinger_current_lcd_number.display) + self.gripper_reported_mode_updated__signal.connect(self.gripper_reported_mode_label.setText) self.gripper_reported_setpoint_updated__signal.connect(self.gripper_reported_setpoint_lcd_number.display) def on_arm_status_update_received__callback(self, data): @@ -254,6 +265,7 @@ class ArmIndication(QtCore.QObject): self.thumb_current_updated__signal.emit(data.thumb_current) self.middlefinger_current_updated__signal.emit(data.middlefinger_current) + self.gripper_reported_mode_updated__signal.emit(GRIPPER_MODES[data.current_mode]) self.gripper_reported_setpoint_updated__signal.emit(data.current_finger_position) @staticmethod diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py index d88f0b1..bdc19c3 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py +++ b/software/ros_packages/ground_station/src/Framework/InputSystems/XBOXControllerControlSender.py @@ -226,7 +226,7 @@ class XBOXControllerControlSender(QtCore.QThread): self.last_right_bumper_state = 0 self.last_back_button_state = 0 - self.gripper_control_mode = 0 + self.gripper_control_mode = 1 self.gripper_control_mode_just_changed = False self.send_new_gripper_mode = False @@ -257,7 +257,7 @@ class XBOXControllerControlSender(QtCore.QThread): "UNSET": [self.gripper_mode_pinch_stylesheet_update_ready__signal, self.gripper_mode_wide_stylesheet_update_ready__signal, self.gripper_mode_normal_stylesheet_update_ready__signal], - "ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION + # "ABS_MOVE": self.PINCH_MODE_ABSOLUTE_SET_POSITION } } @@ -314,15 +314,14 @@ class XBOXControllerControlSender(QtCore.QThread): if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"): if self.last_left_bumper_state == 0 and left_bumper_state == 1: - self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES)) - self.gripper_control_mode_just_changed = True + # self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES)) + # self.gripper_control_mode_just_changed = True self.last_left_bumper_state = 1 elif self.last_left_bumper_state == 1 and left_bumper_state == 0: self.last_left_bumper_state = 0 - if self.last_right_bumper_state == 0 and right_bumper_state == 1: - self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES)) - self.gripper_control_mode_just_changed = True + # self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES)) + # self.gripper_control_mode_just_changed = True self.last_right_bumper_state = 1 elif self.last_right_bumper_state == 1 and right_bumper_state == 0: self.last_right_bumper_state = 0 @@ -390,7 +389,7 @@ class XBOXControllerControlSender(QtCore.QThread): should_publish_arm = True should_publish_gripper = True - arm_control_message.wrist_roll = ((right_x_axis / THUMB_STICK_MAX) * WRIST_ROLL_SCALAR) * right_trigger_ratio + arm_control_message.wrist_roll = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * right_trigger_ratio arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio gripper_control_message.gripper_position_relative = (-(right_y_axis / THUMB_STICK_MAX) * GRIPPER_MOVEMENT_SCALAR) * right_trigger_ratio @@ -408,7 +407,7 @@ class XBOXControllerControlSender(QtCore.QThread): if self.last_back_button_state == 0 and back_state == 1: gripper_control_message.should_home = True - gripper_control_message.gripper_mode = 1 + gripper_control_message.gripper_mode = self.gripper_control_mode + 1 self.gripper_control_publisher.publish(gripper_control_message) self.last_back_button_state = 1 elif self.last_back_button_state == 1 and back_state == 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 77a21ed..0f13075 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiningCore.py @@ -7,8 +7,9 @@ from PyQt5 import QtCore, QtWidgets import logging import rospy -from rover_control.msg import MiningStatusMessage, MiningControlMessage +from rover_control.msg import MiningStatusMessage, MiningControlMessage, CameraControlMessage from rover_science.msg import SoilSensorStatusMessage +from std_msgs.msg import Float64 ##################################### # Global Variables @@ -16,8 +17,12 @@ from rover_science.msg import SoilSensorStatusMessage MINING_STATUS_TOPIC = "/rover_control/mining/status" MINING_CONTROL_TOPIC = "/rover_control/mining/control" +SCALE_MEASUREMENT_TOPIC = "/rover_control/scale/measurement" + SOIL_PROBE_TOPIC = "/rover_science/soil_probe/data" +CAMERA_CONTROL_TOPIC = "/rover_control/camera/control" + TRAVEL_POSITION_LIFT = 110 TRAVEL_POSITION_TILT = 1023 @@ -71,6 +76,17 @@ class Mining(QtCore.QObject): self.science_real_dielectric_lcd_number = self.left_screen.science_real_dielectric_lcd_number # type:QtWidgets.QLCDNumber self.science_imaginary_dielectric_lcd_number = self.left_screen.science_imaginary_dielectric_lcd_number # type:QtWidgets.QLCDNumber + self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton + self.cam_network_output_button = self.left_screen.cam_network_output_button # type:QtWidgets.QPushButton + + self.cam_zoom_in_button = self.left_screen.cam_zoom_in_button # type:QtWidgets.QPushButton + self.cam_zoom_out_button = self.left_screen.cam_zoom_out_button # type:QtWidgets.QPushButton + self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton + self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton + + self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton + self.cam_lcd_output_button = self.left_screen.cam_lcd_output_button # type:QtWidgets.QPushButton + # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -85,8 +101,10 @@ class Mining(QtCore.QObject): self.mining_status_message_received__callback) self.soil_probe_subscriber = rospy.Subscriber(SOIL_PROBE_TOPIC, SoilSensorStatusMessage, self.on_soil_probe_message_received__callback) + self.scale_measurement_subscriber = rospy.Subscriber(SCALE_MEASUREMENT_TOPIC, Float64, self.on_scale_measurement_received__callback) self.mining_control_publisher = rospy.Publisher(MINING_CONTROL_TOPIC, MiningControlMessage, queue_size=1) + self.camera_control_publisher = rospy.Publisher(CAMERA_CONTROL_TOPIC, CameraControlMessage, queue_size=1) self.connect_signals_and_slots() @@ -111,6 +129,9 @@ class Mining(QtCore.QObject): self.real_dielectric_update_ready__signal.connect(self.science_real_dielectric_lcd_number.display) self.imaginary_dielectric_update_ready__signal.connect(self.science_imaginary_dielectric_lcd_number.display) + self.cam_lcd_output_button.clicked.connect(self.on_cam_lcd_button_clicked__slot) + self.cam_network_output_button.clicked.connect(self.on_cam_network_button_clicked__slot) + def on_mining_set_cal_factor_clicked__slot(self): message = MiningControlMessage() @@ -162,11 +183,21 @@ class Mining(QtCore.QObject): self.mining_control_publisher.publish(message) + def on_cam_lcd_button_clicked__slot(self): + message = CameraControlMessage() + message.camera_mode = 1 + self.camera_control_publisher.publish(message) + + def on_cam_network_button_clicked__slot(self): + message = CameraControlMessage() + message.camera_mode = 2 + self.camera_control_publisher.publish(message) + def mining_status_message_received__callback(self, status): 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.weight_measurement_update_ready__signal.emit(status.measured_weight) + # self.weight_measurement_update_ready__signal.emit(status.measured_weight) def on_soil_probe_message_received__callback(self, data): self.temp_update_ready__signal.emit(data.temp_c) @@ -175,3 +206,6 @@ class Mining(QtCore.QObject): self.electrical_conductivity_update_ready__signal.emit(data.soil_electrical_conductivity) self.real_dielectric_update_ready__signal.emit(data.real_dielectric_permittivity) self.imaginary_dielectric_update_ready__signal.emit(data.imaginary_dielectric_permittivity) + + def on_scale_measurement_received__callback(self, data): + self.weight_measurement_update_ready__signal.emit(data.data * 1000) \ No newline at end of file diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py index b6e36f1..802337e 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py @@ -26,7 +26,7 @@ POSITIONAL_TOLERANCE = 0.02 # Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll] ARM_STOW_PROCEDURE = [ - [0.0, -0.035, -0.28, 0.0, 0.0, 0.0], + [0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover [0.0, -0.035, -0.28, -0.25, 0.25, 0.0], [0.0, -0.035, -0.5, -0.25, 0.25, 0.0], [0.0, -0.25, -0.5, -0.25, 0.25, -0.25] @@ -39,6 +39,18 @@ ARM_UNSTOW_PROCEDURE = [ [0.0, -0.035, -0.28, 0.0, 0.0, 0.0] ] +APPROACH_O2 = [ + [0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover + [0.0, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0], # Correct positioning out to left side of rover + [-0.436250296246, 0.0485472094896, -0.273685193022, -0.00102834607876, -0.17200773156, 0] # Directly positioned, ready to grip +] + +APPROACH_BEACON = [ + [0.0, -0.035, -0.28, 0.0, 0.0, 0.0], # Out in front of rover + [0.0, 0.0361371382336, -0.325145836608, -0.00731261537597, -0.129662333807, 0.0569339269566], # Correct positioning out to right side of rover + [0.458505849045, 0.0361371382336, -0.325145633259, -0.00731200471916, -0.131140162948, 0.0920117742056] +] + ##################################### # UbiquitiRadioSettings Class Definition @@ -59,6 +71,11 @@ class MiscArm(QtCore.QThread): self.arm_controls_clear_faults_button = self.left_screen.arm_controls_clear_faults_button # type:QtWidgets.QPushButton self.arm_controls_reset_motor_drivers_button = self.left_screen.arm_controls_reset_motor_drivers_button # type:QtWidgets.QPushButton + self.arm_controls_approach_o2_button = self.left_screen.arm_controls_approach_o2_button # type:QtWidgets.QPushButton + self.arm_controls_depart_o2_button = self.left_screen.arm_controls_depart_o2_button # type:QtWidgets.QPushButton + self.arm_controls_approach_beacon_button = self.left_screen.arm_controls_approach_beacon_button # type:QtWidgets.QPushButton + self.arm_controls_depart_beacon_button = self.left_screen.arm_controls_depart_beacon_button # type:QtWidgets.QPushButton + self.gripper_home_button = self.left_screen.gripper_home_button # type:QtWidgets.QPushButton self.gripper_toggle_light_button = self.left_screen.gripper_toggle_light_button # type:QtWidgets.QPushButton @@ -94,6 +111,11 @@ class MiscArm(QtCore.QThread): self.should_stow_arm = False self.should_unstow_arm = False + self.should_approach_o2 = False + self.should_depart_o2 = False + self.should_approach_beacon = False + self.should_depart_beacon = False + def run(self): self.logger.debug("Starting MiscArm Thread") @@ -107,6 +129,22 @@ class MiscArm(QtCore.QThread): self.unstow_rover_arm() self.should_unstow_arm = False + elif self.should_approach_o2: + self.approach_o2() + self.should_approach_o2 = False + + elif self.should_depart_o2: + self.depart_o2() + self.should_depart_o2 = False + + elif self.should_approach_beacon: + self.approach_beacon() + self.should_approach_beacon = False + + elif self.should_depart_beacon: + self.depart_beacon() + self.should_depart_beacon = False + time_diff = time() - start_time self.msleep(max(int(self.wait_time - time_diff), 0)) @@ -121,6 +159,22 @@ class MiscArm(QtCore.QThread): for movement in ARM_UNSTOW_PROCEDURE: self.process_absolute_move_command(movement) + def approach_o2(self): + for movement in APPROACH_O2: + self.process_absolute_move_command(movement) + + def depart_o2(self): + for movement in reversed(APPROACH_O2): + self.process_absolute_move_command(movement) + + def approach_beacon(self): + for movement in APPROACH_BEACON: + self.process_absolute_move_command(movement) + + def depart_beacon(self): + for movement in reversed(APPROACH_BEACON): + self.process_absolute_move_command(movement) + def process_absolute_move_command(self, movement): message = ArmControlMessage() @@ -134,6 +188,7 @@ class MiscArm(QtCore.QThread): self.arm_absolute_control_publisher.publish(message) self.wait_for_targets_reached(movement) + self.msleep(250) def wait_for_targets_reached(self, movement): base_set = movement[0] @@ -176,6 +231,9 @@ class MiscArm(QtCore.QThread): self.arm_controls_clear_faults_button.clicked.connect(self.on_clear_faults_button_pressed__slot) self.arm_controls_reset_motor_drivers_button.clicked.connect(self.on_reset_drivers_button_pressed__slot) + self.arm_controls_approach_o2_button.clicked.connect(self.on_approach_o2_button_pressed__slot) + self.arm_controls_approach_beacon_button.clicked.connect(self.on_approach_beacon_button_pressed__slot) + self.gripper_home_button.clicked.connect(self.on_gripper_home_pressed) self.gripper_toggle_light_button.clicked.connect(self.on_gripper_toggle_light_pressed) @@ -203,9 +261,21 @@ class MiscArm(QtCore.QThread): def on_unstow_arm_button_pressed__slot(self): self.should_unstow_arm = True + def on_approach_o2_button_pressed__slot(self): + self.should_approach_o2 = True + + def on_depart_o2_button_pressed__slot(self): + self.should_depart_o2 = True + + def on_approach_beacon_button_pressed__slot(self): + self.should_approach_beacon = True + + def on_depart_beacon_button_pressed__slot(self): + self.should_depart_beacon = True + def on_gripper_home_pressed(self): message = GripperControlMessage() - message.gripper_mode = 1 + message.gripper_mode = 2 message.gripper_position_absolute = -1 message.should_home = True @@ -213,7 +283,7 @@ class MiscArm(QtCore.QThread): def on_gripper_toggle_light_pressed(self): message = GripperControlMessage() - message.gripper_mode = 1 + message.gripper_mode = 2 message.toggle_light = True message.gripper_position_absolute = -1 diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py index 2cf19bf..b4541ad 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py +++ b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py @@ -98,7 +98,7 @@ class RDF(QtCore.QThread): valid_range = [] for n in range(0, len(xf)): - if (xf[n] > 0.5) and (xf[n] < 5.0): + if (xf[n] > 0.5) and (xf[n] <= 5.0): valid_range.append(n) yf = numpy.take(yf, valid_range) diff --git a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py index 3f186ad..f0e2334 100644 --- a/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py +++ b/software/ros_packages/ground_station/src/Framework/NavigationSystems/SpeedAndHeadingIndication.py @@ -34,6 +34,9 @@ class SpeedAndHeadingIndication(QtCore.QThread): heading_text_update_ready__signal = QtCore.pyqtSignal(str) new_speed_update_ready__signal = QtCore.pyqtSignal(str) + pitch_update_ready__signal = QtCore.pyqtSignal(float) + roll_update_ready__signal = QtCore.pyqtSignal(float) + def __init__(self, shared_objects): super(SpeedAndHeadingIndication, self).__init__() @@ -46,6 +49,9 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.next_goal_label = self.right_screen.next_goal_label # type: QtWidgets.QLabel self.current_speed_label = self.right_screen.current_speed_label # type: QtWidgets.QLabel + self.imu_pitch_lcd_number = self.right_screen.imu_pitch_lcd_number # type: QtWidgets.QLCDNumber + self.imu_roll_lcd_number = self.right_screen.imu_roll_lcd_number # type: QtWidgets.QLCDNumber + # ########## Get the settings instance ########## self.settings = QtCore.QSettings() @@ -110,6 +116,9 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.roll, self.pitch, self.yaw = transformations.euler_from_quaternion(quat) self.current_heading = (self.euler_interpolator(self.yaw) + MappingSettings.DECLINATION_OFFSET) % 360 + self.pitch_update_ready__signal.emit(self.pitch) + self.roll_update_ready__signal.emit(self.roll) + def rotate_compass_if_needed(self): self.current_heading_shown_rotation_angle = int(self.current_heading) @@ -161,6 +170,9 @@ class SpeedAndHeadingIndication(QtCore.QThread): self.heading_compass_label.mousePressEvent = self.__on_heading_clicked__slot + self.pitch_update_ready__signal.connect(self.imu_pitch_lcd_number.display) + self.roll_update_ready__signal.connect(self.imu_roll_lcd_number.display) + 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) diff --git a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py index fe347dd..38564f9 100644 --- a/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py +++ b/software/ros_packages/ground_station/src/Framework/VideoSystems/RoverVideoCoordinator.py @@ -381,7 +381,7 @@ class RoverVideoCoordinator(QtCore.QThread): self.low_res_button_text_update_ready__signal.emit("DISABLED") self.low_res_button_stylesheet_update_ready__signal.emit(COLOR_GREEN) else: - self.in_low_res_mode = False + self.in_low_res_mode = True self.set_max_resolutions_flag = True self.low_res_button_text_update_ready__signal.emit("ENABLED") diff --git a/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py b/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py index 3c4bd93..b0eba71 100644 --- a/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py +++ b/software/ros_packages/ground_station/src/Resources/Settings/MappingSettings.py @@ -63,4 +63,4 @@ LAST_SELECTION = "Graf Hall" LAST_ZOOM_LEVEL = MAPPING_LOCATIONS[LAST_SELECTION]["default_zoom"] # ##### This is the offset from magnetic north to true north -DECLINATION_OFFSET = 15 +DECLINATION_OFFSET = 0 # We set this to 0 so we can use a phone to calibrate 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 c99d702..e6d9ec6 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 @@ -251,6 +251,48 @@ 0 + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 9999999 + 9999999 + + + + + 10 + 75 + true + + + + background-color:darkgreen; border: 0.5px solid black; + + + QFrame::NoFrame + + + CO2 Levels +--- ppm + + + Qt::AlignCenter + + + @@ -1322,48 +1364,6 @@ N/A - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 9999999 - 9999999 - - - - - 10 - 75 - true - - - - background-color:darkgreen; border: 0.5px solid black; - - - QFrame::NoFrame - - - CO2 Levels ---- ppm - - - Qt::AlignCenter - - - @@ -1390,8 +1390,145 @@ N/A - 1 + 0 + + + Science Cam + + + + + + + + + 75 + true + + + + Video Output + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + LCD + + + + + + + Network + + + + + + + + + + + Qt::Horizontal + + + + 422 + 20 + + + + + + + + + + + 75 + true + + + + Controls + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + + Zoom In + + + + + + + Zoom Out + + + + + + + Full Zoom In + + + + + + + Full Zoom Out + + + + + + + Focus + + + + + + + Shoot + + + + + + + + + + + Qt::Vertical + + + + 20 + 331 + + + + + + RDF @@ -1575,7 +1712,7 @@ N/A - Preset Movements + Special Movements @@ -1702,6 +1839,83 @@ N/A + + + + + + + 75 + true + + + + Task Movements + + + + + + + + + + + + Approach O2 Tank + + + + + + + Qt::Vertical + + + QSizePolicy::MinimumExpanding + + + + 20 + 40 + + + + + + + + + + + Depart O2 Tank + + + + + + + + + + Approach Beacon + + + + + + + + + + Depart Beacon + + + + + + + diff --git a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui index 8c4ece1..d166be0 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui @@ -1604,7 +1604,7 @@ Position - background-color:darkgreen; border: 1px solid black; + border: 1px solid black; Normal @@ -1634,7 +1634,7 @@ Position - border: 1px solid black; + border: 1px solid black; background-color:darkgreen; Pinch @@ -1810,6 +1810,49 @@ Position + + + + 6 + + + + + + 10 + 75 + true + + + + Reported Mode + + + Qt::AlignCenter + + + + + + + + 0 + 50 + + + + border: 1px solid grey; + + + ------- + + + Qt::AlignCenter + + + + + @@ -1824,6 +1867,9 @@ Position Reported Setpoint + + Qt::AlignCenter + @@ -2511,16 +2557,6 @@ Position 0 - - - - -100 - - - 100 - - - @@ -2537,6 +2573,16 @@ Position + + + + + 0 + 50 + + + + @@ -2545,23 +2591,7 @@ Position 0 - - - -100 - - - 100 - - - false - - - true - - - - - + 75 @@ -2576,6 +2606,16 @@ Position + + + + + 0 + 50 + + + + diff --git a/software/ros_packages/rover_control/CMakeLists.txt b/software/ros_packages/rover_control/CMakeLists.txt index bbaa0c0..2c1a9ed 100644 --- a/software/ros_packages/rover_control/CMakeLists.txt +++ b/software/ros_packages/rover_control/CMakeLists.txt @@ -59,6 +59,7 @@ find_package(catkin REQUIRED COMPONENTS MiningStatusMessage.msg GripperControlMessage.msg GripperStatusMessage.msg + CameraControlMessage.msg ) ## Generate services in the 'srv' folder diff --git a/software/ros_packages/rover_control/__init__.py b/software/ros_packages/rover_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/rover_control/msg/CameraControlMessage.msg b/software/ros_packages/rover_control/msg/CameraControlMessage.msg new file mode 100644 index 0000000..ad45e94 --- /dev/null +++ b/software/ros_packages/rover_control/msg/CameraControlMessage.msg @@ -0,0 +1,8 @@ +uint8 camera_mode + +bool zoom_in +bool zoom_out +bool full_zoom_in +bool full_zoom_out +bool focus +bool shoot \ No newline at end of file diff --git a/software/ros_packages/rover_control/src/__init__.py b/software/ros_packages/rover_control/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/rover_control/src/effectors_control/__init__.py b/software/ros_packages/rover_control/src/effectors_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py index d40bb47..f7467df 100755 --- a/software/ros_packages/rover_control/src/effectors_control/effectors_control.py +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_control.py @@ -12,7 +12,7 @@ import minimalmodbus # from std_msgs.msg import UInt8, UInt16 # Custom Imports -from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage +from rover_control.msg import MiningControlMessage, MiningStatusMessage, GripperControlMessage, GripperStatusMessage, CameraControlMessage ##################################### # Global Variables @@ -45,6 +45,8 @@ GRIPPER_STATUS_PUBLISHER_TOPIC = "gripper/status" MINING_CONTROL_SUBSCRIBER_TOPIC = "mining/control" MINING_STATUS_PUBLISHER_TOPIC = "mining/status" +CAMERA_CONTROL_SUBSCRIBER_TOPIC = "camera/control" + # ##### Gripper Defines ##### GRIPPER_MODBUS_REGISTERS = { "MODE": 0, @@ -94,9 +96,17 @@ MINING_MODBUS_REGISTERS = { "TARE": 7, "CAL_FACTOR": 8, - "LIFT_POSITION": 9, - "TILT_POSITION": 10, - "MEASURED_WEIGHT": 11 + "CHANGE_VIEW_MODE": 9, + "ZOOM_IN": 10, + "ZOOM_OUT": 11, + "FULL_ZOOM_IN": 12, + "FULL_ZOOM_OUT": 13, + "FOCUS": 14, + "SHOOT": 15, + + "CURRENT_POSITION_LIFT": 16, + "CURRENT_POSITION_TILT": 17, + "MEASURED_WEIGHT": 18 } @@ -139,6 +149,9 @@ class EffectorsControl(object): self.mining_status_publisher_topic = rospy.get_param("~mining_status_publisher_topic", MINING_STATUS_PUBLISHER_TOPIC) + self.camera_control_subscriber_topic = rospy.get_param("~camera_control_subscriber_topic", + CAMERA_CONTROL_SUBSCRIBER_TOPIC) + self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ) self.gripper_node = None # type:minimalmodbus.Instrument @@ -153,14 +166,14 @@ class EffectorsControl(object): # ##### Subscribers ##### self.gripper_control_subscriber = rospy.Subscriber(self.gripper_control_subscriber_topic, GripperControlMessage, self.gripper_control_message_received__callback) - self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage, - self.mining_control_message_received__callback) + self.mining_control_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage, self.mining_control_message_received__callback) + + self.camera_control_subscriber = rospy.Subscriber(self.camera_control_subscriber_topic, CameraControlMessage, self.camera_control_message_received__callback) # ##### Publishers ##### self.gripper_status_publisher = rospy.Publisher(self.gripper_status_publisher_topic, GripperStatusMessage, queue_size=1) - self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, - queue_size=1) + self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1) # ##### Misc ##### self.modbus_nodes_seen_time = time() @@ -175,6 +188,9 @@ class EffectorsControl(object): self.gripper_control_message = None self.new_gripper_control_message = False + self.camera_control_message = None # type: CameraControlMessage + self.new_camera_control_message = False + self.failed_gripper_modbus_count = 0 self.failed_mining_modbus_count = 0 @@ -210,7 +226,8 @@ class EffectorsControl(object): try: self.run_mining() self.failed_mining_modbus_count = 0 - except: + except Exception, e: + print e self.failed_mining_modbus_count += 1 if self.failed_mining_modbus_count == FAILED_MINING_MODBUS_LIMIMT: @@ -224,6 +241,7 @@ class EffectorsControl(object): def run_mining(self): self.process_mining_control_message() self.send_mining_status_message() + self.process_camera_control_message() def connect_to_nodes(self): self.gripper_node = minimalmodbus.Instrument(self.port, int(self.gripper_node_id)) @@ -268,13 +286,39 @@ class EffectorsControl(object): self.modbus_nodes_seen_time = time() self.new_mining_control_message = False + def process_camera_control_message(self): + if self.new_camera_control_message: + print self.camera_control_message + self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_ABSOLUTE"]] = 1024 + self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_POSITIVE"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_SET_NEGATIVE"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_ABSOLUTE"]] = 1024 + self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_POSITIVE"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["TILT_SET_NEGATIVE"]] = 0 + + self.mining_registers[MINING_MODBUS_REGISTERS["MEASURE"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["TARE"]] = 0 + + self.mining_registers[MINING_MODBUS_REGISTERS["CHANGE_VIEW_MODE"]] = self.camera_control_message.camera_mode + self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_IN"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["ZOOM_OUT"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_IN"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["FULL_ZOOM_OUT"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["FOCUS"]] = 0 + self.mining_registers[MINING_MODBUS_REGISTERS["SHOOT"]] = 0 + + self.mining_node.write_registers(0, self.mining_registers) + self.modbus_nodes_seen_time = time() + + self.new_camera_control_message = False + def send_mining_status_message(self): if self.mining_node_present: self.mining_registers = self.mining_node.read_registers(0, len(MINING_MODBUS_REGISTERS)) message = MiningStatusMessage() - message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["LIFT_POSITION"]] - message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]] + message.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_LIFT"]] + message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_TILT"]] message.measured_weight = self.mining_registers[MINING_MODBUS_REGISTERS["MEASURED_WEIGHT"]] self.mining_status_publisher.publish(message) @@ -296,16 +340,17 @@ class EffectorsControl(object): homing_complete = False while not homing_complete: - registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS)) - # print registers + self.gripper_registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS)) + self.send_gripper_status_message() - if registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0: + if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["FINGER_POSITION_OUTPUT"]] == 0: homing_complete = True self.gripper_registers = None else: if self.gripper_control_message.toggle_light: - self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = not self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] + self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] = 0 if self.gripper_registers[GRIPPER_MODBUS_REGISTERS["LIGHT_STATE"]] else 1 + self.gripper_control_message.toggle_light = False gripper_absolute = self.gripper_control_message.gripper_position_absolute gripper_relative = self.gripper_control_message.gripper_position_relative @@ -320,7 +365,8 @@ class EffectorsControl(object): self.gripper_node.write_registers(0, DEFAULT_GRIPPER_REGISTERS) - self.new_gripper_control_message = False + self.gripper_control_message = None + self.new_gripper_control_message = False def send_gripper_status_message(self): registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS)) @@ -348,6 +394,10 @@ class EffectorsControl(object): self.mining_control_message = control_message self.new_mining_control_message = True + def camera_control_message_received__callback(self, control_message): + self.camera_control_message = control_message + self.new_camera_control_message = True + if __name__ == "__main__": EffectorsControl() diff --git a/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py index 0a0fedb..2d066da 100755 --- a/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py +++ b/software/ros_packages/rover_control/src/effectors_control/effectors_tester.py @@ -2,20 +2,18 @@ import rospy import time -from rover_control.msg import GripperControlMessage +from rover_control.msg import CameraControlMessage -TOPIC = "/gripper/control" +TOPIC = "/rover_control/camera/control" rospy.init_node("effectors_tester") -publisher = rospy.Publisher(TOPIC, GripperControlMessage, queue_size=1) +publisher = rospy.Publisher(TOPIC, CameraControlMessage, queue_size=1) time.sleep(2) -message = GripperControlMessage() -message.gripper_mode = 1 -message.gripper_position = 0 -message.should_home = 0 +message = CameraControlMessage() +message.camera_mode = 2 publisher.publish(message) diff --git a/software/ros_packages/rover_control/src/effectors_control/emergency_scale.py b/software/ros_packages/rover_control/src/effectors_control/emergency_scale.py new file mode 100644 index 0000000..13ae3fb --- /dev/null +++ b/software/ros_packages/rover_control/src/effectors_control/emergency_scale.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python +##################################### +# Imports +##################################### +# Python native imports +import rospy +from time import time, sleep +import serial + +from std_msgs.msg import Float64 + +##################################### +# Global Variables +##################################### +NODE_NAME = "effectors_control" + +# ##### Communication Defines ##### +DEFAULT_PORT = "/dev/rover/ttyScale" +# DEFAULT_PORT = "/dev/ttyUSB3" +DEFAULT_BAUD = 115200 + +DEFAULT_TOPIC = "scale/measurement" + +CAL_FACTOR = 89500 + + +##################################### +# DriveControl Class Definition +##################################### +class EmergencyScale(object): + def __init__(self): + rospy.init_node(NODE_NAME) + + self.scale_serial = serial.Serial(port=DEFAULT_PORT, baudrate=DEFAULT_BAUD, timeout=200) + + self.publisher = rospy.Publisher(DEFAULT_TOPIC, Float64, queue_size=1) + + self.run() + + def run(self): + sleep(.2) + self.scale_serial.write(str(CAL_FACTOR)) + sleep(.2) + + while not rospy.is_shutdown(): + value = self.scale_serial.readline() + + try: + message = Float64() + message.data = float(value) + self.publisher.publish(message) + print + except: + print "No data" + + +if __name__ == "__main__": + EmergencyScale() diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch index a0c5c45..fce59b3 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_senders.launch @@ -24,7 +24,8 @@ {name: "/cameras/main_navigation/camera_control", compress: false, rate: 5.0}, {name: "/cameras/end_effector/camera_control", compress: false, rate: 5.0}, {name: "/rover_status/update_requested", compress: false, rate: 5.0}, - {name: "/rover_arm/control/absolute", compress: true, rate: 5.0}] + {name: "/rover_arm/control/absolute", compress: true, rate: 5.0}, + {name: "/rover_control/camera/control", compress: true, rate: 5.0}] diff --git a/software/ros_packages/rover_main/launch/rover/control.launch b/software/ros_packages/rover_main/launch/rover/control.launch index cf79418..e31275c 100644 --- a/software/ros_packages/rover_main/launch/rover/control.launch +++ b/software/ros_packages/rover_main/launch/rover/control.launch @@ -35,8 +35,11 @@ - + - + + + + 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 8016fe8..c8a4d40 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 @@ -158,7 +158,7 @@ {name: "/rover_control/mining/status", compress: false, rate: 5.0}, {name: "/rover_control/gripper/status", compress: false, rate: 5.0}, {name: "/rover_science/rdf/data", compress: false, rate: 50.0}, - {name: "/rover_science/soil_probe/data", compress: false, rate: 50.0} + {name: "/rover_control/scale/measurement", compress: false, rate: 20.0} ] diff --git a/software/ros_packages/rover_status/src/system_statuses_node.py b/software/ros_packages/rover_status/src/system_statuses_node.py index a562c57..b6a1879 100755 --- a/software/ros_packages/rover_status/src/system_statuses_node.py +++ b/software/ros_packages/rover_status/src/system_statuses_node.py @@ -143,7 +143,11 @@ class SystemStatuses: self.GPS_msg.num_satellites = int(self.Nmea_Message.num_sats) self.GPS_msg.horizontal_dilution = float(self.Nmea_Message.horizontal_dil) if self.Nmea_Message.sentence_type == 'VTG': - self.GPS_msg.kmph = float(self.Nmea_Message.spd_over_grnd_kmph) + try: + self.GPS_msg.kmph = float(self.Nmea_Message.spd_over_grnd_kmph) + except: + pass + if self.Nmea_Message.true_track is not None: self.GPS_msg.gps_heading = float(self.Nmea_Message.true_track) else: