Thread: [Personalrobots-commit] SF.net SVN: personalrobots: [149] pkg/trunk/etherdrive

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 149
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=149&view=rev
Author:   jleibs
Date:     2008-04-21 14:48:44 -0700 (Mon, 21 Apr 2008)

Log Message:
-----------
Some cleanup of etherdrive node.

Added Paths:
-----------
    pkg/trunk/etherdrive/include/etherdrive/
    pkg/trunk/etherdrive/include/etherdrive/etherdrive.h
    pkg/trunk/etherdrive/nodes/
    pkg/trunk/etherdrive/src/etherdrive_control/
    pkg/trunk/etherdrive/src/etherdrive_control/Makefile
    pkg/trunk/etherdrive/src/etherdrive_control/etherdrive_control.cpp
    pkg/trunk/etherdrive/test/test_etherdrive/
    pkg/trunk/etherdrive/test/test_etherdrive/Makefile
    pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp

Removed Paths:
-------------
    pkg/trunk/etherdrive/test/Makefile
    pkg/trunk/etherdrive/test/test_etherdrive.cpp

Added: pkg/trunk/etherdrive/include/etherdrive/etherdrive.h
===================================================================

--- pkg/trunk/etherdrive/include/etherdrive/etherdrive.h	                        (rev 0)
+++ pkg/trunk/etherdrive/include/etherdrive/etherdrive.h	2008-04-21 21:48:44 UTC (rev 149)
@@ -0,0 +1,84 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef ETHERDRIVE_H
+#define ETHERDRIVE_H
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+
+#include <string>
+
+using namespace std;
+
+class EtherDrive
+{
+public:
+  EtherDrive();
+  ~EtherDrive();
+
+  // Initialize 
+  bool init(string ip);
+  
+  void shutdown();
+
+  // Manually send EtherDrive command.
+  int send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len);
+
+  // Enable motors
+  bool motors_on();
+
+  // Disable motors
+  bool motors_off();
+
+  // Send an array of motor commands up to 6 in length.
+  bool drive(size_t num, int32_t* drv);
+
+  // Send most recent motor commands, and retrieve update (this must be run at sufficient rate).
+  bool tick(size_t num = 0, int32_t* enc = 0, int32_t* curr = 0, int32_t* pwm = 0);
+private:
+  bool ready;
+
+  int32_t last_drv[6];
+
+  int mot_sock;
+  int cmd_sock;
+
+  struct sockaddr_in mot_addr_out;
+  struct sockaddr_in cmd_addr_out;
+};
+
+#endif
+

Added: pkg/trunk/etherdrive/src/etherdrive_control/Makefile
===================================================================
--- pkg/trunk/etherdrive/src/etherdrive_control/Makefile	                        (rev 0)
+++ pkg/trunk/etherdrive/src/etherdrive_control/Makefile	2008-04-21 21:48:44 UTC (rev 149)
@@ -0,0 +1,4 @@
+SRC = etherdrive_control.cpp
+OUT = ../../nodes/etherdrive
+PKG = etherdrive
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk

Added: pkg/trunk/etherdrive/src/etherdrive_control/etherdrive_control.cpp
===================================================================
--- pkg/trunk/etherdrive/src/etherdrive_control/etherdrive_control.cpp	                        (rev 0)
+++ pkg/trunk/etherdrive/src/etherdrive_control/etherdrive_control.cpp	2008-04-21 21:48:44 UTC (rev 149)
@@ -0,0 +1,152 @@
+///////////////////////////////////////////////////////////////////////////////
+// The axis_cam package provides a library that talks to Axis IP-based cameras
+// as well as ROS nodes which use these libraries
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without 
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice, 
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright 
+//     notice, this list of conditions and the following disclaimer in the 
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Stanford University nor the names of its 
+//     contributors may be used to endorse or promote products derived from 
+//     this software without specific prior written permission.
+//   
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "ros/ros_slave.h"
+#include "unstable_flows/FlowActuator.h"
+#include "etherdrive/etherdrive.h"
+#include <sstream>
+
+class EtherDrive_Control : public ROS_Slave
+{
+public:
+  FlowActuator* mot[6];
+  FlowActuator* mot_cmd[6];
+
+  string host;
+  EtherDrive *ed;
+  int frame_id;
+
+  float last_mot_val[6];
+  double pulse_per_rad[6];
+
+  ROS_Mutex control_mutex;
+
+  EtherDrive_Control() : ROS_Slave(), ed(NULL)
+  {
+
+    for (int i = 0;i < 6;i++) {
+      ostringstream oss;
+      oss << "mot" << i;
+      register_source(mot[i] = new FlowActuator(oss.str().c_str()));
+
+      oss << "_cmd";
+      register_sink(mot_cmd[i] = new FlowActuator(oss.str().c_str()), ROS_CALLBACK(EtherDrive_Control, mot_callback));
+
+      last_mot_val[i] = 0;
+    }
+
+    register_with_master();
+
+    if (!get_string_param(".host", host))
+      host = "10.0.0.151";
+    printf("EtherDrive host set to [%s]\n", host.c_str());
+
+    if (!get_int_param(".frame_id", &frame_id))
+      frame_id = -1;
+    printf("EtherDrive frame_id set to [%d]\n", frame_id);
+
+    for (int i = 0;i < 6;i++) {
+      ostringstream oss;
+      oss << ".pulse_per_rad" << i;
+      if (!get_double_param(oss.str().c_str(), &(pulse_per_rad[i])))
+	pulse_per_rad[i] = 1591.54943;
+    }
+
+    ed = new EtherDrive();
+    ed->init(host);
+    ed->motors_on();
+  }
+
+  virtual ~EtherDrive_Control()
+  { 
+    if (ed) 
+      delete ed; 
+  }
+
+  bool do_tick()
+  {
+
+    int tmp_mot_cmd[6];
+
+    for (int i = 0; i < 6; i++) {
+    mot_cmd[i]->lock_atom();
+      if (mot_cmd[i]->valid) {
+	if (mot_cmd[i]->rel) {
+	  last_mot_val[i] = mot[i]->val + mot_cmd[i]->val;
+	} else {
+	  last_mot_val[i] = mot_cmd[i]->val;
+	}
+      }
+      mot_cmd[i]->valid = false;  // set to invalid so we don't re-use
+
+      tmp_mot_cmd[i] = (int)(last_mot_val[i] * pulse_per_rad[i]);
+
+      mot_cmd[i]->unlock_atom();
+    }
+
+    control_mutex.lock();
+
+    printf("Commanding: %g -- %d\n", last_mot_val[1], tmp_mot_cmd[1]);
+
+    ed->drive(6,tmp_mot_cmd);
+
+    control_mutex.unlock();
+
+    int val[6];
+    
+    if (!ed->tick(6,val)) {
+      return false;
+    }
+
+    for (int i = 0; i < 6; i++) {
+      mot[i]->val = val[i] / pulse_per_rad[i];
+      mot[i]->rel = false;
+      mot[i]->valid = true;
+      mot[i]->publish();
+    }
+    return true;
+  }
+
+  void mot_callback() { }
+
+};
+
+int main(int argc, char **argv)
+{
+  EtherDrive_Control a;
+  while (a.happy()) {
+    if (!a.do_tick())
+    {
+      a.log(ROS::ERROR,"Etherdrive tick failed.");
+      break;
+    }
+  }
+  return 0;
+}
+

