diff --git a/wesley/arduino/libraries/battery_checker/batterychecker.h b/wesley/arduino/libraries/battery_checker/batterychecker.h new file mode 100644 index 0000000..86773f3 --- /dev/null +++ b/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 + +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 + diff --git a/wesley/arduino/mini/mini.ino b/wesley/arduino/mini/mini.ino index da6a594..6cbe177 100644 --- a/wesley/arduino/mini/mini.ino +++ b/wesley/arduino/mini/mini.ino @@ -1,7 +1,8 @@ #include // basic ROS objects #include // leds are condensed into one byte #include // button: true / false - +#include // 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 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); diff --git a/wesley/ros/src/commander/src/main.cpp b/wesley/ros/src/commander/src/main.cpp index d0eda1a..10c096d 100644 --- a/wesley/ros/src/commander/src/main.cpp +++ b/wesley/ros/src/commander/src/main.cpp @@ -2,7 +2,9 @@ #include "exit_handlers.h" #include #include - +#include +#include +//#include 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; } + + diff --git a/wesley/ros/src/commander/src/watchdog.cpp b/wesley/ros/src/commander/src/watchdog.cpp new file mode 100644 index 0000000..548320a --- /dev/null +++ b/wesley/ros/src/commander/src/watchdog.cpp @@ -0,0 +1,28 @@ +#include "watchdog.h" +#include +#include +#include +#include +#include +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); +} + diff --git a/wesley/ros/src/commander/src/watchdog.h b/wesley/ros/src/commander/src/watchdog.h new file mode 100644 index 0000000..d983660 --- /dev/null +++ b/wesley/ros/src/commander/src/watchdog.h @@ -0,0 +1,33 @@ +#include +#include +#include +#include +#include +#include +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; /* + + powerman + 0.0.0 + The powerman package + + + + + umkc + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + std_msgs + roscpp + std_msgs + + + + + + + + + + + \ No newline at end of file diff --git a/wesley/ros/src/powerman/src/main.cpp b/wesley/ros/src/powerman/src/main.cpp new file mode 100644 index 0000000..8e623d5 --- /dev/null +++ b/wesley/ros/src/powerman/src/main.cpp @@ -0,0 +1,21 @@ +#include +#include + +//Call pm-shutdown +void callback(const std_msg::int8& msg){ + ROS_INFO("powerman is killing it all, choo choo motherfuckers"); + FILE * f = popen("pm-shutdown","r"); + + //Eror while opening file stream + if(f == 0){ + return -2; + } + return 0; +} +int main(int argc, char* argv[]) { + + ros::init(argc, argv, "cmdr"); + ros::NodeHandle nh; + ROS_INFO("Powerman::main --> powerman initializing"); + ros::Subscriber sub = nh.subscribe("/master/panic",1000,callback); +}