using a smaller buffer and small reorganization

ulrichard · Jan 21, 2012 · 0be74f3 · 0be74f3
1 parent 8833a38
commit 0be74f3
Show file tree

Hide file tree

Showing 4 changed files with 59 additions and 97 deletions.
diff --git a/ros/uc_arm/CMakeLists.txt b/ros/uc_arm/CMakeLists.txt
@@ -9,7 +9,6 @@ INCLUDE_DIRECTORIES(/opt/ros/electric/stacks/rosserial/rosserial_arduino/librari
 
 ADD_ROBOT_ARM_EXECUTABLE(RobotArm_ROS
 	RobotArm_ROS.cpp
-	time.cpp
 )
 
 INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/RobotArm_ROS.hex DESTINATION ${CMAKE_INSTALL_PREFIX}/share/robotloader/examples/ros)

diff --git a/ros/uc_arm/RobotArm_ROS.cpp b/ros/uc_arm/RobotArm_ROS.cpp
@@ -3,15 +3,36 @@
 // http://www.ros.org/wiki/rosserial_arduino/Tutorials
 // Richard Ulrich <richi@paraeasy.ch>
 // GPL v3
-
+
+#include "ArexxArmHardware.h"
 // robot arm lib
-#include "RobotArmBaseLib.h"
+#include <RobotArmBaseLib.h>
 // ros
-#include "ros_arexx.h"
+#include <ros/node_handle.h>
 #include <std_msgs/Int16.h>
 #include <std_msgs/UInt8.h>
 // std lib
-#include <ctype.h>
+#include <ctype.h>
+
+/*****************************************************************************/
+namespace ros
+{
+/*
+#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega168__)
+  // downsize our buffers
+  typedef NodeHandle_<ArexxArmHardware, 6, 6, 150, 150> NodeHandle;
+
+#elif defined(__AVR_ATmega328P__)
+*/
+  typedef NodeHandle_<ArexxArmHardware, 25, 25, 280, 280> NodeHandle;
+/*
+#else
+
+  typedef NodeHandle_<ArexxArmHardware> NodeHandle;
+
+#endif
+*/
+}
 /*****************************************************************************/
 // callback functions
 template<uint8_t servonum>
@@ -91,6 +112,40 @@ int main(void)
 	return 0;
 }
 /*****************************************************************************/
+namespace ros
+{
+  void normalizeSecNSec(unsigned long& sec, unsigned long& nsec){
+    unsigned long nsec_part= nsec % 1000000000UL;
+    unsigned long sec_part = nsec / 1000000000UL;
+    sec += sec_part;
+    nsec = nsec_part;
+  }
+
+  Time& Time::fromNSec(long t)
+  {
+    sec = t / 1000000000;
+    nsec = t % 1000000000;
+    normalizeSecNSec(sec, nsec);
+    return *this;
+  }
+
+  Time& Time::operator +=(const Duration &rhs)
+  {
+    sec += rhs.sec;
+    nsec += rhs.nsec;
+    normalizeSecNSec(sec, nsec);
+    return *this;
+  }
+
+  Time& Time::operator -=(const Duration &rhs){
+    sec += -rhs.sec;
+    nsec += -rhs.nsec;
+    normalizeSecNSec(sec, nsec);
+    return *this;
+  }
+
+}
+/*****************************************************************************/
 /// http://stackoverflow.com/questions/920500/what-is-the-purpose-of-cxa-pure-virtual
 extern "C" void __cxa_pure_virtual() { while (1); }
 /*****************************************************************************/

diff --git a/ros/uc_arm/ros_arexx.h b/ros/uc_arm/ros_arexx.h
diff --git a/ros/uc_arm/time.cpp b/ros/uc_arm/time.cpp