Deleted: pkg/trunk/etherdrive/test/Makefile
===================================================================
--- pkg/trunk/etherdrive/test/Makefile	2008-04-21 21:46:26 UTC (rev 148)
+++ pkg/trunk/etherdrive/test/Makefile	2008-04-21 21:48:44 UTC (rev 149)
@@ -1,4 +0,0 @@
-SRC = test_etherdrive.cpp
-OUT = ../bin/test_etherdrive
-PKG = etherdrive
-include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk

Added: pkg/trunk/etherdrive/test/test_etherdrive/Makefile
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive/Makefile	                        (rev 0)
+++ pkg/trunk/etherdrive/test/test_etherdrive/Makefile	2008-04-21 21:48:44 UTC (rev 149)
@@ -0,0 +1,4 @@
+SRC = test_etherdrive.cpp
+OUT = test_etherdrive
+PKG = etherdrive
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk

Added: pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp	                        (rev 0)
+++ pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp	2008-04-21 21:48:44 UTC (rev 149)
@@ -0,0 +1,80 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "etherdrive/etherdrive.h"
+#include <iostream>
+
+using namespace std;
+
+int main() {
+
+  EtherDrive e;
+  
+  if (!e.init("10.0.0.151")) {
+    cout << "Could not initialize etherdrive." << endl;
+    return -1;
+  }
+  e.motors_on();
+  
+  int drv = 100000;
+  int enc[2];
+  int cur[2];
+
+  int count = 0;
+
+  while (1) {
+    if (!e.drive(1,&drv))
+      printf("Drive problem!.");
+    if (!e.tick(2,enc,cur))
+// // //       printf("Tick problem!.");
+
+    printf("Encoder0: %d Current: %d\n", enc[0], cur[0]);
+    printf("Encoder1: %d Current: %d\n", enc[1], cur[1]);
+
+    if (abs(cur[0]) > 200) {
+      if (count++ > 50)
+	e.motors_off();
+    } else {
+      count = 0;
+    }
+
+    if (enc[0] == 100000) {
+      drv = -100000;
+    } else if (enc[0] == -100000) {
+      drv = 100000;
+    }
+  }
+
+  e.shutdown();
+}

Deleted: pkg/trunk/etherdrive/test/test_etherdrive.cpp
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive.cpp	2008-04-21 21:46:26 UTC (rev 148)
+++ pkg/trunk/etherdrive/test/test_etherdrive.cpp	2008-04-21 21:48:44 UTC (rev 149)
@@ -1,67 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-* 
-*  Copyright (c) 2008, Willow Garage, Inc.
-*  All rights reserved.
-* 
-*  Redistribution and use in source and binary forms, with or without
-*  modification, are permitted provided that the following conditions
-*  are met:
-* 
-*   * Redistributions of source code must retain the above copyright
-*     notice, this list of conditions and the following disclaimer.
-*   * Redistributions in binary form must reproduce the above
-*     copyright notice, this list of conditions and the following
-*     disclaimer in the documentation and/or other materials provided
-*     with the distribution.
-*   * Neither the name of the Willow Garage nor the names of its
-*     contributors may be used to endorse or promote products derived
-*     from this software without specific prior written permission.
-* 
-*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-*  POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include "etherdrive.h"
-#include <iostream>
-
-using namespace std;
-
-int main() {
-
-  EtherDrive e;
-  
-  if (!e.init("10.0.0.151")) {
-    cout << "Could not initialize etherdrive." << endl;
-    return -1;
-  }
-  e.motors_on();
-  
-  int drv = 100000;
-  int enc;
-
-  while (1) {
-    e.drive(1,&drv);
-    e.tick(1,&enc);
-
-    printf("Encoder: %d\n", enc);
-
-    if (enc == 100000) {
-      drv = -100000;
-    } else if (enc == -100000) {
-      drv = 100000;
-    }
-  }
-
-  e.shutdown();
-}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.




Thread: [Personalrobots-commit] SF.net SVN: personalrobots: [149] pkg/trunk/etherdrive

personalrobots-commit