Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [149] pkg/trunk/etherdrive
|
From: <jl...@us...> - 2008-04-21 21:48:57
|
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.
|
