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

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

Revision: 5468
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5468&view=rev
Author:   rob_wheeler
Date:     2008-10-17 00:38:00 +0000 (Fri, 17 Oct 2008)

Log Message:
-----------
Bringup motor control boards more slowly (one per ms)

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-17 00:34:45 UTC (rev 5467)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h	2008-10-17 00:38:00 UTC (rev 5468)
@@ -99,6 +99,9 @@
   unsigned char *buffers_;
   unsigned int buffer_size_;
 
+  bool halt_motors_;
+  unsigned int reset_state_;
+
   misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
   struct {
     struct {

Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp	2008-10-17 00:34:45 UTC (rev 5467)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp	2008-10-17 00:38:00 UTC (rev 5468)
@@ -40,7 +40,7 @@
 #include <ros/node.h>
 
 EthercatHardware::EthercatHardware() :
-  hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0), publisher_("/diagnostics", 1)
+  hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0), reset_state_(0), publisher_("/diagnostics", 1)
 {
 }
 
@@ -265,27 +265,40 @@
 
   // Convert HW Interface commands to MCB-specific buffers
   current = current_buffer_;
+  memset(current, 0, buffer_size_);
+
+  if (reset)
+  {
+    reset_state_ = num_slaves_;
+  }
+
   for (unsigned int s = 0, a = 0; s < num_slaves_; ++s)
   {
     if (slaves_[s]->has_actuator_)
     {
-      slaves_[s]->computeCurrent(hw_->actuators_[a]->command_);
-      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_);
-      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;
+      Actuator *act = hw_->actuators_[a];
+      slaves_[s]->computeCurrent(act->command_);
+      act->state_.last_requested_effort_ = act->command_.effort_;
+      act->state_.last_requested_current_ = act->command_.current_;
+      slaves_[s]->truncateCurrent(act->command_);
+      if (reset_state_ && s < reset_state_)
+      {
+        bool tmp = act->command_.enable_;
+        act->command_.enable_ = false;
+        act->command_.current_ = 0;
+        slaves_[s]->convertCommand(act->command_, current);
+        act->command_.enable_ = tmp;
       } else {
-        slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+        slaves_[s]->convertCommand(act->command_, current);
       }
       current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
       ++a;
     }
   }
 
+  if (reset_state_)
+    --reset_state_;
+
   // Transmit process data
   double start = now();
   em_->txandrx_PD(buffer_size_, current_buffer_);
@@ -298,8 +311,9 @@
   {
     if (slaves_[s]->has_actuator_)
     {
-      slaves_[s]->convertState(hw_->actuators_[a]->state_, current, last);
-      slaves_[s]->verifyState(hw_->actuators_[a]->state_, current, last);
+      Actuator *act = hw_->actuators_[a];
+      slaves_[s]->convertState(act->state_, current, last);
+      slaves_[s]->verifyState(act->state_, current, last);
       current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
       last += 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-17 00:34:45 UTC (rev 5467)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp	2008-10-17 00:38:00 UTC (rev 5468)
@@ -75,7 +75,7 @@
 }
 
 static int g_quit = 0;
-static bool g_reset_motors = false;
+static bool g_reset_motors = true;
 static const int NSEC_PER_SEC = 1e+9;
 
 static struct


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