00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "Kuka361nAxesVelocityController.hpp"
00028
00029 #include <rtt/Logger.hpp>
00030
00031 namespace OCL
00032 {
00033 using namespace RTT;
00034 using namespace std;
00035
00036 #define KUKA361_NUM_AXES 6
00037
00038 #define KUKA361_ENCODEROFFSETS { 1000004, 1000000, 1000002, 449784, 1035056, 1230656 }
00039
00040 #define KUKA361_CONV1 94.14706
00041 #define KUKA361_CONV2 -103.23529
00042 #define KUKA361_CONV3 51.44118
00043 #define KUKA361_CONV4 175
00044 #define KUKA361_CONV5 150
00045 #define KUKA361_CONV6 131.64395
00046
00047 #define KUKA361_ENC_RES 4096
00048
00049
00050 #define KUKA361_TICKS2RAD { 2*M_PI / (KUKA361_CONV1 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV2 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV3 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV4 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV5 * KUKA361_ENC_RES), 2*M_PI / (KUKA361_CONV6 * KUKA361_ENC_RES)}
00051
00052
00053 #define KUKA361_RADproSEC2VOLT { 2.5545, 2.67804024532652, 1.37350318088664, 2.34300679603342, 2.0058, 3.3786 } //18 april 2006
00054
00055 typedef Kuka361nAxesVelocityController MyType;
00056
00057 Kuka361nAxesVelocityController::Kuka361nAxesVelocityController(string name,string propertyfile)
00058 : TaskContext(name),
00059 _startAxis( "startAxis", &MyType::startAxis, this),
00060 _startAllAxes( "startAllAxes", &MyType::startAllAxes, this),
00061 _stopAxis( "stopAxis", &MyType::stopAxis, this),
00062 _stopAllAxes( "stopAllAxes", &MyType::stopAllAxes, this),
00063 _unlockAxis( "unlockAxis", &MyType::unlockAxis, this),
00064 _unlockAllAxes( "unlockAllAxes", &MyType::unlockAllAxes, this),
00065 _lockAxis( "lockAxis", &MyType::lockAxis, this),
00066 _lockAllAxes( "lockAllAxes", &MyType::lockAllAxes, this),
00067 _prepareForUse( "prepareForUse", &MyType::prepareForUse,&MyType::prepareForUseCompleted, this),
00068 _prepareForShutdown( "prepareForShutdown", &MyType::prepareForShutdown,&MyType::prepareForShutdownCompleted, this),
00069 _addDriveOffset( "addDriveOffset", &MyType::addDriveOffset, this),
00070 _driveValue(KUKA361_NUM_AXES),
00071 _positionValue(KUKA361_NUM_AXES),
00072 _driveLimits("driveLimits","velocity limits of the axes, (rad/s)",vector<double>(KUKA361_NUM_AXES,0)),
00073 _lowerPositionLimits("LowerPositionLimits","Lower position limits (rad)",vector<double>(KUKA361_NUM_AXES,0)),
00074 _upperPositionLimits("UpperPositionLimits","Upper position limits (rad)",vector<double>(KUKA361_NUM_AXES,0)),
00075 _initialPosition("initialPosition","Initial position (rad) for simulation or hardware",vector<double>(KUKA361_NUM_AXES,0)),
00076 _driveOffset("driveOffset","offset (in rad/s) to the drive value.",vector<double>(KUKA361_NUM_AXES,0)),
00077 _simulation("simulation","true if simulationAxes should be used",true),
00078 _num_axes("NUM_AXES",KUKA361_NUM_AXES),
00079 _driveOutOfRange("driveOutOfRange"),
00080 _positionOutOfRange("positionOutOfRange"),
00081 _propertyfile(propertyfile),
00082 _activated(false),
00083 _positionConvertFactor(KUKA361_NUM_AXES),
00084 _driveConvertFactor(KUKA361_NUM_AXES),
00085 #if (defined OROPKG_OS_LXRT)
00086 _axes_hardware(KUKA361_NUM_AXES),
00087 _encoderInterface(KUKA361_NUM_AXES),
00088 _encoder(KUKA361_NUM_AXES),
00089 _vref(KUKA361_NUM_AXES),
00090 _enable(KUKA361_NUM_AXES),
00091 _drive(KUKA361_NUM_AXES),
00092 _brake(KUKA361_NUM_AXES),
00093 #endif
00094 _axes(KUKA361_NUM_AXES),
00095 _axes_simulation(KUKA361_NUM_AXES)
00096 {
00097 Logger::In in("Kuka361nAxesVelocityController");
00098 double ticks2rad[KUKA361_NUM_AXES] = KUKA361_TICKS2RAD;
00099 double vel2volt[KUKA361_NUM_AXES] = KUKA361_RADproSEC2VOLT;
00100 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00101 _positionConvertFactor[i] = ticks2rad[i];
00102 _driveConvertFactor[i] = vel2volt[i];
00103 }
00104 properties()->addProperty( &_driveLimits );
00105 properties()->addProperty( &_lowerPositionLimits );
00106 properties()->addProperty( &_upperPositionLimits );
00107 properties()->addProperty( &_initialPosition );
00108 properties()->addProperty( &_driveOffset );
00109 properties()->addProperty( &_simulation );
00110 attributes()->addConstant( &_num_axes);
00111
00112 if (!marshalling()->readProperties(_propertyfile)) {
00113 log(Error) << "Failed to read the property file, continueing with default values." << endlog();
00114 }
00115
00116 #if (defined OROPKG_OS_LXRT)
00117 int encoderOffsets[KUKA361_NUM_AXES] = KUKA361_ENCODEROFFSETS;
00118
00119 log(Info)<<"Creating Comedi Devices."<<endlog();
00120 _comediDev = new ComediDevice( 1 );
00121 _comediSubdevAOut = new ComediSubDeviceAOut( _comediDev, "Kuka361" );
00122 log(Info)<<"Creating APCI Devices."<<endlog();
00123 _apci1710 = new EncoderSSI_apci1710_board( 0, 1 );
00124 _apci2200 = new RelayCardapci2200( "Kuka361" );
00125 _apci1032 = new SwitchDigitalInapci1032( "Kuka361" );
00126
00127
00128 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++){
00129 log(Info)<<"Creating Hardware axis "<<i<<endlog();
00130
00131 log(Info)<<"Setting up encoder ..."<<endlog();
00132 _encoderInterface[i] = new EncoderSSI_apci1710( i + 1, _apci1710 );
00133 _encoder[i] = new AbsoluteEncoderSensor( _encoderInterface[i], 1.0 / ticks2rad[i], encoderOffsets[i], -10, 10 );
00134
00135 log(Info)<<"Setting up brake ..."<<endlog();
00136 _brake[i] = new DigitalOutput( _apci2200, i + KUKA361_NUM_AXES );
00137 log(Info)<<"Setting brake to on"<<endlog();
00138 _brake[i]->switchOn();
00139
00140 log(Info)<<"Setting up drive ..."<<endlog();
00141 _vref[i] = new AnalogOutput( _comediSubdevAOut, i );
00142 _enable[i] = new DigitalOutput( _apci2200, i );
00143 _drive[i] = new AnalogDrive( _vref[i], _enable[i], 1.0 / vel2volt[i], _driveOffset.value()[i]);
00144
00145
00146
00147
00148
00149
00150 _axes_hardware[i] = new RTT::Axis( _drive[i] );
00151 _axes_hardware[i]->setBrake( _brake[i] );
00152 _axes_hardware[i]->setSensor( "Position", _encoder[i] );
00153
00154 _axes_hardware[i]->limitDrive(-_driveLimits.value()[i], _driveLimits.value()[i], _driveOutOfRange);
00155 }
00156
00157 #endif
00158 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++){
00159 _axes_simulation[i] = new RTT::SimulationAxis(_initialPosition.value()[i],
00160 _lowerPositionLimits.value()[i],
00161 _upperPositionLimits.value()[i]);
00162 }
00163 #if (defined OROPKG_OS_LXRT)
00164 if(!_simulation.value()){
00165 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++)
00166 _axes[i] = _axes_hardware[i];
00167 log(Info) << "LXRT version of LiASnAxesVelocityController has started" << endlog();
00168 }
00169 else{
00170 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++)
00171 _axes[i] = _axes_simulation[i];
00172 log(Info) << "LXRT simulation version of Kuka361nAxesVelocityController has started" << endlog();
00173 }
00174 #else
00175 for (unsigned int i = 0; i <KUKA361_NUM_AXES; i++)
00176 _axes[i] = _axes_simulation[i];
00177 log(Info) << "GNULINUX simulation version of Kuka361nAxesVelocityController has started" << endlog();
00178 #endif
00179
00180
00181
00182
00183
00184
00185 this->methods()->addMethod( &_startAxis, "start axis, starts updating the drive-value (only possible after unlockAxis)","axis","axis to start" );
00186 this->methods()->addMethod( &_stopAxis,"stop axis, sets drive value to zero and disables the update of the drive-port, (only possible if axis is started","axis","axis to stop");
00187 this->methods()->addMethod( &_lockAxis,"lock axis, enables the brakes (only possible if axis is stopped","axis","axis to lock" );
00188 this->methods()->addMethod( &_unlockAxis,"unlock axis, disables the brakes and enables the drive (only possible if axis is locked","axis","axis to unlock" );
00189 this->methods()->addMethod( &_startAllAxes, "start all axes" );
00190 this->methods()->addMethod( &_stopAllAxes, "stops all axes" );
00191 this->methods()->addMethod( &_lockAllAxes, "locks all axes" );
00192 this->methods()->addMethod( &_unlockAllAxes, "unlock all axes" );
00193 this->commands()->addCommand( &_prepareForUse, "prepares the robot for use" );
00194 this->commands()->addCommand( &_prepareForShutdown,"prepares the robot for shutdown" );
00195 this->methods()->addMethod( &_addDriveOffset,"adds an offset to the drive value of axis","axis","axis to add offset to","offset","offset value in rad/s" );
00196
00197
00201 for (int i=0;i<KUKA361_NUM_AXES;++i) {
00202 char buf[80];
00203 sprintf(buf,"driveValue%d",i);
00204 _driveValue[i] = new ReadDataPort<double>(buf);
00205 ports()->addPort(_driveValue[i]);
00206 sprintf(buf,"positionValue%d",i);
00207 _positionValue[i] = new DataPort<double>(buf);
00208 ports()->addPort(_positionValue[i]);
00209 }
00210
00214 events()->addEvent( &_driveOutOfRange, "Velocity of an Axis is out of range","message","Information about event" );
00215 events()->addEvent( &_positionOutOfRange, "Position of an Axis is out of range","message","Information about event");
00216 }
00217
00218 Kuka361nAxesVelocityController::~Kuka361nAxesVelocityController()
00219 {
00220
00221 prepareForShutdown();
00222
00223
00224 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++)
00225 delete _axes_simulation[i];
00226
00227 #if (defined OROPKG_OS_LXRT)
00228 for (unsigned int i = 0; i < KUKA361_NUM_AXES; i++)
00229 delete _axes_hardware[i];
00230 delete _comediDev;
00231 delete _comediSubdevAOut;
00232 delete _apci1710;
00233 delete _apci2200;
00234 delete _apci1032;
00235 #endif
00236 }
00237
00238
00239 bool Kuka361nAxesVelocityController::startup()
00240 {
00241 return true;
00242 }
00243
00244 void Kuka361nAxesVelocityController::update()
00245 {
00246 for (int axis=0;axis<KUKA361_NUM_AXES;axis++) {
00247
00248 _positionValue[axis]->Set(_axes[axis]->getSensor("Position")->readSensor());
00249
00250
00251 if( (_positionValue[axis]->Get() < _lowerPositionLimits.value()[axis]) ||
00252 (_positionValue[axis]->Get() > _upperPositionLimits.value()[axis]) )
00253 _positionOutOfRange("Position of a Kuka361 Axis is out of range");
00254
00255
00256 if (_axes[axis]->isDriven())
00257 _axes[axis]->drive(_driveValue[axis]->Get());
00258 }
00259 }
00260
00261
00262 void Kuka361nAxesVelocityController::shutdown()
00263 {
00264
00265 prepareForShutdown();
00266
00267 marshalling()->writeProperties(_propertyfile);
00268 }
00269
00270
00271 bool Kuka361nAxesVelocityController::prepareForUse()
00272 {
00273 #if (defined OROPKG_OS_LXRT)
00274 if(!_simulation.value()){
00275 _apci2200->switchOn( 12 );
00276 _apci2200->switchOn( 14 );
00277 log(Warning) <<"Release Emergency stop and push button to start ...."<<endlog();
00278 }
00279 #endif
00280 _activated = true;
00281 return true;
00282 }
00283
00284 bool Kuka361nAxesVelocityController::prepareForUseCompleted()const
00285 {
00286 #if (defined OROPKG_OS_LXRT)
00287 if(!_simulation.rvalue())
00288 return (_apci1032->isOn(12) && _apci1032->isOn(14));
00289 else
00290 #endif
00291 return true;
00292 }
00293
00294 bool Kuka361nAxesVelocityController::prepareForShutdown()
00295 {
00296
00297 stopAllAxes();
00298 lockAllAxes();
00299 #if (defined OROPKG_OS_LXRT)
00300 if(!_simulation.value()){
00301 _apci2200->switchOff( 12 );
00302 _apci2200->switchOff( 14 );
00303 }
00304
00305 #endif
00306 _activated = false;
00307 return true;
00308 }
00309
00310 bool Kuka361nAxesVelocityController::prepareForShutdownCompleted()const
00311 {
00312 return true;
00313 }
00314
00315 bool Kuka361nAxesVelocityController::stopAxisCompleted(int axis)const
00316 {
00317 return _axes[axis]->isStopped();
00318 }
00319
00320 bool Kuka361nAxesVelocityController::lockAxisCompleted(int axis)const
00321 {
00322 return _axes[axis]->isLocked();
00323 }
00324
00325 bool Kuka361nAxesVelocityController::startAxisCompleted(int axis)const
00326 {
00327 return _axes[axis]->isDriven();
00328 }
00329
00330 bool Kuka361nAxesVelocityController::unlockAxisCompleted(int axis)const
00331 {
00332 return !_axes[axis]->isLocked();
00333 }
00334
00335 bool Kuka361nAxesVelocityController::stopAxis(int axis)
00336 {
00337 if (!(axis<0 || axis>KUKA361_NUM_AXES-1))
00338 return _axes[axis]->stop();
00339 else{
00340 log(Error) <<"Axis "<< axis <<"doesn't exist!!"<<endlog();
00341 return false;
00342 }
00343 }
00344
00345 bool Kuka361nAxesVelocityController::startAxis(int axis)
00346 {
00347 if (!(axis<0 || axis>KUKA361_NUM_AXES-1))
00348 return _axes[axis]->drive(0.0);
00349 else{
00350 log(Error) <<"Axis "<< axis <<"doesn't exist!!"<<endlog();
00351 return false;
00352 }
00353 }
00354
00355 bool Kuka361nAxesVelocityController::unlockAxis(int axis)
00356 {
00357 if(_activated){
00358 if (!(axis<0 || axis>KUKA361_NUM_AXES-1))
00359 return _axes[axis]->unlock();
00360 else{
00361 log(Error) <<"Axis "<< axis <<"doesn't exist!!"<<endlog();
00362 return false;
00363 }
00364 }
00365 else
00366 return false;
00367 }
00368
00369 bool Kuka361nAxesVelocityController::lockAxis(int axis)
00370 {
00371 if (!(axis<0 || axis>KUKA361_NUM_AXES-1))
00372 return _axes[axis]->lock();
00373 else{
00374 log(Error) <<"Axis "<< axis <<"doesn't exist!!"<<endlog();
00375 return false;
00376 }
00377 }
00378
00379 bool Kuka361nAxesVelocityController::stopAllAxes()
00380 {
00381 bool _return = true;
00382 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00383 _return &= stopAxis(i);
00384 }
00385 return _return;
00386 }
00387
00388 bool Kuka361nAxesVelocityController::startAllAxes()
00389 {
00390 bool _return = true;
00391 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00392 _return &= startAxis(i);
00393 }
00394 return _return;
00395 }
00396
00397 bool Kuka361nAxesVelocityController::unlockAllAxes()
00398 {
00399 bool _return = true;
00400 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00401 _return &= unlockAxis(i);
00402 }
00403 return _return;
00404 }
00405
00406 bool Kuka361nAxesVelocityController::lockAllAxes()
00407 {
00408 bool _return = true;
00409 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++){
00410 _return &= lockAxis(i);
00411 }
00412 return _return;
00413 }
00414
00415 bool Kuka361nAxesVelocityController::stopAllAxesCompleted()const
00416 {
00417 bool _return = true;
00418 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++)
00419 _return &= stopAxisCompleted(i);
00420 return _return;
00421 }
00422
00423 bool Kuka361nAxesVelocityController::startAllAxesCompleted()const
00424 {
00425 bool _return = true;
00426 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++)
00427 _return &= startAxisCompleted(i);
00428 return _return;
00429 }
00430
00431 bool Kuka361nAxesVelocityController::lockAllAxesCompleted()const
00432 {
00433 bool _return = true;
00434 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++)
00435 _return &= lockAxisCompleted(i);
00436 return _return;
00437 }
00438
00439 bool Kuka361nAxesVelocityController::unlockAllAxesCompleted()const
00440 {
00441 bool _return = true;
00442 for(unsigned int i = 0;i<KUKA361_NUM_AXES;i++)
00443 _return &= unlockAxisCompleted(i);
00444 return _return;
00445 }
00446
00447 bool Kuka361nAxesVelocityController::addDriveOffset(int axis, double offset)
00448 {
00449 _driveOffset.value()[axis] += offset;
00450
00451 #if (defined OROPKG_OS_LXRT)
00452 if (!_simulation.value())
00453 ((Axis*)(_axes[axis]))->getDrive()->addOffset(offset);
00454 #endif
00455 return true;
00456 }
00457
00458 bool Kuka361nAxesVelocityController::addDriveOffsetCompleted(int axis)const
00459 {
00460 return true;
00461 }
00462
00463 unsigned int Kuka361nAxesVelocityController::GetNumAxes()
00464 {
00465 return KUKA361_NUM_AXES;
00466
00467 }
00468
00469
00470 }
