Lots o changes. Too tired to think

software/firmware/rdf/rdf.ino

@@ -3,151 +3,86 @@
 
 ////////// Hardware / Data Enumerations //////////
 enum HARDWARE {
-    RS485_EN = 2,
+  COMMS_RS485_EN = 3,
-    RS485_RX = 7,
+  COMMS_RS485_RX = 9,
-    RS485_TX = 8,
+  COMMS_RS485_TX = 10,
 
-    MOTOR_CURRENT_SENSE = A4,
+  // COMMS_RS485_EN = 2,
-    MOTOR_DIRECTION = 19,
+  // COMMS_RS485_RX = 0,
-    MOTOR_PWM = 20,
+  // COMMS_RS485_TX = 1,
-    MOTOR_SLEEP = 21,
-    MOTOR_FAULT = 22,
 
-    TEMP = A9,
+  RDF_INPUT = A7,
-
-    LED_RED = 1,
-    LED_GREEN = 32,
-    LED_BLUE = 6,
 
   LED_BLUE_EXTRA = 13
 };
 
 enum MODBUS_REGISTERS {
-    DIRECTION = 0,  // Input
+  RAW_DATA = 0
-    SPEED = 1,      // Input
-    SLEEP = 2,      // Input
-
-    CURRENT = 3,    // Output
-    FAULT = 4,      // Output
-
-    TEMPERATURE = 5        // Output
 };
 
-////////// Global Variables //////////
-const uint8_t node_id = 2;
-const uint8_t mobus_serial_port_number = 3;
 
-uint16_t modbus_data[] = {0, 0, 0, 0, 0, 0};
+////////// Global Variables //////////
+///// Modbus
+const uint8_t node_id = 1;
+const uint8_t modbus_serial_port_number = 2;
+
+uint16_t modbus_data[] = {0};
 uint8_t num_modbus_registers = 0;
 
 int8_t poll_state = 0;
 bool communication_good = false;
 uint8_t message_count = 0;
 
-uint16_t rampdown_step = 2000;
-
 ////////// Class Instantiations //////////
-Modbus slave(node_id, mobus_serial_port_number, HARDWARE::RS485_EN);
+Modbus slave(node_id, modbus_serial_port_number, HARDWARE::COMMS_RS485_EN);
 
 void setup() {
+  // Debugging
+ //  Serial.begin(9600);
+ // while (!Serial);
+
+  // Raw pin setup
   setup_hardware();
 
+  // Setup modbus serial communication
   num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
   slave.begin(115200); // baud-rate at 19200
-    slave.setTimeOut(150);
+  slave.setTimeOut(100);
 
-    Serial.begin(9600);
 }
 
 void loop() {
+  // Do normal polling
   poll_modbus();
   set_leds();
-    set_motor();
+
-    poll_sensors_and_motor_state();
 }
 
