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

[<jl...@us...>]

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.