Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2229] pkg/trunk
|
From: <rob...@us...> - 2008-07-29 02:04:41
|
Revision: 2229
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2229&view=rev
Author: rob_wheeler
Date: 2008-07-29 02:04:47 +0000 (Tue, 29 Jul 2008)
Log Message:
-----------
Checkpoint an example of using the controllers over EtherCAT.
Modified Paths:
--------------
pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h
pkg/trunk/controllers/genericControllers/src/JointController.cpp
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Added Paths:
-----------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h
pkg/trunk/mechanism/mechanism_control/src/single_control.cpp
Modified: pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h
===================================================================
--- pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -36,7 +36,7 @@
/***************************************************/
/*! \class controller::JointController
\brief A Joint controller
-
+
This class implements controller loops for
PR2 Joint Control
@@ -62,7 +62,7 @@
4. Set appropriate command (position, torque, velocity)
5. Call Update from Real Time loop
-
+
*/
/***************************************************/
#include <sys/types.h>
@@ -88,7 +88,7 @@
class JointController : Controller
{
- static const double AccelerationThreshold = 0.1; //Distance threshold below which linear acceleration limit is active, if enabled
+ static const double AccelerationThreshold = 0.1; //Distance threshold below which linear acceleration limit is active, if enabled
public:
/*!
@@ -96,10 +96,10 @@
*
*/
JointController();
-
+
/*!
* \brief Destructor of the JointController class.
- */
+ */
~JointController( );
static Controller* create(const std::string&) {
@@ -111,16 +111,16 @@
* \param Joint* joint The joint we are interacting with
* \param string name The namespace identification in ROS
*/
- // controller::controllerErrorCode Init(Joint* joint, string name);
+ // controller::controllerErrorCode Init(Joint* joint, string name);
// JointController(Joint* joint, string name);
- /*!
+ /*!
* \brief Functional way to initialize limits and gains.
*
*/
void init(pidControlParam pcp, controllerControlMode mode, double time, double maxEffort, double minEffort, mechanism::Joint *joint);
- /*!
+ /*!
* \brief Functional way to initialize limits and gains.
*
*/
@@ -132,10 +132,10 @@
* \param string name The namespace identification in ROS
*/
// controller::controllerErrorCode Init(mechanism::Joint* joint, string name);
-
+
// JointController(Joint* joint, string name);
- /*!
+ /*!
* \brief Functional way to initialize limits and gains.
*
*/
@@ -143,19 +143,19 @@
/*!
* \brief TODO: Get the actual time
- *
*
- * \param double* time Pointer to value to change
+ *
+ * \param double* time Pointer to value to change
*/
void getTime(double* time);
-
+
//---------------------------------------------------------------------------------//
//MODE/ENABLE CALLS
//---------------------------------------------------------------------------------//
-
+
/*!
* \brief Switches command mode type (Torque, position, velocity control)
- *
+ *
*/
controllerControlMode setMode(controller::controllerControlMode mode);
@@ -176,9 +176,9 @@
controller::controllerControlMode disableController();
/*!
- * \brief Return true if last command saturated the torque
+ * \brief Return true if last command saturated the torque
*
- *
+ *
*/
bool checkForSaturation(void);
@@ -191,19 +191,19 @@
* \param torque Torque command to issue
*/
controller::controllerErrorCode setTorqueCmd(double torque);
-
+
/*!
- * \brief Fetch the latest user issued torque command
- *
- * \param double* torque Pointer to value to change
- */
+ * \brief Fetch the latest user issued torque command
+ *
+ * \param double* torque Pointer to value to change
+ */
controller::controllerErrorCode getTorqueCmd(double *torque);
-
+
/*!
* \brief Get the actual torque of the joint motor.
- *
+ *
* \param double* torque Pointer to value to change
- */
+ */
controller::controllerErrorCode getTorqueAct(double *torque);
//---------------------------------------------------------------------------------//
@@ -212,45 +212,45 @@
/*!
* \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
- *
+ *
* \param double pos Position command to issue
- */
+ */
controller::controllerErrorCode setPosCmd(double pos);
-
+
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
* \param double* pos Pointer to value to change
- */
+ */
controller::controllerErrorCode getPosCmd(double *pos);
-
+
/*!
* \brief Read the torque of the motor
* \param double* pos Pointer to value to change
- */
- controller::controllerErrorCode getPosAct(double *pos);
+ */
+ controller::controllerErrorCode getPosAct(double *pos);
//---------------------------------------------------------------------------------//
//VELOCITY CALLS
//---------------------------------------------------------------------------------//
-
+
/*!
* \brief Set velocity command to the joint to be issue next update
* \param double vel Velocity to issue next command
*/
controller::controllerErrorCode setVelCmd(double vel);
-
+
/*!
* \brief Get latest velocity command to the joint
* \param double* vel Pointer to value to change
*/
controller::controllerErrorCode getVelCmd(double *vel);
-
+
/*!
* \brief Get actual velocity of the joint
* \param double* vel Pointer to value to change
*/
controller::controllerErrorCode getVelAct(double *vel);
-
+
/*!
* \brief Compute max velocity coming into the end stop to stop with linear velocity in endstop.
*
@@ -263,12 +263,14 @@
/*!
* \brief Issues commands to joint based on control mode
*
- *
+ *
*/
//Issues commands to the joint. Should be called at regular intervals
virtual void update(void);
+ virtual void update(double current_time);
+
//---------------------------------------------------------------------------------//
//PARAM SERVER CALLS
//---------------------------------------------------------------------------------//
@@ -280,7 +282,7 @@
* constraints for this controller
* \param string label Name of param to change
* \param double value New value
- *<br>
+ *<br>
*<UL TYPE="none">
*<LI> e.g. SetParam('PGain',10);
*<LI> or SetParam('IGain',10);
@@ -298,7 +300,7 @@
* user can get maximum velocity
* and maximum acceleration
* constraints for this controller
- *<br>
+ *<br>
*<UL TYPE="none">
*<UL TYPE="none">
*<LI> e.g. GetParam('PGain',value);
@@ -310,9 +312,9 @@
*/
controller::controllerErrorCode getParam(std::string label, double* value);
//controller::controllerErrorCode GetParam(std::string label, std::string value);
-
+
/*!
- * \brief Set the name of the controller
+ * \brief Set the name of the controller
*
* \param name - std::string representation of the name of the controller
*/
@@ -341,13 +343,13 @@
/*!
* \brief Actually issue torque set command of the joint motor.
- */
+ */
void setJointEffort(double torque);
-
- mechanism::Joint* joint; /*!< Joint we're controlling>*/
- Pid pidController; /*!< Internal PID controller>*/
+ mechanism::Joint* joint; /*!< Joint we're controlling>*/
+ Pid pidController; /*!< Internal PID controller>*/
+
double lastTime;/*!< Last time stamp of update> */
double cmdTorque;/*!< Last commanded torque>*/
@@ -356,11 +358,11 @@
double cmdVel;/*!< Last commanded Velocity> */
- bool saturationFlag; /*!< Flag to indicate last command exceed torque limits and was truncated>*/
+ bool saturationFlag; /*!< Flag to indicate last command exceed torque limits and was truncated>*/
bool enabled; /*!<Can controller issue commands?>*/
- controller::controllerControlMode controlMode; /*!< Indicate current controller mode (torque, position, velocity)>*/
+ controller::controllerControlMode controlMode; /*!< Indicate current controller mode (torque, position, velocity)>*/
double pGain; /*!< Proportional gain>*/
@@ -382,7 +384,7 @@
void loadParam(std::string label, int &value);
- std::string name; /*!< Namespace ID for this controller>*/
+ std::string name; /*!< Namespace ID for this controller>*/
};
}
Modified: pkg/trunk/controllers/genericControllers/src/JointController.cpp
===================================================================
--- pkg/trunk/controllers/genericControllers/src/JointController.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/controllers/genericControllers/src/JointController.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -37,45 +37,45 @@
#include <sys/time.h>
#define DEFAULTMAXACCEL 5
//#define DEBUG 1
-//Todo:
+//Todo:
//1. Get and set params via server
//2. Integrate Joint and robot objects
//3. Integrate Motor controller time
using namespace controller;
-
+
//---------------------------------------------------------------------------------//
//CONSTRUCTION/DESTRUCTION CALLS
//---------------------------------------------------------------------------------//
JointController::JointController()
-{
+{
//Instantiate PID class
- pidController.InitPid(0,0,0,0,0); //Constructor for pid controller
+ pidController.InitPid(0,0,0,0,0); //Constructor for pid controller
//Set commands to zero
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
controlMode = controller::CONTROLLER_DISABLED;
}
JointController::~JointController( )
{
-
+
}
void JointController::init(double pGain, double iGain, double dGain, double windupMax, double windupMin, controllerControlMode mode, double time, double maxEffort, double minEffort, mechanism::Joint *joint) {
- pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
+ pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
//Init time
lastTime = time;
@@ -87,7 +87,7 @@
this->windupMin = windupMin;
this->minEffort = minEffort;
- this->maxEffort = maxEffort;
+ this->maxEffort = maxEffort;
this->joint = joint;
controlMode = mode;
@@ -96,12 +96,12 @@
void JointController::init(pidControlParam pcp, controllerControlMode mode, double time, double maxEffort, double minEffort, Joint *joint) {
- pidController.InitPid(pcp.pGain,pcp.iGain,pcp.dGain,pcp.windupMax,pcp.windupMin); //Constructor for pid controller
+ pidController.InitPid(pcp.pGain,pcp.iGain,pcp.dGain,pcp.windupMax,pcp.windupMin); //Constructor for pid controller
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
//Init time
lastTime = time;
@@ -113,7 +113,7 @@
this->windupMin = pcp.windupMin;
this->minEffort = minEffort;
- this->maxEffort = maxEffort;
+ this->maxEffort = maxEffort;
this->joint = joint;
@@ -123,12 +123,12 @@
void JointController::init(double time, Joint *joint) {
- pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
+ pidController.InitPid(pGain,iGain,dGain,windupMax,windupMin); //Constructor for pid controller
cmdTorque = 0;
cmdPos = 0;
cmdVel = 0;
-
+
lastTime = time;
this->joint = joint;
enableController();
@@ -176,7 +176,7 @@
return CONTROLLER_DISABLED;
}
-bool JointController::checkForSaturation(void){
+bool JointController::checkForSaturation(void){
return saturationFlag;
}
@@ -187,12 +187,12 @@
//---------------------------------------------------------------------------------//
controllerErrorCode JointController::setTorqueCmd(double torque){
// double maxEffort = joint->effortLimit;
-
+
if(controlMode != CONTROLLER_TORQUE) //Make sure we're in torque command mode
return CONTROLLER_MODE_ERROR;
-
- cmdTorque = torque;
-
+
+ cmdTorque = torque;
+
if(cmdTorque >= maxEffort){ //Truncate to positive limit
cmdTorque = maxEffort;
return CONTROLLER_TORQUE_LIMIT;
@@ -201,7 +201,7 @@
cmdTorque = minEffort;
return CONTROLLER_TORQUE_LIMIT;
}
-
+
return CONTROLLER_CMD_SET;
}
@@ -214,7 +214,7 @@
controllerErrorCode JointController::getTorqueAct(double *torque)
{
*torque = joint->appliedEffort; //Read torque from joint
- return CONTROLLER_ALL_OK;
+ return CONTROLLER_ALL_OK;
}
//---------------------------------------------------------------------------------//
@@ -222,13 +222,13 @@
//---------------------------------------------------------------------------------//
-//Query mode, then set desired position
+//Query mode, then set desired position
controllerErrorCode JointController::setPosCmd(double pos)
{
if(controlMode != CONTROLLER_POSITION) //Make sure we're in position command mode
return CONTROLLER_MODE_ERROR;
-
- cmdPos = pos;
+
+ cmdPos = pos;
if(cmdPos >= joint->jointLimitMax && joint->type != mechanism::JOINT_CONTINUOUS){ //Truncate to positive limit
cmdPos = joint->jointLimitMax;
return CONTROLLER_JOINT_LIMIT;
@@ -237,21 +237,21 @@
cmdPos = joint->jointLimitMin;
return CONTROLLER_JOINT_LIMIT;
}
- return CONTROLLER_CMD_SET;
+ return CONTROLLER_CMD_SET;
}
//Return the current position command
controller::controllerErrorCode JointController::getPosCmd(double *pos)
{
*pos = cmdPos;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
//Query the joint for the actual position
controller::controllerErrorCode JointController::getPosAct(double *pos)
{
*pos = joint->position;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
//---------------------------------------------------------------------------------//
@@ -261,7 +261,7 @@
controllerErrorCode JointController::setVelCmd(double vel)
{
if(controlMode == CONTROLLER_VELOCITY || controlMode == ETHERDRIVE_SPEED){ //Make sure we're in velocity command mode
- cmdVel = vel;
+ cmdVel = vel;
return CONTROLLER_CMD_SET;
}
else return CONTROLLER_MODE_ERROR;
@@ -271,14 +271,14 @@
controller::controllerErrorCode JointController::getVelCmd(double *vel)
{
*vel = cmdVel;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
//Query our joint for velocity
controller::controllerErrorCode JointController::getVelAct(double *vel)
{
*vel = joint->velocity;
- return controller::CONTROLLER_ALL_OK;
+ return controller::CONTROLLER_ALL_OK;
}
@@ -300,15 +300,14 @@
{
double error(0),time(0),currentTorqueCmd(0);
double maxVelocity = cmdVel;
- double currentVoltageCmd,v_backemf,v_clamp_min,v_clamp_max,k;
+ double currentVoltageCmd,v_backemf,v_clamp_min,v_clamp_max,k;
-
+
if(controlMode == controller::CONTROLLER_DISABLED){
printf("JointController.cpp: Error:: controller disabled\n");
return; //If we're not initialized, don't try to interact
}
getTime(&time); //TODO: Replace time with joint->timeStep
-
switch(controlMode)
{
case CONTROLLER_TORQUE: //Pass through torque command
@@ -316,8 +315,8 @@
break;
case CONTROLLER_POSITION: //Close the loop around position
- if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
- error = shortest_angular_distance(cmdPos, joint->position);
+ if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
+ error = shortest_angular_distance(cmdPos, joint->position);
else
error = joint->position - cmdPos;
currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
@@ -332,16 +331,17 @@
}
}
error = joint->velocity - cmdVel;
+
currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
break;
case ETHERDRIVE_SPEED: // Use hack to contol speed in voltage control mode for the etherdrive
#ifdef DEBUG
printf("JC:: %f\n",cmdVel);
#endif
- currentVoltageCmd = cmdVel*20*60/(136*2*M_PI);
+ currentVoltageCmd = cmdVel*20*60/(136*2*M_PI);
v_backemf = joint->velocity*20*60/(136*2*M_PI);
- v_clamp_min = v_backemf - 3;// 0.655*16.7;
+ v_clamp_min = v_backemf - 3;// 0.655*16.7;
v_clamp_max = v_backemf + 3;//0.655*16.7;
k = 1.0/ 36.0;
@@ -358,16 +358,83 @@
default:
printf("JointController.cpp: Error:: invalid controlMode\n");
- currentTorqueCmd = 0;
+ currentTorqueCmd = 0;
}
lastTime = time;
- setJointEffort(currentTorqueCmd);
+ setJointEffort(currentTorqueCmd);
}
+void JointController::update(double time)
+{
+ double error(0),currentTorqueCmd(0);
+ double maxVelocity = cmdVel;
+ double currentVoltageCmd,v_backemf,v_clamp_min,v_clamp_max,k;
+ if(controlMode == controller::CONTROLLER_DISABLED){
+ printf("JointController.cpp: Error:: controller disabled\n");
+ return; //If we're not initialized, don't try to interact
+ }
+
+ switch(controlMode)
+ {
+ case CONTROLLER_TORQUE: //Pass through torque command
+ currentTorqueCmd = cmdTorque;
+ break;
+
+ case CONTROLLER_POSITION: //Close the loop around position
+ if(joint->type == mechanism::JOINT_ROTARY || joint->type == mechanism::JOINT_CONTINUOUS)
+ error = shortest_angular_distance(cmdPos, joint->position);
+ else
+ error = joint->position - cmdPos;
+ currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
+ break;
+
+ case CONTROLLER_VELOCITY: //Close the loop around velocity
+ if(capAccel){
+ maxVelocity = getMaxVelocity(); //Check max velocity coming into wall
+ if(fabs(cmdVel)>maxVelocity){
+ cmdVel = -maxVelocity; //Truncate velocity smoothly
+ // std::cout<<"*******************"<<cmdVel<<std::endl;
+ }
+ }
+ error = joint->velocity - cmdVel;
+ currentTorqueCmd = pidController.UpdatePid(error,time-lastTime);
+ break;
+ case ETHERDRIVE_SPEED: // Use hack to contol speed in voltage control mode for the etherdrive
+#ifdef DEBUG
+ printf("JC:: %f\n",cmdVel);
+#endif
+ currentVoltageCmd = cmdVel*20*60/(136*2*M_PI);
+ v_backemf = joint->velocity*20*60/(136*2*M_PI);
+
+ v_clamp_min = v_backemf - 3;// 0.655*16.7;
+ v_clamp_max = v_backemf + 3;//0.655*16.7;
+
+ k = 1.0/ 36.0;
+#ifdef DEBUG
+ printf("JC::%f\t%f\t%f\n", v_clamp_min, currentVoltageCmd, v_clamp_max);
+#endif
+ if (currentVoltageCmd > v_clamp_max)
+ currentVoltageCmd = v_clamp_max;
+
+ if (currentVoltageCmd < v_clamp_min)
+ currentVoltageCmd = v_clamp_min;
+ currentTorqueCmd = currentVoltageCmd * k; //Convert to match PWM conversion inside boards
+ break;
+
+ default:
+ printf("JointController.cpp: Error:: invalid controlMode\n");
+ currentTorqueCmd = 0;
+ }
+ lastTime = time;
+
+ setJointEffort(currentTorqueCmd);
+}
+
+
controllerErrorCode JointController::setParam(std::string label,double value)
-{
+{
return CONTROLLER_ALL_OK;
}
@@ -381,7 +448,7 @@
void JointController::setJointEffort(double effort)
{
double newEffort;
-
+
newEffort = effort;
saturationFlag = false;
@@ -393,7 +460,7 @@
newEffort = minEffort;
saturationFlag = true;
}
- joint->commandedEffort = newEffort;
+ joint->commandedEffort = newEffort;
}
controllerErrorCode JointController::loadXML(std::string filename)
@@ -435,7 +502,7 @@
if(!exists)
return CONTROLLER_MODE_ERROR;
- paramMap = data.getDataTagValues("controller",this->name);
+ paramMap = data.getDataTagValues("controller",this->name);
loadParam("pGain",pGain);
loadParam("dGain",dGain);
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-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -76,10 +76,12 @@
MotorControlBoard *configSlave(EtherCAT_SlaveHandler *sh);
MotorControlBoard **slaves;
- unsigned int numSlaves;
+ unsigned int num_slaves_;
- unsigned char *buffer;
- unsigned int bufferSize;
+ unsigned char *current_buffer_;
+ unsigned char *last_buffer_;
+ unsigned char *buffers_;
+ unsigned int buffer_size_;
};
#endif /* ETHERCAT_HARDWARE_H */
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1000.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -48,9 +48,13 @@
void convertCommand(ActuatorCommand &command, unsigned char *buffer)
{
}
- void convertState(ActuatorState &state, unsigned char *buffer)
+ void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer)
{
}
+ bool hasActuator(void)
+ {
+ return false;
+ }
private:
static const EC_UDINT MK1000_PRODUCT_CODE = 0x000003E8;
};
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/mk1001.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -82,7 +82,11 @@
}
void configure(int &startAddress, EtherCAT_SlaveHandler *sh);
void convertCommand(ActuatorCommand &command, unsigned char *buffer);
- void convertState(ActuatorState &state, unsigned char *buffer);
+ void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer);
+ bool hasActuator(void)
+ {
+ return true;
+ }
private:
static const EC_UDINT MK1001_PRODUCT_CODE = 0x000003E9;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -55,7 +55,8 @@
}
virtual void configure(int &startAddress, EtherCAT_SlaveHandler *sh) = 0;
virtual void convertCommand(ActuatorCommand &command, unsigned char *buffer) = 0;
- virtual void convertState(ActuatorState &state, unsigned char *buffer) = 0;
+ virtual void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer) = 0;
+ virtual bool hasActuator(void) = 0;
EC_UDINT productCode;
unsigned int commandSize;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -94,7 +94,11 @@
}
void configure(int &startAddress, EtherCAT_SlaveHandler *sh);
void convertCommand(ActuatorCommand &command, unsigned char *buffer);
- void convertState(ActuatorState &state, unsigned char *buffer);
+ void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer);
+ bool hasActuator(void)
+ {
+ return true;
+ }
private:
static const EC_UDINT WG05_PRODUCT_CODE = 0x57473035;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -39,7 +39,7 @@
#include <dll/ethercat_dll.h>
EthercatHardware::EthercatHardware() :
- hw(0), ni(0), buffer(0), bufferSize(0)
+ hw(0), ni(0), current_buffer_(0), last_buffer_(0), buffer_size_(0)
{
}
@@ -49,10 +49,14 @@
{
close_socket(ni);
}
- if (buffer)
+ if (current_buffer_)
{
- delete buffer;
+ delete current_buffer_;
}
+ if (last_buffer_)
+ {
+ delete last_buffer_;
+ }
if (hw)
{
delete hw;
@@ -61,23 +65,6 @@
void EthercatHardware::init(char *interface, char *configuration)
{
- // Determine configuration from XML file 'configuration'
- // Create HardwareInterface
-
- // XXXwheeler: replace with XML parser
- hw = new HardwareInterface(5);
-
- hw->actuator[0].command.enable = true;
- hw->actuator[0].command.current = 0;
- hw->actuator[1].command.enable = true;
- hw->actuator[1].command.current = 0.0;
- hw->actuator[2].command.enable = true;
- hw->actuator[2].command.current = 0.0;
- hw->actuator[3].command.enable = true;
- hw->actuator[3].command.current = 0;
- hw->actuator[4].command.enable = true;
- hw->actuator[4].command.current = 0;
-
// Initialize network interface
if ((ni = init_ec(interface)) == NULL)
{
@@ -109,6 +96,7 @@
slaves = new MotorControlBoard*[numSlaves];
+ unsigned int num_actuators = 0;
for (unsigned int slave = 0; slave < numSlaves; ++slave)
{
EC_FixedStationAddress fsa(slave + 1);
@@ -121,39 +109,51 @@
if ((slaves[slave] = configSlave(sh)) != NULL)
{
- bufferSize += slaves[slave]->commandSize + slaves[slave]->statusSize;
+ num_actuators += slaves[slave]->hasActuator();
+ buffer_size_ += slaves[slave]->commandSize + slaves[slave]->statusSize;
if (!sh->to_state(EC_OP_STATE))
{
perror("to_state");
}
}
}
- buffer = new unsigned char[bufferSize];
+ buffers_ = new unsigned char[2 * buffer_size_];
+ current_buffer_ = buffers_;
+ last_buffer_ = buffers_ + buffer_size_;
+
+ // Determine configuration from XML file 'configuration'
+ // Create HardwareInterface
+ hw = new HardwareInterface(num_actuators);
}
void EthercatHardware::update()
{
- unsigned char *p;
+ unsigned char *current, *last;
// Convert HW Interface commands to MCB-specific buffers
- p = buffer;
+ current = current_buffer_;
for (int i = 0; i < hw->numActuators; ++i)
{
- slaves[i]->convertCommand(hw->actuator[i].command, p);
- p += slaves[i]->commandSize + slaves[i]->statusSize;
+ slaves[i]->convertCommand(hw->actuator[i].command, current);
+ current += slaves[i]->commandSize + slaves[i]->statusSize;
}
// Transmit process data
- em->txandrx_PD(bufferSize, buffer);
+ em->txandrx_PD(buffer_size_, current_buffer_);
// Convert status back to HW Interface
- p = buffer;
+ current = current_buffer_;
+ last = last_buffer_;
for (int i = 0; i < hw->numActuators; ++i)
{
- slaves[i]->convertState(hw->actuator[i].state, p);
- p += slaves[i]->commandSize + slaves[i]->statusSize;
+ slaves[i]->convertState(hw->actuator[i].state, current, last);
+ current += slaves[i]->commandSize + slaves[i]->statusSize;
+ last += slaves[i]->commandSize + slaves[i]->statusSize;
+ }
- }
+ unsigned char *tmp = current_buffer_;
+ current_buffer_ = last_buffer_;
+ last_buffer_ = tmp;
}
MotorControlBoard *
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -106,7 +106,7 @@
memcpy(buffer + sizeof(MK1001Status), &c, sizeof(c));
}
-void MK1001::convertState(ActuatorState &state, unsigned char *buffer)
+void MK1001::convertState(ActuatorState &state, unsigned char *buffer, unsigned char *not_used)
{
MK1001Status s;
MK1001Command c;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -101,28 +101,55 @@
memcpy(buffer + sizeof(WG05Status), &c, sizeof(c));
}
-void WG05::convertState(ActuatorState &state, unsigned char *buffer)
+void WG05::convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer)
{
- WG05Status s;
- WG05Command c;
+ WG05Status current_status, last_status;
+ WG05Command current_command, last_command;
- memcpy(&s, buffer, sizeof(s));
- memcpy(&c, buffer + sizeof(s), sizeof(c));
- state.encoderCount = s.encoderCount;
- state.encoderVelocity = 0;//delta count / delta time (in seconds);
- state.calibrationReading = s.calibrationReading;
- state.lastCalibrationHighTransition = s.lastCalibrationHighTransition;
- state.lastCalibrationLowTransition = s.lastCalibrationLowTransition;
- state.isEnabled = s.mode != MODE_OFF;
- state.runStopHit = (s.mode & MODE_UNDERVOLTAGE) != 0;
+ memcpy(¤t_status, current_buffer, sizeof(current_status));
+ memcpy(¤t_command, current_buffer + sizeof(current_status), sizeof(current_command));
- state.lastRequestedCurrent = c.programmedCurrent / CURRENT_FACTOR; // Should be actual request, before safety
- state.lastCommandedCurrent = s.programmedCurrent / CURRENT_FACTOR;
- state.lastMeasuredCurrent = s.measuredCurrent / CURRENT_FACTOR;
+ memcpy(&last_status, last_buffer, sizeof(last_status));
+ memcpy(&last_command, last_buffer + sizeof(last_status), sizeof(last_command));
- state.numEncoderErrors = s.numEncoderErrors;
- state.numCommunicationErrors = s.pdiTimeoutErrorCount + s.pdiChecksumErrorCount;
+ state.encoderCount = current_status.encoderCount;
+ state.encoderVelocity = double(int(current_status.encoderCount - last_status.encoderCount)) / (current_status.timestamp - last_status.timestamp) * 1e+6;
+ state.calibrationReading = current_status.calibrationReading;
+ state.lastCalibrationHighTransition = current_status.lastCalibrationHighTransition;
+ state.lastCalibrationLowTransition = current_status.lastCalibrationLowTransition;
+ state.isEnabled = current_status.mode != MODE_OFF;
+ state.runStopHit = (current_status.mode & MODE_UNDERVOLTAGE) != 0;
- state.motorVoltage = s.motorVoltage;
+ state.lastRequestedCurrent = current_command.programmedCurrent / CURRENT_FACTOR; // Should be actual request, before safety
+ state.lastCommandedCurrent = current_status.programmedCurrent / CURRENT_FACTOR;
+ state.lastMeasuredCurrent = current_status.measuredCurrent / CURRENT_FACTOR;
+
+ state.numEncoderErrors = current_status.numEncoderErrors;
+ state.numCommunicationErrors = current_status.pdiTimeoutErrorCount + current_status.pdiChecksumErrorCount;
+
+ state.motorVoltage = current_status.motorVoltage;
+#if 1
+{
+static int times = 0;
+
+if (++times % 2000 == 0) {
+ printf("-----------------------------------------------------\n");
+ printf("timestamp = %#08x\n", current_status.timestamp);
+ printf("encoderCount = %#08x\n", current_status.encoderCount);
+ printf("last timestamp = %#08x\n", last_status.timestamp);
+ printf("last encoderCount = %#08x\n", last_status.encoderCount);
+ printf("delta encoderCount = %#08x %f\n", current_status.encoderCount - last_status.encoderCount, double(int(current_status.encoderCount - last_status.encoderCount)));
+ printf("delta timestamp = %#08x %f\n", current_status.timestamp - last_status.timestamp, double(current_status.timestamp - last_status.timestamp));
+ printf("encoderVelocity = %f\n", state.encoderVelocity);
+ printf("programmedCurrent = %#08x\n", current_status.programmedCurrent);
+ printf("measuredCurrent = %#08x\n", current_status.measuredCurrent);
+ printf("mode = %#08x\n", current_status.mode);
+ printf("Kp = %#08x\n", int(current_status.currentLoopKp));
+ printf("Ki = %#08x\n", int(current_status.currentLoopKi));
}
+}
+#endif
+}
+
+
Modified: pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-07-29 02:04:47 UTC (rev 2229)
@@ -4,5 +4,6 @@
#rospack_add_executable(mechanismControl src/base_control.cpp)
#target_link_libraries(mechanism_control etherdrive_hardware pr2Controllers mechanism_model)
rospack_add_library(mechanism_control src/mechanism_control.cpp src/base_control.cpp)
+rospack_add_library(single_control src/single_control.cpp)
rospack_add_library(gazebo_mechanism_control src/gazebo_control.cpp) # src/controllerFactory.cpp)
Added: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/single_control.h 2008-07-29 02:04:47 UTC (rev 2229)
@@ -0,0 +1,80 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Willow Garage, Inc.
+//
+// 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 Willow Garage, Inc. 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 SINGLE_CONTROL_H
+#define SINGLE_CONTROL_H
+
+#include <mechanism_model/robot.h>
+#include <rosthread/mutex.h>
+#include <map>
+#include <genericControllers/JointController.h>
+#include <hw_interface/hardware_interface.h>
+#include <ros/node.h>
+#include <joy/Joy.h>
+
+using std::map;
+
+using namespace controller;
+
+//using CONTROLLER::Controller;
+
+//Base requirements for a piece of code that will control a full mechanism
+const int MAX_NUM_CONTROLLERS = 1000;
+
+typedef Controller*(*ControllerAllocationFunc)(const char *);
+
+class SingleControl
+{
+
+ public:
+
+ SingleControl();
+
+ void update(); //Must be realtime safe
+
+ void registerControllerType(const char *type, ControllerAllocationFunc f);
+
+ void requestController(const char *type, const char *ns);
+
+ void init(HardwareInterface *hw);
+
+ JointController *controller;
+
+ //JointController *controller;
+
+ private:
+
+ HardwareInterface *hw;
+
+ Robot *r;
+
+ void initRobot();
+
+ void initControllers();
+
+};
+
+#endif
Modified: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-07-29 02:04:47 UTC (rev 2229)
@@ -13,7 +13,7 @@
<depend package="etherdrive_hardware"/>
<export>
- <cpp cflags='-I${prefix}/include' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control -lgazebo_mechanism_control'/>
+ <cpp cflags='-I${prefix}/include' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control -lgazebo_mechanism_control -lsingle_control'/>
</export>
</package>
Added: pkg/trunk/mechanism/mechanism_control/src/single_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/single_control.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/src/single_control.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -0,0 +1,108 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// 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 Willow Garage Inc. 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 <mechanism_control/single_control.h>
+#include <signal.h>
+#include <sys/time.h>
+
+#include <mechanism_model/joint.h>
+#include <genericControllers/JointController.h>
+
+using namespace std;
+
+#define NUM_JOINTS 1
+
+const double maxPositiveTorque = 0.75;
+
+SingleControl::SingleControl(){
+ this->hw = NULL;
+}
+
+void SingleControl::init(HardwareInterface *hw){
+ this->hw = hw;
+ r = new Robot((char*)"robot");
+
+ r->numJoints = NUM_JOINTS;
+ r->numTransmissions = NUM_JOINTS;
+ r->numLinks = NUM_JOINTS;
+
+ r->transmission = new SimpleTransmission[NUM_JOINTS];
+ r->joint = new Joint[NUM_JOINTS];
+ r->link = new Link[NUM_JOINTS];
+
+ for(int ii=0; ii<NUM_JOINTS; ii++){
+ r->transmission[ii].actuator = &(hw->actuator[ii]);
+ r->transmission[ii].joint = &(r->joint[ii]);
+ r->transmission[ii].mechanicalReduction = -1;
+ r->transmission[ii].motorTorqueConstant = 1.0;
+ r->transmission[ii].pulsesPerRevolution = 1200;
+ hw->actuator[ii].command.enable = true;
+ r->joint[ii].effortLimit = maxPositiveTorque;
+ }
+
+ r->joint->jointLimitMax = 10;
+ r->joint->jointLimitMin = -10;
+ r->joint->effortLimit = 5;
+ controller = new JointController();
+
+#if POSITION
+ controller->init(0.3, 0.01, 0.0, 0.75,-0.75,controller::CONTROLLER_POSITION,hw->current_time_,0.75,-0.75, r->joint);
+ controller->setPosCmd(1.0);
+#elif TORQUE
+ controller->init(0.01, 0.1, 0, 0.75,-0.75,controller::CONTROLLER_TORQUE,hw->current_time_,0.75,-0.75, r->joint);
+ controller->setTorqueCmd(0.1);
+#else
+ controller->init(0.01, 0.1, 0, 0.75,-0.75,controller::CONTROLLER_VELOCITY,hw->current_time_,0.75,-0.75, r->joint);
+ controller->setVelCmd(100);
+#endif
+}
+
+void SingleControl::update()//This function is called only from the realtime loop. Everything it calls must also be realtime safe.
+{
+ //Clear actuator commands
+
+ //Process robot model transmissions
+ for(int i = 0; i < r->numTransmissions; i++){
+ r->transmission[i].propagatePosition();
+ }
+
+ //update KDL model with new joint position/velocities
+ controller->update(hw->current_time_);
+#ifdef DEBUG
+ for(int i = 0; i < r->numJoints; i++){
+ printf("base_control:: cmd:: %d, %f\n",i,r->joint[i].commandedEffort);
+ }
+#endif
+ /* for(int i = 0; i < r->numJoints; i++){
+ r->joint[i].enforceLimits();
+ }*/
+
+ for(int i = 0; i < r->numTransmissions; i++){
+ r->transmission[i].propagateEffort();
+ }
+}
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-07-29 01:35:41 UTC (rev 2228)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-07-29 02:04:47 UTC (rev 2229)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include <mechanism_control/base_control.h>
+#include <mechanism_control/single_control.h>
#include <ethercat_hardware/ethercat_hardware.h>
#include <stdio.h>
@@ -43,19 +43,27 @@
static int quit = 0;
static const int NSEC_PER_SEC = 1e+9;
+static void getTime(HardwareInterface *hw)
+{
+ struct timespec now;
+ clock_gettime(CLOCK_REALTIME, &now);
+ hw->current_time_ = double(now.tv_nsec) / NSEC_PER_SEC + now.tv_sec;
+}
+
void *controlLoop(void *arg)
{
char *interface = (char *)arg;
EthercatHardware ec;
ec.init(interface, (char *)"foo.xml");
+ getTime(ec.hw);
- MechanismControl mc;
+ SingleControl mc;
mc.init(ec.hw);
// Switch to hard real-time
- int period = 1000000;
- struct timespec tick, now;
+ int period = 1e+6; // 1 ms in nanoseconds
+ struct timespec tick;
#if defined(__XENO__)
pthread_set_mode_np(0, PTHREAD_PRIMARY|PTHREAD_WARNSW);
#endif
@@ -64,8 +72,7 @@
while (!quit)
{
ec.update();
- clock_gettime(CLOCK_REALTIME, &now);
- ec.hw->current_time_ = now.tv_nsec / NSEC_PER_SEC + now.tv_sec;
+ getTime(ec.hw);
mc.update();
tick.tv_nsec += period;
while (tick.tv_nsec >= NSEC_PER_SEC)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
