Got point and shoot control working. Needs zooming. Backup scale node added. Lots of random things, super tired.

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"

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);
 }

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_LIFT = 16,
-    CURRENT_POSITION_TILT = 10,
+    CURRENT_POSITION_TILT = 17,
-    MEASURED_WEIGHT = 11
+    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

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;
+  }
+}

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

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 = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
-                self.gripper_control_mode_just_changed = True
+                # 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 = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
-                self.gripper_control_mode_just_changed = True
+                # 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:

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)

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
 

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)

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)

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")

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

software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui

@@ -251,6 +251,48 @@
            <property name="spacing">
             <number>0</number>
            </property>
+           <item row="10" column="1">
+            <widget class="QLabel" name="co2_levels_label">
+             <property name="sizePolicy">
+              <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+               <horstretch>0</horstretch>
+               <verstretch>0</verstretch>
+              </sizepolicy>
+             </property>
+             <property name="minimumSize">
+              <size>
+               <width>0</width>
+               <height>0</height>
+              </size>
+             </property>
+             <property name="maximumSize">
+              <size>
+               <width>9999999</width>
+               <height>9999999</height>
+              </size>
+             </property>
+             <property name="font">
+              <font>
+               <pointsize>10</pointsize>
+               <weight>75</weight>
+               <bold>true</bold>
+              </font>
+             </property>
+             <property name="styleSheet">
+              <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
+             </property>
+             <property name="frameShape">
+              <enum>QFrame::NoFrame</enum>
+             </property>
+             <property name="text">
+              <string>CO2 Levels
+--- ppm</string>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
            <item row="5" column="3">
             <widget class="QLabel" name="emmc">
              <property name="font">
@@ -1322,48 +1364,6 @@ N/A</string>
              </property>
             </widget>
            </item>
-           <item row="10" column="1">
-            <widget class="QLabel" name="co2_levels_label">
-             <property name="sizePolicy">
-              <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-               <horstretch>0</horstretch>
-               <verstretch>0</verstretch>
-              </sizepolicy>
-             </property>
-             <property name="minimumSize">
-              <size>
-               <width>0</width>
-               <height>0</height>
-              </size>
-             </property>
-             <property name="maximumSize">
-              <size>
-               <width>9999999</width>
-               <height>9999999</height>
-              </size>
-             </property>
-             <property name="font">
-              <font>
-               <pointsize>10</pointsize>
-               <weight>75</weight>
-               <bold>true</bold>
-              </font>
-             </property>
-             <property name="styleSheet">
-              <string notr="true">background-color:darkgreen; border: 0.5px solid black;</string>
-             </property>
-             <property name="frameShape">
-              <enum>QFrame::NoFrame</enum>
-             </property>
-             <property name="text">
-              <string>CO2 Levels
---- ppm</string>
-             </property>
-             <property name="alignment">
-              <set>Qt::AlignCenter</set>
-             </property>
-            </widget>
-           </item>
           </layout>
          </item>
         </layout>
@@ -1390,8 +1390,145 @@ N/A</string>
          </size>
         </property>
         <property name="currentIndex">
-         <number>1</number>
+         <number>0</number>
         </property>
+        <widget class="QWidget" name="tab_6">
+         <attribute name="title">
+          <string>Science Cam</string>
+         </attribute>
+         <layout class="QGridLayout" name="gridLayout_20">
+          <item row="0" column="0">
+           <layout class="QVBoxLayout" name="verticalLayout_26">
+            <item>
+             <widget class="QLabel" name="label_72">
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="text">
+               <string>Video Output</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <layout class="QHBoxLayout" name="horizontalLayout_21">
+              <item>
+               <widget class="QPushButton" name="cam_lcd_output_button">
+                <property name="text">
+                 <string>LCD</string>
+                </property>
+               </widget>
+              </item>
+              <item>
+               <widget class="QPushButton" name="cam_network_output_button">
+                <property name="text">
+                 <string>Network</string>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+           </layout>
+          </item>
+          <item row="0" column="1">
+           <spacer name="horizontalSpacer_20">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>422</width>
+              <height>20</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+          <item row="1" column="0">
+           <layout class="QVBoxLayout" name="verticalLayout_27">
+            <item>
+             <widget class="QLabel" name="label_73">
+              <property name="font">
+               <font>
+                <weight>75</weight>
+                <bold>true</bold>
+               </font>
+              </property>
+              <property name="text">
+               <string>Controls</string>
+              </property>
+              <property name="alignment">
+               <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <layout class="QFormLayout" name="formLayout">
+              <item row="0" column="0">
+               <widget class="QPushButton" name="cam_zoom_in_button">
+                <property name="text">
+                 <string>Zoom In</string>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QPushButton" name="cam_zoom_out_button">
+                <property name="text">
+                 <string>Zoom Out</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="0">
+               <widget class="QPushButton" name="cam_full_zoom_in_button">
+                <property name="text">
+                 <string>Full Zoom In</string>
+                </property>
+               </widget>
+              </item>
+              <item row="1" column="1">
+               <widget class="QPushButton" name="cam_full_zoom_out_button">
+                <property name="text">
+                 <string>Full Zoom Out</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="0">
+               <widget class="QPushButton" name="cam_focus_button">
+                <property name="text">
+                 <string>Focus</string>
+                </property>
+               </widget>
+              </item>
+              <item row="2" column="1">
+               <widget class="QPushButton" name="cam_shoot_button">
+                <property name="text">
+                 <string>Shoot</string>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+           </layout>
+          </item>
+          <item row="2" column="0">
+           <spacer name="verticalSpacer_8">
+            <property name="orientation">
+             <enum>Qt::Vertical</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>20</width>
+              <height>331</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+         </layout>
+        </widget>
         <widget class="QWidget" name="tab_5">
          <attribute name="title">
           <string>RDF</string>
