Power man

wesley/arduino/libraries/battery_checker/batterychecker.h

@@ -0,0 +1,70 @@
+/**
+ * March 2014
+ * Author: Sarah W
+ *
+ * Battery checker, reports if the voltage drops below TOLERANCE 
+ **/
+
+#ifndef BATTERYCHECKER_H 
+#define BATTERYCHECKER_H
+
+// Tolerance in volts (ex: 3.6)
+#define TOLERANCE 4.0
+
+#include <Arduino.h>
+
+class BatteryChecker {
+private:
+
+    int analogPin;
+    float lastReadValue;
+    bool safeOperatingVoltage;
+
+
+public:
+
+
+    // Parameter for analog pin that voltage is connected to
+    void init(int inputPin)  {
+        analogPin = inputPin;
+        lastReadValue = 0;
+        safeOperatingVoltage = true;
+
+        analogReference(DEFAULT);
+
+    }
+
+    // print input voltage
+    void printDebug()  {
+		if(lastReadValue < TOLERANCE)
+			Serial.print("***WARNING!*** ");
+        Serial.print("Current voltage: ");
+        Serial.println(lastReadValue);
+    }
+
+
+    bool update() {
+        int value = analogRead(analogPin);
+        lastReadValue = 0.0049 * value;
+
+        if (lastReadValue < TOLERANCE)
+        {
+            safeOperatingVoltage = false;        
+        }
+
+        return safeOperatingVoltage; 
+    }
+
+    bool isSafe() {
+        return safeOperatingVoltage;
+    }
+
+    void reset()
+    {
+        safeOperatingVoltage = true;        
+    }
+
+};
+
+#endif
+

wesley/arduino/mini/mini.ino

@@ -1,7 +1,8 @@
 #include <ros.h>					// basic ROS objects
 #include <std_msgs/Byte.h>			// leds are condensed into one byte
 #include <std_msgs/Bool.h>			// button: true / false
-
+#include <std_msgs/Int8.h
+#include <battery_checker.h>
 // majority of code lives in the ROS namespace
 using namespace ros;
 
@@ -45,11 +46,13 @@ NodeHandle nh;
 
 // keep track of running_state : this is altered by a button press.
 std_msgs::Bool running_state;
+std_msgs::int8 panic;
 std_msgs::Byte set_leds;
 
 Publisher                  pub("/master/button", &running_state);
 Subscriber<std_msgs::Byte> sub("/master/leds",   &display_status);
-
+Publisher panic_pub("/master/panic",1000); 
+BatteryChecker batteryChecker;
 void setup() {
 	// status display LEDs
 	pinMode(RED1, OUTPUT);
@@ -72,8 +75,11 @@ void setup() {
 	// initialize the node and attach the publication and subscription
 	nh.initNode();
 	nh.advertise(pub);
+	nh.advertise(panic_pub);
 	nh.subscribe(sub);
 
+	batteryChecker.init();
+
 	// set initial values of check.
 	running_state.data = false;
 }
@@ -135,6 +141,11 @@ void loop() {
 	// 8) sit and spin, checking each round for:
 	//    a) a message in on /master/leds
 	//    b) or a message waiting to go out on /master/button
+	batteryChecker.update();
+	panic.msg = (batteryChecker.isSafe())? 0 : 1;
+	if(panic.msg == 1){
+		panic_pub.publish(panic);
+	}
 	nh.spinOnce();
 	delay(5);
 

wesley/ros/src/commander/src/main.cpp

@@ -2,7 +2,9 @@
 #include "exit_handlers.h"
 #include <string>
 #include <unistd.h>
-
+#include <signal.h>
+#include <watchdog.h>
+//#include <thread.h> Include if using watchdog
 using std::string;
 
 #define grasp() executeBinary("rostopic pub -1 /arm/put/point wesley/arm_point '{direct_mode: false, cmd: grasp}'", "");
@@ -116,6 +118,11 @@ int executeBinary(string binaryName, string prefix, string mode ){
 	if(f == 0){
 		return -2;
 	}
+	/**
+	 * Uncomment this to turn on watchdogging 
+	//WatchDog watch;
+	//std::thread(watch,binaryName);
+	*/
 	//Get return value, don't ask why it's this but it is. It's from the stack overflow on popen. 
 	//
 	// http://bytes.com/topic/c/answers/131694-pclose-returning-termination-status-command#post472837
@@ -126,3 +133,5 @@ int executeBinary(string binaryName, string prefix, string mode ){
 	//    man 2 waitpid) did not shed any extra light on this.
 	return pclose(f)/256;
 }
+
+

wesley/ros/src/commander/src/watchdog.cpp

@@ -0,0 +1,28 @@
+#include "watchdog.h"
+#include <unistd.h>
+#include <stdio.h>
+#include <sstream>
+#include <thread>
+#include <chrono>
+using std::stringstream;
+
+void WatchDog::execute_pkill(string name){
+	stringstream ss;
+	ss << "pkill " << name;
+	FILE * f = popen(ss.str().c_str(),"r");
+	pclose(f);
+}
+
+void WatchDog::tick(){
+	amountCalled++;
+}
+
+void WatchDog::operator()(string binaryName){
+	amountCalled = 0;
+	while(amountCalled < checkAmount){
+		tick();
+		std::this_thread::sleep_for(std::chrono::seconds(seconds_to_sleep));
+	}
+	execute_pkill(binaryName);
+}
+

wesley/ros/src/commander/src/watchdog.h

@@ -0,0 +1,33 @@
+#include <string>
+#include <unistd.h>
+#include <signal.h>
+#include <thread>
+#include <map>
+#include <stdio.h>
+using std::map;
+using std::string;
+
+#ifndef WATCHDOG_H
+#define WATCHDOG_H
+/**
+ * Watch dog is a functor object that should be called as such
+ * WatchDog watch; 
+ * std::thread(watch,binaryName);
+ */
+class WatchDog{
+	string name;
+	int amountCalled;
+	int checkAmount; /*<The time in near-seconds for the thread to be alive before it's killed */
+	const int seconds_to_sleep = 1;
+	//Tick through each process updating the amount of times it's been called in call_map and kills it if it's been checked checkAmount times
+	void tick();
+	void execute_pkill(string name);	
+
+public:
+	WatchDog(){checkAmount=1000;}
+	WatchDog(int amountOfTicksBeforeKilled){checkAmount=amountOfTicksBeforeKilled;}
+	void operator()(string binaryName);
+
+};
+#endif
+

wesley/ros/src/powerman/CMakeLists.txt

@@ -0,0 +1,135 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(powerman)
+
+## 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
+  std_msgs
+)
+
+## 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 and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+###################################
+## 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 you 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 powerman
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+# add_library(powerman
+#   src/${PROJECT_NAME}/powerman.cpp
+# )
+
+## Declare a cpp executable
+# add_executable(powerman_node src/powerman_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(powerman_node powerman_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(powerman_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 powerman powerman_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_powerman.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)