|
From: <jl...@us...> - 2008-04-25 19:43:58
|
Revision: 178
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=178&view=rev
Author: jleibs
Date: 2008-04-25 12:43:55 -0700 (Fri, 25 Apr 2008)
Log Message:
-----------
A little bit of clean up to the ethedrive class.
Modified Paths:
--------------
pkg/trunk/etherdrive/include/etherdrive/etherdrive.h
pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp
pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp
Added Paths:
-----------
pkg/trunk/etherdrive/test/test_etherdrive2/
pkg/trunk/etherdrive/test/test_etherdrive2/Makefile
pkg/trunk/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp
Modified: pkg/trunk/etherdrive/include/etherdrive/etherdrive.h
===================================================================
--- pkg/trunk/etherdrive/include/etherdrive/etherdrive.h 2008-04-25 00:56:12 UTC (rev 177)
+++ pkg/trunk/etherdrive/include/etherdrive/etherdrive.h 2008-04-25 19:43:55 UTC (rev 178)
@@ -43,6 +43,8 @@
using namespace std;
+class EDMotor;
+
class EtherDrive
{
public:
@@ -63,15 +65,28 @@
// Disable motors
bool motors_off();
+ EDMotor get_motor(uint32_t m);
+
+ void set_drv(uint32_t m, int32_t drv);
+ int32_t get_enc(uint32_t m);
+ int32_t get_cur(uint32_t m);
+ int32_t get_pwm(uint32_t m);
+
// 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];
+ int32_t last_enc[6];
+ int32_t last_cur[6];
+ int32_t last_pwm[6];
int mot_sock;
int cmd_sock;
@@ -80,5 +95,34 @@
struct sockaddr_in cmd_addr_out;
};
+class EDMotor
+{
+ friend class EtherDrive;
+public:
+ void set_drv(int32_t drv) {
+ driver->set_drv(motor, drv);
+ }
+
+ int32_t get_enc() {
+ return driver->get_enc(motor);
+ }
+
+ int32_t get_cur() {
+ return driver->get_cur(motor);
+ }
+
+ int32_t get_pwm() {
+ return driver->get_pwm(motor);
+ }
+protected:
+ EDMotor(EtherDrive* driver, uint32_t motor) : driver(driver), motor(motor) {}
+
+private:
+ EtherDrive* driver;
+
+ uint32_t motor;
+};
+
+
#endif
Modified: pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp
===================================================================
--- pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp 2008-04-25 00:56:12 UTC (rev 177)
+++ pkg/trunk/etherdrive/src/libetherdrive/etherdrive.cpp 2008-04-25 19:43:55 UTC (rev 178)
@@ -48,6 +48,11 @@
}
+EtherDrive::~EtherDrive()
+{
+ shutdown();
+}
+
bool EtherDrive::init(string ip) {
if (ready) {
@@ -94,12 +99,6 @@
}
-EtherDrive::~EtherDrive()
-{
- shutdown();
-}
-
-
void EtherDrive::shutdown() {
if (ready = true) {
@@ -112,6 +111,26 @@
}
+int EtherDrive::send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len) {
+
+ int n_sent;
+ int n_recv;
+
+ struct sockaddr_in from;
+ socklen_t fromlen = sizeof(struct sockaddr_in);
+
+ n_sent = sendto(cmd_sock, cmd, cmd_len, 0, (struct sockaddr *)&cmd_addr_out, sizeof(mot_addr_out));
+
+ if (n_sent != cmd_len) {
+ return -1;
+ }
+
+ n_recv = recvfrom(cmd_sock, buf, buf_len, 0, (struct sockaddr *)&from, &fromlen);
+
+ return n_recv;
+
+}
+
bool EtherDrive::motors_on() {
if (ready) {
@@ -159,45 +178,36 @@
}
-int EtherDrive::send_cmd(char* cmd, size_t cmd_len, char* buf, size_t buf_len) {
+EDMotor EtherDrive::get_motor(uint32_t m) {
+ return EDMotor(this, m);
+}
- int n_sent;
- int n_recv;
-
- struct sockaddr_in from;
- socklen_t fromlen = sizeof(struct sockaddr_in);
-
- n_sent = sendto(cmd_sock, cmd, cmd_len, 0, (struct sockaddr *)&cmd_addr_out, sizeof(mot_addr_out));
-
- if (n_sent != cmd_len) {
- return -1;
+void EtherDrive::set_drv(uint32_t m, int32_t drv) {
+ if (m < 6) {
+ last_drv[m] = drv;
}
-
- n_recv = recvfrom(cmd_sock, buf, buf_len, 0, (struct sockaddr *)&from, &fromlen);
-
- return n_recv;
-
}
-
-bool EtherDrive::drive(size_t num, int32_t* drv)
-{
- if (num > 6) {
- return false;
+int32_t EtherDrive::get_enc(uint32_t m) {
+ if (m < 6) {
+ return last_enc[m];
}
+}
- for (int i = 0; i < num; i++) {
- last_drv[i] = drv[i];
+int32_t EtherDrive::get_cur(uint32_t m) {
+ if (m < 6) {
+ return last_enc[m];
}
+}
- for (int i = num; i < 6; i++) {
- last_drv[i] = 0;
+int32_t EtherDrive::get_pwm(uint32_t m) {
+ if (m < 6) {
+ return last_pwm[m];
}
-
- return true;
}
-bool EtherDrive::tick(size_t num, int32_t* enc, int32_t* curr, int32_t* pwm)
+
+bool EtherDrive::tick(size_t num, int32_t* enc, int32_t* cur, int32_t* pwm)
{
if (num > 6) {
@@ -224,23 +234,47 @@
return false;
}
+
+ for (int i = 0; i < 6; i++) {
+ last_enc[i] = buf[i];
+ }
+ for (int i = 0; i < 6; i++) {
+ last_cur[i] = buf[6+i];
+ }
+ for (int i = 0; i < 6; i++) {
+ last_pwm[i] = buf[6+i];
+ }
+
+
if (enc > 0) {
for (int i = 0; i < num; i++) {
- enc[i] = buf[i];
+ enc[i] = last_enc[i];
}
}
-
- if (curr > 0) {
+ if (cur > 0) {
for (int i = 0; i < num; i++) {
- curr[i] = buf[6+i];
+ cur[i] = last_cur[i];
}
}
-
if (pwm > 0) {
for (int i = 0; i < num; i++) {
- pwm[i] = buf[12+i];
+ pwm[i] = last_pwm[i];
}
}
return n_recv;
}
+
+bool EtherDrive::drive(size_t num, int32_t* drv)
+{
+ if (num > 6) {
+ return false;
+ }
+
+ for (int i = 0; i < num; i++) {
+ last_drv[i] = drv[i];
+ }
+
+ return true;
+}
+
Modified: pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp 2008-04-25 00:56:12 UTC (rev 177)
+++ pkg/trunk/etherdrive/test/test_etherdrive/test_etherdrive.cpp 2008-04-25 19:43:55 UTC (rev 178)
@@ -57,7 +57,7 @@
if (!e.drive(1,&drv))
printf("Drive problem!.");
if (!e.tick(2,enc,cur))
-// // // printf("Tick problem!.");
+ printf("Tick problem!.");
printf("Encoder0: %d Current: %d\n", enc[0], cur[0]);
printf("Encoder1: %d Current: %d\n", enc[1], cur[1]);
Added: pkg/trunk/etherdrive/test/test_etherdrive2/Makefile
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive2/Makefile (rev 0)
+++ pkg/trunk/etherdrive/test/test_etherdrive2/Makefile 2008-04-25 19:43:55 UTC (rev 178)
@@ -0,0 +1,4 @@
+SRC = test_etherdrive2.cpp
+OUT = test_etherdrive2
+PKG = etherdrive
+include $(shell $(ROS_ROOT)/rospack find roscpp)/make_include/node.mk
Added: pkg/trunk/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp
===================================================================
--- pkg/trunk/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp (rev 0)
+++ pkg/trunk/etherdrive/test/test_etherdrive2/test_etherdrive2.cpp 2008-04-25 19:43:55 UTC (rev 178)
@@ -0,0 +1,85 @@
+/*********************************************************************
+* 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;
+ }
+
+ EDMotor m0 = e.get_motor(0);
+ EDMotor m1 = e.get_motor(1);
+
+ e.motors_on();
+
+ int count = 0;
+
+ int drv = 100000;
+
+ while (1) {
+
+ m0.set_drv(drv);
+
+ if (!e.tick())
+ printf("Tick problem!.");
+
+ printf("Encoder0: %d Current: %d\n", m0.get_enc(), m0.get_cur());
+ printf("Encoder1: %d Current: %d\n", m1.get_enc(), m1.get_cur());
+
+
+ // Crappy control
+ if (abs(m0.get_cur()) > 200) {
+ if (count++ > 50)
+ e.motors_off();
+ } else {
+ count = 0;
+ }
+
+ if (m0.get_enc() == 100000) {
+ drv = -100000;
+ } else if (m0.get_enc() == -100000) {
+ drv = 100000;
+ }
+ }
+
+ e.shutdown();
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|