@@ -1575,7 +1712,7 @@ N/A</string>
                    </font>
                   </property>
                   <property name="text">
-                   <string>Preset Movements</string>
+                   <string>Special Movements</string>
                   </property>
                  </widget>
                 </item>
@@ -1702,6 +1839,83 @@ N/A</string>
                 </item>
                </layout>
               </item>
+              <item>
+               <layout class="QVBoxLayout" name="verticalLayout_25">
+                <item>
+                 <widget class="QLabel" name="label_69">
+                  <property name="font">
+                   <font>
+                    <weight>75</weight>
+                    <bold>true</bold>
+                   </font>
+                  </property>
+                  <property name="text">
+                   <string>Task Movements</string>
+                  </property>
+                 </widget>
+                </item>
+                <item>
+                 <layout class="QGridLayout" name="gridLayout_19">
+                  <item row="0" column="0">
+                   <widget class="QPushButton" name="arm_controls_approach_o2_button">
+                    <property name="styleSheet">
+                     <string notr="true"/>
+                    </property>
+                    <property name="text">
+                     <string>Approach O2 Tank</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item row="5" column="0">
+                   <spacer name="verticalSpacer_7">
+                    <property name="orientation">
+                     <enum>Qt::Vertical</enum>
+                    </property>
+                    <property name="sizeType">
+                     <enum>QSizePolicy::MinimumExpanding</enum>
+                    </property>
+                    <property name="sizeHint" stdset="0">
+                     <size>
+                      <width>20</width>
+                      <height>40</height>
+                     </size>
+                    </property>
+                   </spacer>
+                  </item>
+                  <item row="1" column="0">
+                   <widget class="QPushButton" name="arm_controls_depart_o2_button">
+                    <property name="styleSheet">
+                     <string notr="true"/>
+                    </property>
+                    <property name="text">
+                     <string>Depart O2 Tank</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item row="3" column="0">
+                   <widget class="QPushButton" name="arm_controls_approach_beacon_button">
+                    <property name="styleSheet">
+                     <string notr="true"/>
+                    </property>
+                    <property name="text">
+                     <string>Approach Beacon</string>
+                    </property>
+                   </widget>
+                  </item>
+                  <item row="4" column="0">
+                   <widget class="QPushButton" name="arm_controls_depart_beacon_button">
+                    <property name="styleSheet">
+                     <string notr="true"/>
+                    </property>
+                    <property name="text">
+                     <string>Depart Beacon</string>
+                    </property>
+                   </widget>
+                  </item>
+                 </layout>
+                </item>
+               </layout>
+              </item>
               <item>
                <spacer name="horizontalSpacer_15">
                 <property name="orientation">

software/ros_packages/ground_station/src/Resources/Ui/right_screen.ui

@@ -1604,7 +1604,7 @@ Position</string>
                           </font>
                          </property>
                          <property name="styleSheet">
-                          <string notr="true">background-color:darkgreen; border: 1px solid black;</string>
+                          <string notr="true">border: 1px solid black;</string>
                          </property>
                          <property name="text">
                           <string>Normal</string>
@@ -1634,7 +1634,7 @@ Position</string>
                           </font>
                          </property>
                          <property name="styleSheet">
-                          <string notr="true">border: 1px solid black;</string>
+                          <string notr="true">border: 1px solid black; background-color:darkgreen;</string>
                          </property>
                          <property name="text">
                           <string>Pinch</string>
@@ -1810,6 +1810,49 @@ Position</string>
                      </property>
                     </spacer>
                    </item>
