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

[<rob...@us...>]

Revision: 5283
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5283&view=rev
Author:   rob_wheeler
Date:     2008-10-11 01:19:33 +0000 (Sat, 11 Oct 2008)

Log Message:
-----------
Add service to reset MCBs after a safety lockout

Modified Paths:
--------------
    pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
    pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
    pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp

Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h	2008-10-11 01:01:59 UTC (rev 5282)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h	2008-10-11 01:19:33 UTC (rev 5283)
@@ -64,7 +64,7 @@
   /*!
    * \brief update send most recent motor commands and retrieve updates. This command must be run at a sufficient rate or else the motors will be disabled.
    */
-  void update();
+  void update(bool reset);
 
   /*!
    * \brief Initialize the EtherCAT Master Library.

Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp	2008-10-11 01:01:59 UTC (rev 5282)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp	2008-10-11 01:19:33 UTC (rev 5283)
@@ -257,7 +257,7 @@
   }
 }
 
-void EthercatHardware::update()
+void EthercatHardware::update(bool reset)
 {
   unsigned char *current, *last;
 
@@ -273,7 +273,14 @@
       hw_->actuators_[a]->state_.last_requested_effort_ = hw_->actuators_[a]->command_.effort_;
       hw_->actuators_[a]->state_.last_requested_current_ = hw_->actuators_[a]->command_.current_;
       slaves_[s]->truncateCurrent(hw_->actuators_[a]->command_);
-      slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+      if (reset) {
+        bool tmp = hw_->actuators_[a]->command_.enable_;
+        hw_->actuators_[a]->command_.enable_ = false;
+        slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+        hw_->actuators_[a]->command_.enable_ = tmp;
+      } else {
+        slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+      }
       current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
       ++a;
     }

Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp	2008-10-11 01:01:59 UTC (rev 5282)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp	2008-10-11 01:19:33 UTC (rev 5283)
@@ -74,7 +74,8 @@
   }
 }
 
-static int quit = 0;
+static int g_quit = 0;
+static bool g_reset_motors = false;
 static const int NSEC_PER_SEC = 1e+9;
 
 static struct
@@ -136,7 +137,7 @@
 
 static void *syncClocks(void *)
 {
-  while (!quit)
+  while (!g_quit)
   {
     struct timespec ts;
     struct timeval tv;
@@ -208,10 +209,18 @@
   int period = 1e+6; // 1 ms in nanoseconds
 
   static int count = 0;
-  while (!quit)
+  while (!g_quit)
   {
     double start = now();
-    ec.update();
+    if (g_reset_motors)
+    {
+      ec.update(true);
+      g_reset_motors = false;
+    }
+    else
+    {
+      ec.update(false);
+    }
     double after_ec = now();
     mcn.update();
     double after_mc = now();
@@ -240,7 +249,7 @@
     ec.hw_->actuators_[i]->command_.enable_ = false;
     ec.hw_->actuators_[i]->command_.effort_ = 0;
   }
-  ec.update();
+  ec.update(false);
 
   publisher.stop();
 
@@ -254,7 +263,7 @@
 
 void quitRequested(int sig)
 {
-  quit = 1;
+  g_quit = 1;
 }
 
 class Shutdown {
@@ -266,6 +275,15 @@
   }
 };
 
+class Reset {
+public:
+  bool resetMotorsService(std_srvs::Empty::request &req, std_srvs::Empty::response &resp)
+  {
+    g_reset_motors = true;
+    return true;
+  }
+};
+
 void warnOnSecondary(int sig)
 {
   void *bt[32];
@@ -356,6 +374,7 @@
   pthread_attr_setschedpolicy(&rtThreadAttr, SCHED_FIFO);
 
   node->advertise_service("shutdown", &Shutdown::shutdownService);
+  node->advertise_service("reset_motors", &Reset::resetMotorsService);
 
   //Start thread
   int rv;


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