caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Lots o changes. Too tired to think

Browse Source
This commit is contained in:
Corwin Perren
2018-08-05 11:23:26 -07:00
parent 91be05baff
commit e688aff233
15 changed files with 933 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

135
software/firmware/rdf/rdf.ino
View File

@@ -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,
LED_RED = 1,
LED_GREEN = 32,
LED_BLUE = 6,
RDF_INPUT = A7,
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() {
// 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::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::RDF_INPUT, INPUT);
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(){
if(poll_state > 4){
void set_leds() {
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;
}
digitalWrite(HARDWARE::LED_GREEN, LOW);
digitalWrite(HARDWARE::LED_RED, HIGH);
}else if(!communication_good){
} 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]);
}

172
software/firmware/temp/new_rdf.ino Normal file
View File

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

4
software/ros_packages/ground_station/src/Framework/MiscSystems/MiscArmCore.py
View File

@@ -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]

91
software/ros_packages/ground_station/src/Framework/MiscSystems/RDFCore.py Normal file
View File

@@ -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

10
software/ros_packages/ground_station/src/Resources/Ui/left_screen.ui
View File

@@ -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>

2
software/ros_packages/ground_station/src/ground_station.py
View File

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

1
software/ros_packages/rover_main/launch/rover.launch
View File

@@ -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"/>

3
software/ros_packages/rover_main/launch/rover/science.launch Normal file
View File

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

3
software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch
View File

@@ -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>

204
software/ros_packages/rover_science/CMakeLists.txt Normal file
View File

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

2
software/ros_packages/rover_science/msg/RDFStatusMessage.msg Normal file
View File

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

68
software/ros_packages/rover_science/package.xml Normal file
View File

@@ -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>

128
software/ros_packages/rover_science/src/freq_estimates.py Normal file
View File

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

197
software/ros_packages/rover_science/src/rover_science.py Executable file
View File

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

1
software/rover_setup.sh
View File

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