-void setup_hardware(){
+
+void setup_hardware() {
   // Setup pins as inputs / outputs
-    pinMode(HARDWARE::RS485_EN, OUTPUT);
+  pinMode(HARDWARE::RDF_INPUT, INPUT);
-
-    pinMode(HARDWARE::MOTOR_CURRENT_SENSE, INPUT);
-    pinMode(HARDWARE::MOTOR_DIRECTION, OUTPUT);
-    pinMode(HARDWARE::MOTOR_PWM, OUTPUT);
-    pinMode(HARDWARE::MOTOR_SLEEP, OUTPUT);
-    pinMode(HARDWARE::MOTOR_FAULT, INPUT);
-
-    pinMode(HARDWARE::TEMP, INPUT);
-
-    pinMode(HARDWARE::LED_RED, OUTPUT);
-    pinMode(HARDWARE::LED_GREEN, OUTPUT);
-    pinMode(HARDWARE::LED_BLUE, OUTPUT);
-
   pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
 
-    // Set default pin states
-    digitalWrite(HARDWARE::MOTOR_SLEEP, HIGH);
-
-    digitalWrite(HARDWARE::LED_RED, LOW);
-    digitalWrite(HARDWARE::LED_GREEN, HIGH);
-    digitalWrite(HARDWARE::LED_BLUE, HIGH);
-
-    digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
-
-    // Set the PWM resolution to 16-bits
-    analogWriteResolution(16);
-
-    // Change motor PWM frequency so it's not in the audible range
-    analogWriteFrequency(HARDWARE::MOTOR_PWM, 25000);
-
-    // Set teensy to increased analog resolution
   analogReadResolution(13);
+
 }
 
-void poll_modbus(){
+void poll_modbus() {
   poll_state = slave.poll(modbus_data, num_modbus_registers);
   communication_good = !slave.getTimeOutState();
+
+  modbus_data[MODBUS_REGISTERS::RAW_DATA] = analogRead(HARDWARE::RDF_INPUT);
 }
 
-void set_leds(){
+void set_leds() {
-    if(poll_state > 4){
+  if (poll_state > 4) {
     message_count++;
-        if(message_count > 2){
+    if (message_count > 2) {
       digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA));
       message_count = 0;
     }
-
+  } else if (!communication_good) {
-        digitalWrite(HARDWARE::LED_GREEN, LOW);
-        digitalWrite(HARDWARE::LED_RED, HIGH);
-    }else if(!communication_good){
     digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
-        digitalWrite(HARDWARE::LED_GREEN, HIGH);
-        digitalWrite(HARDWARE::LED_RED, LOW);
   }
 }
-
-void set_motor(){
-    if(communication_good){
-        digitalWrite(HARDWARE::MOTOR_DIRECTION, modbus_data[MODBUS_REGISTERS::DIRECTION]);
-        analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
-        digitalWrite(HARDWARE::MOTOR_SLEEP, modbus_data[MODBUS_REGISTERS::SLEEP]);
-    }else{
-        while(modbus_data[MODBUS_REGISTERS::SPEED] != 0 && modbus_data[MODBUS_REGISTERS::SPEED] > rampdown_step){
-            modbus_data[MODBUS_REGISTERS::SPEED] -= rampdown_step;
-            analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
-            delay(2);
-        }
-
-        modbus_data[MODBUS_REGISTERS::SPEED] = 0;
-        analogWrite(HARDWARE::MOTOR_PWM, modbus_data[MODBUS_REGISTERS::SPEED]);
-    }
-
-}
-
-void poll_sensors_and_motor_state(){
-    // Not the most elegant calculations, could clean up.
-    modbus_data[MODBUS_REGISTERS::CURRENT] = (uint16_t)(((((analogRead(HARDWARE::MOTOR_CURRENT_SENSE) / 8192.0) * 3.3) - 0.05) / 0.02) * 1000);
-    modbus_data[MODBUS_REGISTERS::FAULT] = !digitalRead(HARDWARE::MOTOR_FAULT);
-    modbus_data[MODBUS_REGISTERS::TEMPERATURE] = (uint16_t)(((((analogRead(HARDWARE::TEMP) / 8192.0) * 3.3) - 0.750) / 0.01) * 1000);
-
-    Serial.println(modbus_data[MODBUS_REGISTERS::CURRENT]);
-}

software/firmware/temp/new_rdf.ino

@@ -0,0 +1,172 @@
+////////// Includes //////////
+#include <ModbusRtu.h>
+
+////////// Hardware / Data Enumerations //////////
+enum HARDWARE {
+  COMMS_RS485_EN = 3,
+  COMMS_RS485_RX = 9,
+  COMMS_RS485_TX = 10,
+
+  // COMMS_RS485_EN = 2,
+  // COMMS_RS485_RX = 0,
+  // COMMS_RS485_TX = 1,
+
+  RDF_INPUT = A7,
+
+  LED_BLUE_EXTRA = 13
+};
+
+enum MODBUS_REGISTERS {
+  SENSITIVITY = 0,
+  RAW_DATA = 1,  // Input
+  CLEAN_DATA_POSITIVE = 2,
+  CLEAN_DATA_NEGATIVE = 3,
+  FREQUENCY = 4,
+};
+
+
+////////// Global Variables //////////
+///// Modbus
+const uint8_t node_id = 1;
+const uint8_t modbus_serial_port_number = 2;
+
+uint16_t modbus_data[] = {50, 0, 0, 0, 0};
+uint8_t num_modbus_registers = 0;
+
+int8_t poll_state = 0;
+bool communication_good = false;
+uint8_t message_count = 0;
+
+
+//////////////// Anothony's stuff /////////////
+int state;
+float freq;
+float ambientNoise;
+unsigned long totalDataPoints;
+int dataBuff[3];
+int data[3];
+unsigned long t1,t2,t3;
+int dt1,dt2 =0;
+float dtavg;
+int tcnt =2;
+bool upstate = false;
+
+
+////////// Class Instantiations //////////
+Modbus slave(node_id, modbus_serial_port_number, HARDWARE::COMMS_RS485_EN);
+
+void setup() {
+  // Debugging
+  Serial.begin(9600);
+ while (!Serial);
+
+  // Raw pin setup
+  setup_hardware();
+
+  // Setup modbus serial communication
+  num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]);
+  slave.begin(115200); // baud-rate at 19200
+  slave.setTimeOut(150);
+}
+
+void loop() {
+    anthonys_rdf_code();
+  // Do normal polling
+  poll_modbus();
+  set_leds();
+
+}
+
+void anthonys_rdf_code(){
+    modbus_data[MODBUS_REGISTERS::RAW_DATA] = analogRead(HARDWARE::RDF_INPUT);
+
+    for(int i=0;i<3;i++){
+      dataBuff[i] = data[i];
+  }
+    dataSet();
+    int change = changeCheck();
+    if(change){
+      state = change;
+      if(change == 1){
+        t1 = millis();
+        dt1 = t1-t2;
+      }else{
+        t2 = millis();
+        dt2 = t2-t1;
+      }
+      if(dt2>dt1-100&&dt1>dt2-100){
+        dtavg = (dtavg*float(tcnt-2)/float(tcnt))+(((dt1+dt2)/2.0)*float(2.0/tcnt));
+        tcnt += 2;
+        freq = 500.0/dtavg;
+      }
+    }
+    if(change ==1 || millis() > dtavg*2+t3){
+      t3 = millis();
+      upstate = true;
+    }
+    if(millis()<t3+dtavg){
+      for(int i=0;i<3;i++){
+        int out_data = data[i]-ambientNoise;
+
+        if(out_data >=0){
+            modbus_data[MODBUS_REGISTERS::CLEAN_DATA_NEGATIVE] = 0;
+            modbus_data[MODBUS_REGISTERS::CLEAN_DATA_POSITIVE] = out_data;
+        }else{
+            modbus_data[MODBUS_REGISTERS::CLEAN_DATA_POSITIVE] = 0;
+            modbus_data[MODBUS_REGISTERS::CLEAN_DATA_NEGATIVE] = out_data;
+        }
+        modbus_data[MODBUS_REGISTERS::FREQUENCY] = freq * 100;
+        // Serial.print(data[i]-ambientNoise);
+        // Serial.print(", ");
+        Serial.println(freq);
+      }
+    }else{
+      float avgdat,dtot =0;
+      for(int i=0;i<3;i++)
+        dtot+=data[i];
+      avgdat = dtot/3.0;
+      ambientNoise = ambientNoise*float(totalDataPoints)/float(totalDataPoints+3)+avgdat*(3.0/float(totalDataPoints+3));
+    }
+}
+
+void dataSet(){
+  for(int i=0;i<3;i++){
+    data[i]=analogRead(HARDWARE::RDF_INPUT);
+    delay(1);
+  }
+}
+
+int changeCheck(){
+    uint16_t sensitivity = modbus_data[MODBUS_REGISTERS::SENSITIVITY];
+    int newSignalState = 0;                // 0= no change      1= signal start       2= signal stop
+    if((data[0])>(dataBuff[2]+sensitivity)||data[2] > data[0]+sensitivity)
+      newSignalState = 1;
+    if((data[2] < data[0]-sensitivity)||(data[0]<(dataBuff[2]-sensitivity)))
+      newSignalState = 2;
+    return newSignalState;
+
+}
+
+void setup_hardware() {
+  // Setup pins as inputs / outputs
+  pinMode(HARDWARE::RDF_INPUT, INPUT);
+  pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT);
+
+}
+
+void poll_modbus() {
+  poll_state = slave.poll(modbus_data, num_modbus_registers);
+  communication_good = !slave.getTimeOutState();
+}
+
+void set_leds() {
+  if (poll_state > 4) {
+    message_count++;
+    if (message_count > 2) {
+      digitalWrite(HARDWARE::LED_BLUE_EXTRA, !digitalRead(HARDWARE::LED_BLUE_EXTRA));
+      message_count = 0;
+    }
+  } else if (!communication_good) {
+    digitalWrite(HARDWARE::LED_BLUE_EXTRA, LOW);
+  }
+}

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

@@ -72,11 +72,11 @@ ARM_STOW_PROCEDURE = [
     [0.0, -0.035, -0.28, 0.0, 0.0, 0.0],
     [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.0]
+    [0.0, -0.25, -0.5, -0.25, 0.25, -0.25]
 ]
 
 ARM_UNSTOW_PROCEDURE = [
-    [0.0, -0.25, -0.5, -0.25, 0.25, 0.0],
+    [0.0, -0.25, -0.5, -0.25, 0.25, -0.25],
     [0.0, -0.035, -0.5, -0.25, 0.25, 0.0],
     [0.0, -0.035, -0.28, -0.25, 0.25, 0.0],
     [0.0, -0.035, -0.28, 0.0, 0.0, 0.0]

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

@@ -0,0 +1,91 @@
+# coding=utf-8
+#####################################
+# Imports
+#####################################
+# Python native imports
+from PyQt5 import QtCore, QtWidgets
+import logging
+import rospy
+from time import time
+
+from std_msgs.msg import Float64MultiArray
+
+#####################################
+# Global Variables
+#####################################
+RDF_DATA_TOPIC = "/rdf/data"
+
+THREAD_HERTZ = 5
+
+
+#####################################
+# UbiquitiRadioSettings Class Definition
+#####################################
+class RDF(QtCore.QThread):
+
+    lcd_number_update_ready__signal = QtCore.pyqtSignal(int)
+
+    def __init__(self, shared_objects):
+        super(RDF, self).__init__()
+
+        # ########## Reference to class init variables ##########
+        self.shared_objects = shared_objects
+        self.left_screen = self.shared_objects["screens"]["left_screen"]
+
+        self.rssi_lcdnumber = self.left_screen.rssi_lcdnumber  # type:QtWidgets.QLCDNumber
+
+        # ########## Get the settings instance ##########
+        self.settings = QtCore.QSettings()
+
+        # ########## Get the Pick And Plate instance of the logger ##########
+        self.logger = logging.getLogger("groundstation")
+
+        # ########## Thread Flags ##########
+        self.run_thread_flag = True
+
+        # ########## Class Variables ##########
+        self.wait_time = 1.0 / THREAD_HERTZ
+
+        self.rdf_subscriber = rospy.Subscriber(RDF_DATA_TOPIC, Float64MultiArray, self.new_rdf_message_received__callback)
+
+        self.data = []
+        self.data_limit = 100
+
+
+    def run(self):
+        self.logger.debug("Starting RDF Thread")
+
+        while self.run_thread_flag:
+            start_time = time()
+
+            temp = list(self.data)
+            if temp:
+                average = sum(temp) / len(temp)
+                self.lcd_number_update_ready__signal.emit(average)
+
+            time_diff = time() - start_time
+
+            self.msleep(max(int(self.wait_time - time_diff), 0))
+
+        self.logger.debug("Stopping RDF Thread")
+
+    def new_rdf_message_received__callback(self, data):
+        if len(self.data) >= self.data_limit:
+            del self.data[0]
+
+        # if len(self.times) >= self.data_limit:
+        #     del self.times[0]
+
+        self.data.append(data.data[0])
+        # self.times.append(data.data[1])
+
+    def connect_signals_and_slots(self):
+        self.lcd_number_update_ready__signal.connect(self.rssi_lcdnumber.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)
+        kill_signal.connect(self.on_kill_threads_requested__slot)
+
+    def on_kill_threads_requested__slot(self):
+        self.run_thread_flag = False

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

@@ -1392,6 +1392,16 @@ N/A</string>
         <property name="currentIndex">
          <number>0</number>
         </property>
+        <widget class="QWidget" name="tab_5">
+         <attribute name="title">
+          <string>RDF</string>
+         </attribute>
+         <layout class="QGridLayout" name="gridLayout_16">
+          <item row="0" column="0">
+           <widget class="QLCDNumber" name="rssi_lcdnumber"/>
+          </item>
+         </layout>
+        </widget>
         <widget class="QWidget" name="tab_4">
          <attribute name="title">
           <string>Arm</string>

software/ros_packages/ground_station/src/ground_station.py

@@ -27,6 +27,7 @@ import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
 import Framework.MiscSystems.MiningCore as MiningCore
 import Framework.MiscSystems.BashConsoleCore as BashConsoleCore
 import Framework.MiscSystems.MiscArmCore as MiscArmCore
+import Framework.MiscSystems.RDFCore as RDFCore
 
 #####################################
 # Global Variables
@@ -121,6 +122,7 @@ class GroundStation(QtCore.QObject):
         self.__add_thread("Spacenav Sender", SpaceNavControlSender.SpaceNavControlSender(self.shared_objects))
         self.__add_thread("Bash Console", BashConsoleCore.BashConsole(self.shared_objects))
         self.__add_thread("Misc Arm", MiscArmCore.MiscArm(self.shared_objects))
+        self.__add_thread("RDF", RDFCore.RDF(self.shared_objects))
 
         self.connect_signals_and_slots_signal.emit()
         self.__connect_signals_to_slots()

software/ros_packages/rover_main/launch/rover.launch

@@ -2,6 +2,7 @@
     <!-- ########## Start Rover Control Nodes ########## -->
     <include file="$(find rover_main)/launch/rover/control.launch"/>
     <include file="$(find rover_main)/launch/rover/arm.launch"/>
+    <include file="$(find rover_main)/launch/rover/science.launch"/>
 
     <!-- ########## Start All Rover Camera Nodes ########## -->
     <include file="$(find rover_main)/launch/rover/cameras.launch"/>

software/ros_packages/rover_main/launch/rover/science.launch

@@ -0,0 +1,3 @@
+<launch>
+    <node name="rover_science" pkg="rover_science" type="rover_science.py" respawn="true" output="screen"/>
+</launch>

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

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

software/ros_packages/rover_science/CMakeLists.txt

@@ -0,0 +1,204 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rover_science)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+add_message_files(
+    FILES
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES system_statuses
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/system_statuses.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/system_statuses_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_system_statuses.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

software/ros_packages/rover_science/msg/RDFStatusMessage.msg

@@ -0,0 +1,2 @@
+uint16 raw_value
+double time

software/ros_packages/rover_science/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rover_science</name>
+  <version>0.0.0</version>
+  <description>The rover_status package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="matcurtay@matcurtay.net">matcurtay</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/rover_status</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <build_depend>message_generation</build_depend> 
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <exec_depend>message_runtime</exec_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

software/ros_packages/rover_science/src/freq_estimates.py

@@ -0,0 +1,128 @@
+from __future__ import division
+from numpy.fft import rfft
+from numpy import argmax, mean, diff, log
+from matplotlib.mlab import find
+from scipy.signal import blackmanharris, fftconvolve
+from numpy import polyfit, arange
+
+
+def freq_from_crossings(sig, fs):
+    """
+    Estimate frequency by counting zero crossings
+    """
+    # Find all indices right before a rising-edge zero crossing
+    indices = find((sig[1:] >= 0) & (sig[:-1] < 0))
+
+    # Naive (Measures 1000.185 Hz for 1000 Hz, for instance)
+    # crossings = indices
+
+    # More accurate, using linear interpolation to find intersample
+    # zero-crossings (Measures 1000.000129 Hz for 1000 Hz, for instance)
+    crossings = [i - sig[i] / (sig[i+1] - sig[i]) for i in indices]
+
+    # Some other interpolation based on neighboring points might be better.
+    # Spline, cubic, whatever
+
+    return fs / mean(diff(crossings))
+
+
+def freq_from_fft(sig, fs):
+    """
+    Estimate frequency from peak of FFT
+    """
+    # Compute Fourier transform of windowed signal
+    windowed = sig * blackmanharris(len(sig))
+    f = rfft(windowed)
+
+    # Find the peak and interpolate to get a more accurate peak
+    i = argmax(abs(f))  # Just use this for less-accurate, naive version
+    true_i = parabolic(log(abs(f)), i)[0]
+
+    # Convert to equivalent frequency
+    return fs * true_i / len(windowed)
+
+
+def freq_from_autocorr(sig, fs):
+    """
+    Estimate frequency using autocorrelation
+    """
+    # Calculate autocorrelation (same thing as convolution, but with
+    # one input reversed in time), and throw away the negative lags
+    corr = fftconvolve(sig, sig[::-1], mode='full')
+    corr = corr[len(corr)//2:]
+
+    # Find the first low point
+    d = diff(corr)
+    start = find(d > 0)[0]
+
+    # Find the next peak after the low point (other than 0 lag).  This bit is
+    # not reliable for long signals, due to the desired peak occurring between
+    # samples, and other peaks appearing higher.
+    # Should use a weighting function to de-emphasize the peaks at longer lags.
+    peak = argmax(corr[start:]) + start
+    px, py = parabolic(corr, peak)
+
+    return fs / px
+
+
+def freq_from_HPS(sig, fs):
+    """
+    Estimate frequency using harmonic product spectrum (HPS)
+    """
+    windowed = sig * blackmanharris(len(sig))
+
+    from pylab import subplot, plot, log, copy, show
+
+    # harmonic product spectrum:
+    c = abs(rfft(windowed))
+    maxharms = 8
+    subplot(maxharms, 1, 1)
+    plot(log(c))
+    for x in range(2, maxharms):
+        a = copy(c[::x])  # Should average or maximum instead of decimating
+        # max(c[::x],c[1::x],c[2::x],...)
+        c = c[:len(a)]
+        i = argmax(abs(c))
+        true_i = parabolic(abs(c), i)[0]
+        print 'Pass %d: %f Hz' % (x, fs * true_i / len(windowed))
+        c *= a
+        subplot(maxharms, 1, x)
+        plot(log(c))
+    show()
+
+
+def parabolic(f, x):
+    """Quadratic interpolation for estimating the true position of an
+    inter-sample maximum when nearby samples are known.
+
+    f is a vector and x is an index for that vector.
+
+    Returns (vx, vy), the coordinates of the vertex of a parabola that goes
+    through point x and its two neighbors.
+
+    Example:
+    Defining a vector f with a local maximum at index 3 (= 6), find local
+    maximum if points 2, 3, and 4 actually defined a parabola.
+
+    In [3]: f = [2, 3, 1, 6, 4, 2, 3, 1]
+
+    In [4]: parabolic(f, argmax(f))
+    Out[4]: (3.2142857142857144, 6.1607142857142856)
+
+    """
+    xv = 1 / 2. * (f[x - 1] - f[x + 1]) / (f[x - 1] - 2 * f[x] + f[x + 1]) + x
+    yv = f[x] - 1 / 4. * (f[x - 1] - f[x + 1]) * (xv - x)
+    return (xv, yv)
+
+
+def parabolic_polyfit(f, x, n):
+    """Use the built-in polyfit() function to find the peak of a parabola
+
+    f is a vector and x is an index for that vector.
+
+    n is the number of samples of the curve used to fit the parabola.
+    """
+    a, b, c = polyfit(arange(x - n // 2, x + n // 2 + 1), f[x - n // 2:x + n // 2 + 1], 2)
+    xv = -0.5 * b / a
+    yv = a * xv ** 2 + b * xv + c
+    return (xv, yv)

software/ros_packages/rover_science/src/rover_science.py

@@ -0,0 +1,197 @@
+#!/usr/bin/env python
+#####################################
+# Imports
+#####################################
+# Python native imports
+import rospy
+
+from time import time, sleep
+
+import serial.rs485
+import minimalmodbus
+import numpy
+
+# Custom Imports
+from rover_control.msg import TowerPanTiltControlMessage
+from std_msgs.msg import Float64MultiArray
+
+#####################################
+# Global Variables
+#####################################
+NODE_NAME = "science_node"
+
+DEFAULT_PORT = "/dev/rover/ttyIRIS_2_0"
+DEFAULT_BAUD = 115200
+
+DEFAULT_INVERT = False
+
+DEFAULT_RDF_PUBLISHER_TOPIC = "rdf/data"
+
+PAN_TILT_NODE_ID = 1
+
+COMMUNICATIONS_TIMEOUT = 0.005  # Seconds
+
+RX_DELAY = 0.01
+TX_DELAY = 0.01
+
+DEFAULT_HERTZ = 20
+
+PAN_TILT_MODBUS_REGISTERS = {
+    "CENTER_ALL": 0,
+
+    "PAN_ADJUST_POSITIVE": 1,
+    "PAN_ADJUST_NEGATIVE": 2,
+    "TILT_ADJUST_POSITIVE": 3,
+    "TILT_ADJUST_NEGATIVE": 4
+}
+
+NODE_LAST_SEEN_TIMEOUT = 2  # seconds
+
+
+#####################################
+# DriveControl Class Definition
+#####################################
+class RoverScience(object):
+    def __init__(self):
+        rospy.init_node(NODE_NAME)
+
+        self.port = rospy.get_param("~port", DEFAULT_PORT)
+        self.baud = rospy.get_param("~baud", DEFAULT_BAUD)
+
+        self.pan_tilt_node_id = rospy.get_param("~pan_tilt_node_id", PAN_TILT_NODE_ID)
+
+        self.wait_time = 1.0 / rospy.get_param("~hertz", DEFAULT_HERTZ)
+
+        self.pan_tilt_node = None
+        self.tower_node = None
+
+        self.connect_to_pan_tilt_and_tower()
+
+        self.rdf_publisher = rospy.Publisher(DEFAULT_RDF_PUBLISHER_TOPIC, Float64MultiArray, queue_size=1)
+
+        self.pan_tilt_control_message = None
+        self.new_pan_tilt_control_message = False
+
+        self.modbus_nodes_seen_time = time()
+
+        self.data = numpy.array([])
+        self.times = numpy.array([])
+        self.max_data_len = 200
+
+        self.count = 0
+        self.start_time = time()
+
+        self.run()
+
+    def __setup_minimalmodbus_for_485(self):
+        self.pan_tilt_node.serial = serial.rs485.RS485(self.port, baudrate=self.baud, timeout=COMMUNICATIONS_TIMEOUT)
+        self.pan_tilt_node.serial.rs485_mode = serial.rs485.RS485Settings(rts_level_for_rx=1, rts_level_for_tx=0,
+                                                                          delay_before_rx=RX_DELAY,
+                                                                          delay_before_tx=TX_DELAY)
+
+    def run(self):
+        # self.send_startup_centering_command()
+        while not rospy.is_shutdown():
+            start_time = time()
+            try:
+                registers = self.pan_tilt_node.read_registers(0, 1)
+                self.rdf_publisher.publish(Float64MultiArray(data=[registers[0], time()]))
+                self.modbus_nodes_seen_time = time()
+            except Exception, Error:
+                print Error
+
+            if (time() - self.modbus_nodes_seen_time) > NODE_LAST_SEEN_TIMEOUT:
+                print "Science not seen for", NODE_LAST_SEEN_TIMEOUT, "seconds. Exiting."
+                return  # Exit so respawn can take over
+
+            time_diff = time() - start_time
+
+    # def run(self):
+    #     # self.send_startup_centering_command()
+    #     while not rospy.is_shutdown():
+    #         registers = self.pan_tilt_node.read_registers(0, 1)
+    #         self.data = numpy.append(self.data, registers[0])
+    #         self.times = numpy.append(self.times, time())
+    #
+    #         if len(self.data) > self.max_data_len:
+    #             numpy.delete(self.data, 0)
+    #             numpy.delete(self.times, 0)
+    #
+    #             # for item in self.smoothListTriangle(self.data):
+    #             #     print item
+    #
+    #             print fq.freq_from_fft(self.data, 1/40.0)
+    #             # self.data = self.smoothListGaussian(self.data)
+    #             #
+    #             # w = numpy.fft.fft(self.data)
+    #             #
+    #             # # for item in w:
+    #             # #     print abs(item)
+    #             # #
+    #             # # print
+    #             # # print
+    #             # numpy.delete(w, 0)
+    #             # freqs = numpy.fft.fftfreq(len(w), 1/40.0)
+    #             # # # print len(freqs), len(w)
+    #             # #
+    #             # # # print freqs
+    #             # # # print(freqs.min(), freqs.max())
+    #             # # # # (-0.5, 0.499975)
+    #             # # #
+    #             # # # # Find the peak in the coefficients
+    #             # idx = numpy.argmax(numpy.abs(w))
+    #             # freq = freqs[idx]
+    #             # freq_in_hertz = abs(freq * 40)
+    #             # print(freq_in_hertz)
+
+
+    def smoothListTriangle(self, list, strippedXs=False, degree=5):
+
+        weight = []
+
+        window = degree * 2 - 1
+
+        smoothed = [0.0] * (len(list) - window)
+
+        for x in range(1, 2 * degree): weight.append(degree - abs(degree - x))
+
+        w = numpy.array(weight)
+
+        for i in range(len(smoothed)):
+            smoothed[i] = sum(numpy.array(list[i:i + window]) * w) / float(sum(w))
+
+        return smoothed
+
+    def smoothListGaussian(self, list, strippedXs=False, degree=5):
+
+        window = degree * 2 - 1
+
+        weight = numpy.array([1.0] * window)
+
+        weightGauss = []
+
+        for i in range(window):
+            i = i - degree + 1
+
+            frac = i / float(window)
+
+            gauss = 1 / (numpy.exp((4 * (frac)) ** 2))
+
+            weightGauss.append(gauss)
+
+        weight = numpy.array(weightGauss) * weight
+
+        smoothed = [0.0] * (len(list) - window)
+
+        for i in range(len(smoothed)):
+            smoothed[i] = sum(numpy.array(list[i:i + window]) * weight) / sum(weight)
+
+        return smoothed
+
+    def connect_to_pan_tilt_and_tower(self):
+        self.pan_tilt_node = minimalmodbus.Instrument(self.port, int(self.pan_tilt_node_id))
+        self.__setup_minimalmodbus_for_485()
+
+
+if __name__ == "__main__":
+    RoverScience()

software/rover_setup.sh

@@ -15,6 +15,7 @@ folders_to_link=(
     nimbro_topic_transport
     rover_main
     rover_odometry
+    rover_science
 )
 
 # Print heading