+                   <item>
+                    <layout class="QVBoxLayout" name="verticalLayout_6">
+                     <property name="rightMargin">
+                      <number>6</number>
+                     </property>
+                     <item>
+                      <widget class="QLabel" name="label_102">
+                       <property name="font">
+                        <font>
+                         <pointsize>10</pointsize>
+                         <weight>75</weight>
+                         <bold>true</bold>
+                        </font>
+                       </property>
+                       <property name="text">
+                        <string>Reported Mode</string>
+                       </property>
+                       <property name="alignment">
+                        <set>Qt::AlignCenter</set>
+                       </property>
+                      </widget>
+                     </item>
+                     <item>
+                      <widget class="QLabel" name="gripper_reported_mode_label">
+                       <property name="minimumSize">
+                        <size>
+                         <width>0</width>
+                         <height>50</height>
+                        </size>
+                       </property>
+                       <property name="styleSheet">
+                        <string notr="true">border: 1px solid grey;</string>
+                       </property>
+                       <property name="text">
+                        <string>-------</string>
+                       </property>
+                       <property name="alignment">
+                        <set>Qt::AlignCenter</set>
+                       </property>
+                      </widget>
+                     </item>
+                    </layout>
+                   </item>
                    <item>
                     <layout class="QVBoxLayout" name="verticalLayout_68">
                      <item>
@@ -1824,6 +1867,9 @@ Position</string>
                        <property name="text">
                         <string>Reported Setpoint</string>
                        </property>
+                       <property name="alignment">
+                        <set>Qt::AlignCenter</set>
+                       </property>
                       </widget>
                      </item>
                      <item>
@@ -2511,16 +2557,6 @@ Position</string>
                      <property name="spacing">
                       <number>0</number>
                      </property>
-                     <item>
-                      <widget class="QDial" name="dial_3">
-                       <property name="minimum">
-                        <number>-100</number>
-                       </property>
-                       <property name="maximum">
-                        <number>100</number>
-                       </property>
-                      </widget>
-                     </item>
                      <item>
                       <widget class="QLabel" name="label">
                        <property name="font">
@@ -2537,6 +2573,16 @@ Position</string>
                        </property>
                       </widget>
                      </item>
+                     <item>
+                      <widget class="QLCDNumber" name="imu_pitch_lcd_number">
+                       <property name="minimumSize">
+                        <size>
+                         <width>0</width>
+                         <height>50</height>
+                        </size>
+                       </property>
+                      </widget>
+                     </item>
                     </layout>
                    </item>
                    <item>
@@ -2545,23 +2591,7 @@ Position</string>
                       <number>0</number>
                      </property>
                      <item>
-                      <widget class="QDial" name="dial_4">
+                      <widget class="QLabel" name="imu_pitch_lcd_number_2">
-                       <property name="minimum">
-                        <number>-100</number>
-                       </property>
-                       <property name="maximum">
-                        <number>100</number>
-                       </property>
-                       <property name="invertedAppearance">
-                        <bool>false</bool>
-                       </property>
-                       <property name="wrapping">
-                        <bool>true</bool>
-                       </property>
-                      </widget>
-                     </item>
-                     <item>
-                      <widget class="QLabel" name="label_15">
                        <property name="font">
                         <font>
                          <weight>75</weight>
@@ -2576,6 +2606,16 @@ Position</string>
                        </property>
                       </widget>
                      </item>
+                     <item>
+                      <widget class="QLCDNumber" name="imu_roll_lcd_number">
+                       <property name="minimumSize">
+                        <size>
+                         <width>0</width>
+                         <height>50</height>
+                        </size>
+                       </property>
+                      </widget>
+                     </item>
                     </layout>
                    </item>
                   </layout>

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

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

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,
+    "CHANGE_VIEW_MODE": 9,
-    "TILT_POSITION": 10,
+    "ZOOM_IN": 10,
-    "MEASURED_WEIGHT": 11
+    "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_subscriber = rospy.Subscriber(self.mining_control_subscriber_topic, MiningControlMessage, self.mining_control_message_received__callback)
-                                                          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,
+        self.mining_status_publisher = rospy.Publisher(self.mining_status_publisher_topic, MiningStatusMessage, queue_size=1)
-                                                       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.lift_position = self.mining_registers[MINING_MODBUS_REGISTERS["CURRENT_POSITION_LIFT"]]
-            message.tilt_position = self.mining_registers[MINING_MODBUS_REGISTERS["TILT_POSITION"]]
+            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))
+                    self.gripper_registers = self.gripper_node.read_registers(0, len(GRIPPER_MODBUS_REGISTERS))
-                    # print 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()

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 = CameraControlMessage()
-message.gripper_mode = 1
+message.camera_mode = 2
-message.gripper_position = 0
-message.should_home = 0
 
 publisher.publish(message)
 

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

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}]
             </rosparam>
         </node>
     </group>

software/ros_packages/rover_main/launch/rover/control.launch

@@ -35,8 +35,11 @@
 
         <node name="chassis_pan_tilt" pkg="rover_control" type="chassis_pan_tilt_control.py" respawn="true" output="screen"/>
 
-        <!--<node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">-->
+        <node name="effectors" pkg="rover_control" type="effectors_control.py" respawn="true" output="screen">
                 <!--<param name="port" value="/dev/rover/ttyARM"/>-->
-        <!--</node>-->
+        </node>
+
+        <node name="scale" pkg="rover_control" type="emergency_scale.py" respawn="true" output="screen">
+        </node>
     </group>
 </launch>

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}
                 ]
             </rosparam>
         </node>

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: