diff --git a/software/firmware/rdf/rdf.ino b/software/firmware/rdf/rdf.ino index 6639da1..2168922 100644 --- a/software/firmware/rdf/rdf.ino +++ b/software/firmware/rdf/rdf.ino @@ -3,151 +3,86 @@ ////////// Hardware / Data Enumerations ////////// enum HARDWARE { - RS485_EN = 2, - RS485_RX = 7, - RS485_TX = 8, + COMMS_RS485_EN = 3, + COMMS_RS485_RX = 9, + COMMS_RS485_TX = 10, - MOTOR_CURRENT_SENSE = A4, - MOTOR_DIRECTION = 19, - MOTOR_PWM = 20, - MOTOR_SLEEP = 21, - MOTOR_FAULT = 22, + // COMMS_RS485_EN = 2, + // COMMS_RS485_RX = 0, + // COMMS_RS485_TX = 1, - TEMP = A9, + RDF_INPUT = A7, - LED_RED = 1, - LED_GREEN = 32, - LED_BLUE = 6, - - LED_BLUE_EXTRA = 13 + LED_BLUE_EXTRA = 13 }; enum MODBUS_REGISTERS { - DIRECTION = 0, // Input - SPEED = 1, // Input - SLEEP = 2, // Input - - CURRENT = 3, // Output - FAULT = 4, // Output - - TEMPERATURE = 5 // Output + RAW_DATA = 0 }; -////////// 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() { - setup_hardware(); + // Debugging + // Serial.begin(9600); + // while (!Serial); - num_modbus_registers = sizeof(modbus_data) / sizeof(modbus_data[0]); - slave.begin(115200); // baud-rate at 19200 - slave.setTimeOut(150); + // 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(100); - Serial.begin(9600); } void loop() { - poll_modbus(); - set_leds(); - set_motor(); - poll_sensors_and_motor_state(); + // Do normal polling + poll_modbus(); + set_leds(); + } -void setup_hardware(){ - // Setup pins as inputs / outputs - pinMode(HARDWARE::RS485_EN, OUTPUT); - 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); +void setup_hardware() { + // Setup pins as inputs / outputs + pinMode(HARDWARE::RDF_INPUT, INPUT); + pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); - pinMode(HARDWARE::TEMP, INPUT); + analogReadResolution(13); - pinMode(HARDWARE::LED_RED, OUTPUT); - pinMode(HARDWARE::LED_GREEN, OUTPUT); - pinMode(HARDWARE::LED_BLUE, OUTPUT); +} - pinMode(HARDWARE::LED_BLUE_EXTRA, OUTPUT); +void poll_modbus() { + poll_state = slave.poll(modbus_data, num_modbus_registers); + communication_good = !slave.getTimeOutState(); - // Set default pin states - digitalWrite(HARDWARE::MOTOR_SLEEP, HIGH); - - digitalWrite(HARDWARE::LED_RED, LOW); - digitalWrite(HARDWARE::LED_GREEN, HIGH); - digitalWrite(HARDWARE::LED_BLUE, HIGH); + modbus_data[MODBUS_REGISTERS::RAW_DATA] = analogRead(HARDWARE::RDF_INPUT); +} +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); - - // 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(){ - 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; - } - - 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]); + } } diff --git a/software/firmware/temp/new_rdf.ino b/software/firmware/temp/new_rdf.ino new file mode 100644 index 0000000..ee8627f --- /dev/null +++ b/software/firmware/temp/new_rdf.ino @@ -0,0 +1,172 @@ +////////// Includes ////////// +#include + +////////// 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()=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); + } +} diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py index 105d57a..2755e98 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py +++ b/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] diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py new file mode 100644 index 0000000..011b6d6 --- /dev/null +++ b/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 diff --git a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui index 76a7b62..fc9ae74 100644 --- a/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui +++ b/software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui @@ -1392,6 +1392,16 @@ N/A 0 + + + RDF + + + + + + + Arm diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index 85ae46b..3fee80c 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/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() diff --git a/software/ros_packages/rover_main/launch/rover.launch b/software/ros_packages/rover_main/launch/rover.launch index c01a022..e86b82b 100644 --- a/software/ros_packages/rover_main/launch/rover.launch +++ b/software/ros_packages/rover_main/launch/rover.launch @@ -2,6 +2,7 @@ + diff --git a/software/ros_packages/rover_main/launch/rover/science.launch b/software/ros_packages/rover_main/launch/rover/science.launch new file mode 100644 index 0000000..9e88337 --- /dev/null +++ b/software/ros_packages/rover_main/launch/rover/science.launch @@ -0,0 +1,3 @@ + + + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index 4b667b2..3cdb67e 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -155,7 +155,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} ] diff --git a/software/ros_packages/rover_science/CMakeLists.txt b/software/ros_packages/rover_science/CMakeLists.txt new file mode 100644 index 0000000..f0a18b3 --- /dev/null +++ b/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) diff --git a/software/ros_packages/rover_science/msg/RDFStatusMessage.msg b/software/ros_packages/rover_science/msg/RDFStatusMessage.msg new file mode 100644 index 0000000..1b39a87 --- /dev/null +++ b/software/ros_packages/rover_science/msg/RDFStatusMessage.msg @@ -0,0 +1,2 @@ +uint16 raw_value +double time \ No newline at end of file diff --git a/software/ros_packages/rover_science/package.xml b/software/ros_packages/rover_science/package.xml new file mode 100644 index 0000000..e4bbd2a --- /dev/null +++ b/software/ros_packages/rover_science/package.xml @@ -0,0 +1,68 @@ + + + rover_science + 0.0.0 + The rover_status package + + + + + matcurtay + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + message_generation + + + + + + message_runtime + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/software/ros_packages/rover_science/src/freq_estimates.py b/software/ros_packages/rover_science/src/freq_estimates.py new file mode 100644 index 0000000..8bc1604 --- /dev/null +++ b/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) diff --git a/software/ros_packages/rover_science/src/rover_science.py b/software/ros_packages/rover_science/src/rover_science.py new file mode 100755 index 0000000..c2562b4 --- /dev/null +++ b/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() diff --git a/software/rover_setup.sh b/software/rover_setup.sh index 2209c80..0845072 100755 --- a/software/rover_setup.sh +++ b/software/rover_setup.sh @@ -15,6 +15,7 @@ folders_to_link=( nimbro_topic_transport rover_main rover_odometry + rover_science ) # Print heading