OrocosComponentLibrary: StaubliRX130nAxesVelocityController.cpp Source File

00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/ocl
00007 
00008 // This program is free software; you can redistribute it and/or modify
00009 // it under the terms of the GNU General Public License as published by
00010 // the Free Software Foundation; either version 2 of the License, or
00011 // (at your option) any later version.
00012 
00013 // This program is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016 // GNU General Public License for more details.
00017 
00018 // You should have received a copy of the GNU General Public License
00019 // along with this program; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 // Servo controller based on work of CRSnAxesVelocityContoller by
00023 // Erwin Aertbelien
00024 
00025 #include "StaubliRX130nAxesVelocityController.hpp"
00026 #include <ocl/ComponentLoader.hpp>
00027 
00028 ORO_CREATE_COMPONENT( OCL::StaubliRX130nAxesVelocityController );
00029 
00030 #include <rtt/Logger.hpp>
00031 #include <stdlib.h>
00032 #include <unistd.h>
00033 #include <termios.h>
00034 #include <fcntl.h>
00035 #include <bitset>
00036 
00037 #if (defined OROPKG_OS_LXRT)
00038 #include "ComediDevices.hpp"
00039 #endif
00040 
00041 namespace OCL
00042 {
00043     using namespace RTT;
00044     using namespace KDL;
00045     using namespace std;
00046 
00047 #define STAUBLIRX130_NUM_AXES 6
00048 
00049 #define STAUBLIRX130_ENCODEROFFSETS { 0, 0, 0, 0, 0, 0 }
00050 
00051 #define STAUBLIRX130_CONV1  -128.6107
00052 #define STAUBLIRX130_CONV2  -129.3939/1.00044
00053 #define STAUBLIRX130_CONV3  1112.0/11.0
00054 #define STAUBLIRX130_CONV4  88
00055 #define STAUBLIRX130_CONV5  75
00056 #define STAUBLIRX130_CONV6  32
00057 
00058 #define STAUBLIRX130_ENC_RES  4096
00059 
00060     // Conversion from encoder ticks to radiants
00061 #define STAUBLIRX130_TICKS2RAD { 2*M_PI / (STAUBLIRX130_CONV1 * STAUBLIRX130_ENC_RES), 2*M_PI / (STAUBLIRX130_CONV2 * STAUBLIRX130_ENC_RES), 2*M_PI / (STAUBLIRX130_CONV3 * STAUBLIRX130_ENC_RES), 2*M_PI / (STAUBLIRX130_CONV4 * STAUBLIRX130_ENC_RES), 2*M_PI / (STAUBLIRX130_CONV5 * STAUBLIRX130_ENC_RES), 2*M_PI / (STAUBLIRX130_CONV6 * STAUBLIRX130_ENC_RES)}
00062 
00063   // Conversion from angular speed to voltage
00064 #define STAUBLIRX130_RADproSEC2VOLT { -2.9452, -2.9452, 1.25, -1.28, 1.15, 0.475 }
00065 #define DRIVEVALUE_5_TO_6 (1.15/0.475)
00066 
00067 #define KINEMATICS_EPS 0.0001
00068 #define SQRT3d2 0.8660254037844386
00069 #define M_PI_T2 2 * M_PI
00070 #define SQRT3t2 3.46410161513775 // 2 sqrt(3)
00071 
00072     typedef StaubliRX130nAxesVelocityController MyType;
00073 
00074     StaubliRX130nAxesVelocityController::StaubliRX130nAxesVelocityController(string name)
00075         : TaskContext(name,PreOperational),
00076           startAllAxes_mtd( "startAllAxes", &MyType::startAllAxes, this),
00077           stopAllAxes_mtd( "stopAllAxes", &MyType::stopAllAxes, this),
00078           unlockAllAxes_mtd( "unlockAllAxes", &MyType::unlockAllAxes, this),
00079           lockAllAxes_mtd( "lockAllAxes", &MyType::lockAllAxes, this),
00080           prepareForUse_cmd( "prepareForUse", &MyType::prepareForUse,&MyType::prepareForUseCompleted, this),
00081           prepareForShutdown_cmd( "prepareForShutdown", &MyType::prepareForShutdown,&MyType::prepareForShutdownCompleted, this),
00082           addDriveOffset_mtd( "addDriveOffset", &MyType::addDriveOffset, this),
00083           driveValues_port("nAxesOutputVelocity"),
00084           servoValues_port("nAxesServoVelocity"),
00085           positionValues_port("nAxesSensorPosition"),
00086           velocityValues_port("nAxesSensorVelocity"),
00087           deltaTime_port("DeltaTime"),
00088           driveLimits_prop("driveLimits","velocity limits of the axes, (rad/s)",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00089           lowerPositionLimits_prop("LowerPositionLimits","Lower position limits (rad)",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00090           upperPositionLimits_prop("UpperPositionLimits","Upper position limits (rad)",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00091           velocityLimits_prop("velocityLimits","velocity limits of the axes, (rad/s)",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00092           initialPosition_prop("initialPosition","Initial position (rad) for simulation or hardware",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00093           driveOffset_prop("driveOffset","offset (in rad/s) to the drive value.",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00094           simulation_prop("simulation","true if simulationAxes should be used",true),
00095           simulation(true),
00096           num_axes_attr("NUM_AXES",STAUBLIRX130_NUM_AXES),
00097           chain_attr("Kinematics"),
00098           driveOutOfRange_evt("driveOutOfRange"),
00099           positionOutOfRange_evt("positionOutOfRange"),
00100           velocityOutOfRange_evt("velocityOutOfRange"),
00101           servoIntegrationFactor_prop("ServoIntegrationFactor","Inverse of Integration time for servoloop",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00102           servoGain_prop("ServoGain","Feedback Gain for servoloop",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00103           servoFFScale_prop("ServoFFScale","Feedforward scale for servoloop",std::vector<double>(STAUBLIRX130_NUM_AXES,0)),
00104           EmergencyEvents_prop("EmergencyEvents","List of events that will result in an emergencystop of the robot"),
00105           activated(false),
00106           positionConvertFactor(STAUBLIRX130_NUM_AXES,0),
00107           driveConvertFactor(STAUBLIRX130_NUM_AXES,0),
00108           positionValues(STAUBLIRX130_NUM_AXES,0),
00109           driveValues(STAUBLIRX130_NUM_AXES,0),
00110           velocityValues(STAUBLIRX130_NUM_AXES,0),
00111           servoIntError(STAUBLIRX130_NUM_AXES,0),
00112           outputvel(STAUBLIRX130_NUM_AXES,0),
00113           servoIntegrationFactor(STAUBLIRX130_NUM_AXES,0),
00114           servoGain(STAUBLIRX130_NUM_AXES,0),
00115           servoFFScale(STAUBLIRX130_NUM_AXES,0),
00116           servoInitialized(false),
00117 #if (defined OROPKG_OS_LXRT)
00118           axes_hardware(STAUBLIRX130_NUM_AXES),
00119           encoderInterface(STAUBLIRX130_NUM_AXES),
00120           encoder(STAUBLIRX130_NUM_AXES),
00121           vref(STAUBLIRX130_NUM_AXES),
00122           enable(STAUBLIRX130_NUM_AXES),
00123           drive(STAUBLIRX130_NUM_AXES),
00124           driveFailure(STAUBLIRX130_NUM_AXES),
00125 #endif
00126           axes(STAUBLIRX130_NUM_AXES),
00127           _previous_time(STAUBLIRX130_NUM_AXES),
00128           init_pos(STAUBLIRX130_NUM_AXES,0.0)
00129     {
00130         Logger::In in(this->getName());
00131         double ticks2rad[STAUBLIRX130_NUM_AXES] = STAUBLIRX130_TICKS2RAD;
00132         double vel2volt[STAUBLIRX130_NUM_AXES] = STAUBLIRX130_RADproSEC2VOLT;
00133         for(unsigned int i = 0;i<STAUBLIRX130_NUM_AXES;i++){
00134             positionConvertFactor[i] = ticks2rad[i];
00135             driveConvertFactor[i] = vel2volt[i];
00136         }
00137 
00138 #if (defined OROPKG_OS_LXRT)
00139         double encoderOffsets[STAUBLIRX130_NUM_AXES] = STAUBLIRX130_ENCODEROFFSETS;
00140 
00141         log(Info)<<"Creating Comedi Devices."<<endlog();
00142         ComediLoader::Instance()->CreateComediDevices();
00143 
00144         SubAOut = AnalogOutInterface::nameserver.getObject("AnalogOut");
00145         SubDIn = DigitalInInterface::nameserver.getObject("DigitalIn");
00146         SubDOut = DigitalOutInterface::nameserver.getObject("DigitalOut");
00147         encoderInterface[0] = EncoderInterface::nameserver.getObject("Counter0");
00148         encoderInterface[1] = EncoderInterface::nameserver.getObject("Counter1");
00149         encoderInterface[2] = EncoderInterface::nameserver.getObject("Counter2");
00150         encoderInterface[3] = EncoderInterface::nameserver.getObject("Counter3");
00151         encoderInterface[4] = EncoderInterface::nameserver.getObject("Counter4");
00152         encoderInterface[5] = EncoderInterface::nameserver.getObject("Counter5");
00153 
00154         brakeOff = new DigitalOutput(SubDOut,17);
00155         brakeOff->switchOff();
00156 
00157         highPowerEnable = new DigitalOutput(SubDOut,16);
00158 
00159         eStop = new DigitalInput(SubDIn,14);
00160 
00161         armPowerOn = new DigitalInput(SubDIn,15);
00162 
00163 
00164         for (unsigned int i = 0; i < STAUBLIRX130_NUM_AXES; i++){
00165             log(Info)<<"Creating Hardware axis "<<i<<endlog();
00166             //Setting up encoders
00167             log(Info)<<"Setting up encoder ..."<<endlog();
00168             encoder[i] = new IncrementalEncoderSensor( encoderInterface[i], 1.0 / ticks2rad[i],
00169                                                        encoderOffsets[i],
00170                                                        -10, 10,STAUBLIRX130_ENC_RES );
00171 
00172             log(Info)<<"Setting up drive ..."<<endlog();
00173             log(Debug)<<"Creating AnalogOutput"<<endlog();
00174             vref[i]   = new AnalogOutput(SubAOut, i );
00175             log(Debug)<<"Creating Digital Output Enable"<<endlog();
00176             enable[i] = new DigitalOutput( SubDOut, 18+i );
00177             log(Debug)<<"Initial switch off brake"<<endlog();
00178             enable[i]->switchOff();
00179             log(Debug)<<"Creating AnalogDrive"<<endlog();
00180             drive[i]  = new AnalogDrive( vref[i], enable[i], 1.0 / vel2volt[i], 0.0);
00181 
00182             log(Debug)<<"Creating Digital Input DriveFailure"<<endlog();
00183             driveFailure[i] = new DigitalInput(SubDIn,8+i);
00184             log(Debug)<<"Creating Axis"<<endlog();
00185             axes_hardware[i] = new Axis( drive[i] );
00186             log(Debug)<<"Adding Position Sensor to Axis"<<endlog();
00187             axes_hardware[i]->setSensor( "Position", encoder[i] );
00188         }
00189 
00190 
00191 #endif
00192         //Definition of kinematics for the StaubliRX130
00193         kinematics.addSegment(Segment(Joint(Joint::RotZ),Frame(Rotation::RotZ(M_PI_2),Vector(0.0,0.0,0.2075+0.550))));
00194         kinematics.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.625))));
00195         kinematics.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.625))));
00196         kinematics.addSegment(Segment(Joint(Joint::RotZ)));
00197         kinematics.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.110))));
00198         kinematics.addSegment(Segment(Joint(Joint::RotZ),Frame(Rotation::RotZ(-M_PI_2))));
00199 
00200         chain_attr.set(kinematics);
00201 
00202         /*
00203          *  Execution Interface
00204          */
00205         methods()->addMethod( &startAllAxes_mtd, "start all axes"  );
00206         methods()->addMethod( &stopAllAxes_mtd, "stops all axes"  );
00207         methods()->addMethod( &lockAllAxes_mtd, "locks all axes"  );
00208         methods()->addMethod( &unlockAllAxes_mtd, "unlock all axes"  );
00209         commands()->addCommand( &prepareForUse_cmd, "prepares the robot for use"  );
00210         commands()->addCommand( &prepareForShutdown_cmd,"prepares the robot for shutdown"  );
00211         methods()->addMethod( &addDriveOffset_mtd,"adds an offset to the drive value of axis","offset","offset values in rad/s" );
00212         events()->addEvent( &driveOutOfRange_evt, "Drive value of an Axis is out of range","message","Information about event" );
00213         events()->addEvent( &positionOutOfRange_evt, "Position of an Axis is out of range","message","Information about event");
00214         events()->addEvent( &velocityOutOfRange_evt, "Velocity of an Axis is out of range","message","Information about event");
00215 
00219         ports()->addPort(&driveValues_port);
00220         ports()->addPort(&servoValues_port);
00221         ports()->addPort(&positionValues_port);
00222         ports()->addPort(&velocityValues_port);
00223         ports()->addPort(&deltaTime_port);
00224 
00228         properties()->addProperty( &driveLimits_prop);
00229         properties()->addProperty( &lowerPositionLimits_prop);
00230         properties()->addProperty( &upperPositionLimits_prop);
00231         properties()->addProperty( &velocityLimits_prop);
00232         properties()->addProperty( &initialPosition_prop);
00233         properties()->addProperty( &driveOffset_prop);
00234         properties()->addProperty( &simulation_prop);
00235         properties()->addProperty( &servoIntegrationFactor_prop);
00236         properties()->addProperty( &servoGain_prop);
00237         properties()->addProperty( &servoFFScale_prop);
00238         properties()->addProperty( &EmergencyEvents_prop);
00239         attributes()->addConstant( &num_axes_attr);
00240         attributes()->addAttribute(&chain_attr);
00241 
00242     }
00243 
00244     StaubliRX130nAxesVelocityController::~StaubliRX130nAxesVelocityController()
00245     {
00246         // make sure robot is shut down
00247         prepareForShutdown_cmd();
00248         // make sure last position is stored
00249         this->cleanupHook();
00250 
00251         // brake, drive, sensors and switches are deleted by each axis
00252         if(simulation_prop.value())
00253             for (unsigned int i = 0; i < STAUBLIRX130_NUM_AXES; i++)
00254                 delete axes[i];
00255 
00256 #if (defined OROPKG_OS_LXRT)
00257         for (unsigned int i = 0; i < STAUBLIRX130_NUM_AXES; i++){
00258             delete axes_hardware[i];
00259             delete driveFailure[i];
00260         }
00261         delete armPowerOn;
00262         delete eStop;
00263         delete highPowerEnable;
00264         delete brakeOff;
00265         
00266         ComediLoader::Instance()->DestroyComediDevices();
00267 #endif
00268     }
00269 
00270     bool StaubliRX130nAxesVelocityController::configureHook()
00271     {
00272         Logger::In in(this->getName().data());
00273 
00274         simulation=simulation_prop.value();
00275 
00276         if(!(driveLimits_prop.value().size()==STAUBLIRX130_NUM_AXES&&
00277              lowerPositionLimits_prop.value().size()==STAUBLIRX130_NUM_AXES&&
00278              upperPositionLimits_prop.value().size()==STAUBLIRX130_NUM_AXES&&
00279              velocityLimits_prop.value().size()==STAUBLIRX130_NUM_AXES&&
00280              initialPosition_prop.value().size()==STAUBLIRX130_NUM_AXES&&
00281              driveOffset_prop.value().size()==STAUBLIRX130_NUM_AXES))
00282             return false;
00283 
00284 #if (defined OROPKG_OS_LXRT)
00285         if(!simulation){
00286 
00287             for (unsigned int i = 0; i <STAUBLIRX130_NUM_AXES; i++){
00288                 axes_hardware[i]->limitDrive(-driveLimits_prop.value()[i], driveLimits_prop.value()[i], driveOutOfRange_evt);
00289                 axes[i] = axes_hardware[i];
00290                 ((Axis*)(axes[i]))->getDrive()->addOffset(driveOffset_prop.value()[i]);
00291                 log(Info) << "Hardware version of StaubliRX130nAxesVelocityController has started" << endlog();
00292             }
00293 
00294             if(!this->readAbsolutePosition(init_pos))
00295                 return false;
00296             /*else
00297                 for(unsigned int i=0;i<STAUBLIRX130_NUM_AXES;i++)
00298                     if(i!=5)
00299                         ((IncrementalEncoderSensor*)(axes[i]->getSensor("Position")))->writeSensor(initPos[i]);
00300                     else
00301                         ((IncrementalEncoderSensor*)(axes[i]->getSensor("Position")))->writeSensor(initPos[i]+initPos[i-1]);
00302                         //positionValues[i]=initPos[i];*/
00303             }
00304         else{
00305 #endif
00306             for (unsigned int i = 0; i <STAUBLIRX130_NUM_AXES; i++)
00307                 axes[i] = new SimulationAxis(initialPosition_prop.value()[i],
00308                                              lowerPositionLimits_prop.value()[i],
00309                                              upperPositionLimits_prop.value()[i]);
00310             log(Info) << "Simulation version of StaubliRX130nAxesVelocityController has started" << endlog();
00311 #if (defined OROPKG_OS_LXRT)
00312         }
00313 #endif
00314         //Initialise sensorport
00315         positionValues_port.Set(positionValues);
00316         
00320         servoGain = servoGain_prop.value();
00321         servoIntegrationFactor = servoIntegrationFactor_prop.value();
00322         servoFFScale = servoFFScale_prop.value();
00323 
00327         log(Info)<<"Creating EmergencyEvent Handlers"<<endlog();
00328 
00329         for(std::vector<string>::const_iterator it=EmergencyEvents_prop.rvalue().begin();it!=EmergencyEvents_prop.rvalue().end();it++){
00330             string::size_type idx = (*it).find('.');
00331             if(idx==string::npos)
00332                 log(Warning)<<"Could not connect EmergencyStop to "<<(*it)<<"\n Syntax of "
00333                             <<(*it)<<" is not correct. I want a ComponentName.EventName "<<endlog();
00334             string peername = (*it).substr(0,idx);
00335             string eventname = (*it).substr(idx+1);
00336             TaskContext* peer;
00337             if(peername==this->getName())
00338                 peer = this;
00339             else if(this->hasPeer(peername)){
00340                 peer = this->getPeer(peername);
00341             }else{
00342                 log(Warning)<<"Could not connect EmergencyStop to "<<(*it)<<", "<<peername<<" is not a peer of "<<this->getName()<<endlog();
00343                 continue;
00344             }
00345 
00346             if(peer->events()->hasEvent(eventname)){
00347                 Handle handle = peer->events()->setupConnection(eventname).callback(this,&StaubliRX130nAxesVelocityController::EmergencyStop).handle();
00348                 if(handle.connect()){
00349                     EmergencyEventHandlers.push_back(handle);
00350                     log(Info)<<"EmergencyStop connected to "<< (*it)<<" event."<<endlog();
00351                 }else
00352                     log(Warning)<<"Could not connect EmergencyStop to "<<(*it)<<", "<<eventname<<" has to have a message parameter."<<endlog();
00353             }else
00354                 log(Warning)<<"Could not connect EmergencyStop to "<<(*it)<<", "<<eventname <<" not found in "<<peername<<"s event-list"<<endlog();
00355         }
00356 
00357 
00358         return true;
00359     }
00360 
00361     bool StaubliRX130nAxesVelocityController::startHook()
00362     {
00363         for (int axis=0;axis<STAUBLIRX130_NUM_AXES;axis++) {
00364 #if defined OROPKG_OS_LXRT
00365             if (axis != STAUBLIRX130_NUM_AXES-1||simulation){
00366 #endif
00367                     _previous_time[axis] = TimeService::Instance()->getTicks();
00368                     positionValues[axis] = axes[axis]->getSensor("Position")->readSensor()+init_pos[axis];
00369 #if defined OROPKG_OS_LXRT
00370             }else{
00371                 //Last Axes moves together with previous axes
00372                 _previous_time[axis] = TimeService::Instance()->getTicks();
00373                 positionValues[axis] = axes[axis]->getSensor("Position")->readSensor()+init_pos[axis] - axes[axis-1]->getSensor("Position")->readSensor();
00374             }
00375 #endif
00376         }
00377         positionValues_port.Set(positionValues);
00378         return true;
00379     }
00380 
00381     void StaubliRX130nAxesVelocityController::updateHook()
00382     {
00383         for (int axis=0;axis<STAUBLIRX130_NUM_AXES;axis++) {
00384 #if defined OROPKG_OS_LXRT
00385             if (axis != STAUBLIRX130_NUM_AXES-1||simulation){
00386 #endif
00387                 _delta_time = TimeService::Instance()->secondsSince(_previous_time[axis]);
00388                 velocityValues[axis] = (axes[axis]->getSensor("Position")->readSensor()+init_pos[axis] - positionValues[axis])/_delta_time;
00389                 _previous_time[axis] = TimeService::Instance()->getTicks();
00390                 positionValues[axis] = axes[axis]->getSensor("Position")->readSensor()+init_pos[axis];
00391                 deltaTime_port.Set(_delta_time);
00392 #if defined OROPKG_OS_LXRT
00393             }else{
00394                 //Last Axes moves together with previous axes
00395                 _delta_time = TimeService::Instance()->secondsSince(_previous_time[axis]);
00396                 velocityValues[axis] = (axes[axis]->getSensor("Position")->readSensor()+init_pos[axis] - axes[axis-1]->getSensor("Position")->readSensor() - positionValues[axis])/_delta_time;
00397                 _previous_time[axis] = TimeService::Instance()->getTicks();
00398                 positionValues[axis] = axes[axis]->getSensor("Position")->readSensor()+init_pos[axis] - axes[axis-1]->getSensor("Position")->readSensor();
00399                 deltaTime_port.Set(_delta_time);
00400             }
00401 #endif
00402         }
00403         positionValues_port.Set(positionValues);
00404         velocityValues_port.Set(velocityValues);
00405 
00406 #if defined OROPKG_OS_LXRT
00407         double dt;
00408         // Determine sampling time :
00409         if (servoInitialized) {
00410             dt              = TimeService::Instance()->secondsSince(previousTime);
00411             previousTime    = TimeService::Instance()->getTicks();
00412         } else {
00413             dt = 0.0;
00414             for (unsigned int i=0;i<STAUBLIRX130_NUM_AXES;i++) {
00415                 servoIntError[i] = 0.0;
00416             }
00417             previousTime = TimeService::Instance()->getTicks();
00418             servoInitialized = true;
00419         }
00420 #endif
00421         driveValues_port.Get(driveValues);
00422 #if defined OROPKG_OS_LXRT
00423         // Last axis moves together with second last
00424         // Substract drive value as a feedforward
00425         if(!simulation)
00426             driveValues[5] = driveValues[5] + driveValues[4]/(DRIVEVALUE_5_TO_6);
00427 #endif
00428 
00429         for (unsigned int i=0;i<STAUBLIRX130_NUM_AXES;i++) {
00430 #if defined OROPKG_OS_LXRT
00431             if (driveFailure[i]->isOn()){
00432                 log(Error)<<"Failure drive "<<i<<", stopping all axes"<<endlog();
00433                 this->fatal();
00434             }
00435 #endif
00436 
00437             // emit event when velocity is out of range
00438             if( (velocityValues[i] < -velocityLimits_prop.value()[i]) ||
00439                 (velocityValues[i] > velocityLimits_prop.value()[i]) ){
00440                 char msg[80];
00441                 sprintf(msg,"Velocity of StaubliRX130 Axis %d is out of range: %f",i+1,velocityValues[i]);
00442                 velocityOutOfRange_evt(msg);
00443             }
00444 
00445             // emit event when position is out of range
00446             if( (positionValues[i] < lowerPositionLimits_prop.value()[i]) ||
00447                 (positionValues[i] > upperPositionLimits_prop.value()[i]) ){
00448                 char msg[80];
00449                 sprintf(msg,"Position of StaubliRX130 Axis %d is out of range: %f",i+1,positionValues[i]);
00450                 positionOutOfRange_evt(msg);
00451             }
00452 
00453             // send the drive value to hw and performs checks
00454             if (axes[i]->isDriven()){
00455 #if defined OROPKG_OS_LXRT
00456                 // perform control action ( dt is zero the first time !) :
00457                 double error        = driveValues[i] - velocityValues[i];
00458                 servoIntError[i]    += dt*error;
00459                 outputvel[i] = servoGain[i]*(error + servoIntegrationFactor[i]*servoIntError[i]) + servoFFScale[i]*driveValues[i];
00460             } else {
00461                 outputvel[i] = 0.0;
00462             }
00463             if( (outputvel[i] < -velocityLimits_prop.value()[i]) ||
00464                 (outputvel[i] > velocityLimits_prop.value()[i]) ){
00465                 char msg[80];
00466                 sprintf(msg,"Velocity of StaubliRX130 Axis %d is out of range: %f",i+1,velocityValues[i]);
00467                 velocityOutOfRange_evt(msg);
00468             }
00469 
00470             driveValues[i] = outputvel[i];
00471             axes[i]->drive(driveValues[i]);
00472         }
00473 
00474 #else
00475                 axes[i]->drive(driveValues[i]);
00476             }
00477         }
00478 
00479 #endif
00480         servoValues_port.Set(driveValues);
00481     }
00482 
00483 
00484     void StaubliRX130nAxesVelocityController::stopHook()
00485     {
00486         //Make sure machine is shut down
00487         prepareForShutdown();
00488     }
00489 
00490     void StaubliRX130nAxesVelocityController::cleanupHook()
00491     {
00492         //Write properties back to file
00493         marshalling()->writeProperties(this->getName()+".cpf");
00494     }
00495 
00496     bool StaubliRX130nAxesVelocityController::prepareForUse()
00497     {
00498 #if (defined OROPKG_OS_LXRT)
00499         if(!simulation){
00500             highPowerEnable->switchOn();
00501             log(Warning) <<"Release Emergency stop and push button to start ...."<<endlog();
00502         }
00503 #endif
00504         activated = true;
00505         return true;
00506     }
00507 
00508     bool StaubliRX130nAxesVelocityController::prepareForUseCompleted()const
00509     {
00510 #if (defined OROPKG_OS_LXRT)
00511         if(!simulation)
00512             return (!(eStop->isOn()) && armPowerOn->isOn());
00513         else
00514 #endif
00515             return true;
00516     }
00517 
00518     bool StaubliRX130nAxesVelocityController::prepareForShutdown()
00519     {
00520         //make sure all axes are stopped and locked
00521 #if (defined OROPKG_OS_LXRT)
00522         if(!simulation)
00523             highPowerEnable->switchOff();
00524 #endif
00525         stopAllAxes();
00526         lockAllAxes();
00527         activated = false;
00528         return true;
00529     }
00530 
00531     bool StaubliRX130nAxesVelocityController::prepareForShutdownCompleted()const
00532     {
00533         return true;
00534     }
00535 
00536     bool StaubliRX130nAxesVelocityController::stopAllAxes()
00537     {
00538         bool succes = true;
00539         for(unsigned int i = 0;i<STAUBLIRX130_NUM_AXES;i++)
00540             succes &= axes[i]->stop();
00541 
00542         return succes;
00543     }
00544 
00545     bool StaubliRX130nAxesVelocityController::startAllAxes()
00546     {
00547         bool succes = true;
00548         for(unsigned int i = 0;i<STAUBLIRX130_NUM_AXES;i++)
00549             succes &= axes[i]->drive(0.0);
00550 
00551         return succes;
00552     }
00553 
00554     bool StaubliRX130nAxesVelocityController::unlockAllAxes()
00555     {
00556         if(!activated)
00557             return false;
00558 
00559         bool succes = true;
00560         for(unsigned int i = 0;i<STAUBLIRX130_NUM_AXES;i++)
00561             succes &= axes[i]->unlock();
00562 #if defined OROPKG_OS_LXRT
00563         if (succes)
00564             brakeOff->switchOn();
00565 #endif
00566         return succes;
00567     }
00568 
00569     bool StaubliRX130nAxesVelocityController::lockAllAxes()
00570     {
00571         bool succes = true;
00572 #if defined OROPKG_OS_LXRT
00573         brakeOff->switchOff();
00574 #endif
00575     for(unsigned int i = 0;i<STAUBLIRX130_NUM_AXES;i++){
00576             succes &= axes[i]->lock();
00577         }
00578         return succes;
00579     }
00580 
00581 bool StaubliRX130nAxesVelocityController::addDriveOffset(const std::vector<double>& offset)
00582     {
00583         if(offset.size()!=STAUBLIRX130_NUM_AXES)
00584             return false;
00585 
00586         for(unsigned int i=0;i<STAUBLIRX130_NUM_AXES;i++){
00587             driveOffset_prop.value()[i] += offset[i];
00588 #if (defined OROPKG_OS_LXRT)
00589             if (!simulation)
00590                 ((Axis*)(axes[i]))->getDrive()->addOffset(offset[i]);
00591 #endif
00592         }
00593         return true;
00594     }
00595 
00596 
00597     bool StaubliRX130nAxesVelocityController::readAbsolutePosition(std::vector<double>& initPos)
00598     {
00599 #if (defined OROPKG_OS_LXRT)
00600         //Initialising serial communication
00601         port="/dev/ttyS0";
00602         fd=open(port,O_RDWR | O_NOCTTY | O_NDELAY);
00603         if(fd==-1)
00604             log(Critical)<<"Could not open serial communication with Adept Controller"<<endlog();
00605         fcntl(fd,F_SETFL,0);
00606 
00607         if(tcgetattr(fd,&tio)<0){
00608             log(Error)<<"Serial IO-error!!!"<<endlog();
00609             return false;
00610         }
00611         //setting parameters for communication
00612         //tio.c_lflag=0; //Non-canonical mode
00613         tio.c_cflag &= ~PARENB;
00614         tio.c_cflag &= ~CSTOPB;
00615         tio.c_cflag &= ~CSIZE;
00616         tio.c_cflag |= CS8;
00617         tio.c_cflag|=(CLOCAL | CREAD);
00618         if(cfsetispeed(&tio,B9600)<0&&cfsetospeed(&tio,B9600)<0){
00619             log(Error)<<"Could not set serial communication speed"<<endlog();
00620             return false;
00621         }
00622         tio.c_oflag &=~OPOST;
00623         tio.c_lflag &=~(ICANON|ECHO|ECHOE|ISIG);
00624         tio.c_cc[VMIN] = 0;
00625         tio.c_cc[VTIME]=10;
00626 
00627         tcflush(fd,TCIFLUSH);
00628         tcsetattr(fd,TCSANOW,&tio);
00629 
00630         char buffer[2048];/* Input buffer */
00631         char *bufptr;/* Current char in buffer */
00632         int  nbytes;/* Number of bytes read */
00633 
00634         //Send CR to see if Adept Controller is ready:
00635         write(fd,"\r",1);
00636         bufptr=buffer;
00637         while((nbytes=read(fd,bufptr,buffer+sizeof(buffer)-bufptr-1))>0){
00638             bufptr+=nbytes;
00639             if(bufptr[-1]=='\n'||bufptr[-1]=='\r')
00640                 break;
00641         }
00642         /* nul terminate the string and see if we got an OK response */
00643         *bufptr = '\0';
00644         log(Debug)<<"Received:"<<buffer<<endlog();
00645         char dot;
00646         sscanf(buffer,"%*s%s",&dot);
00647         if(dot=='.')
00648             log(Info)<<"Serial communication with Adept Controller OK."<<endlog();
00649         else{
00650             log(Error)<<"Serial communication with Adept Controller not ready!"<<endlog();
00651             return false;
00652         }
00653 
00654         //Get Absolute Position Values
00655         write(fd,"where\r",7);
00656         bufptr=buffer;
00657         unsigned int nlines=0;
00658         while((nbytes=read(fd,bufptr,buffer+sizeof(buffer)-bufptr-1))>0){
00659             bufptr+=nbytes;
00660             if(bufptr[-1]=='\n'||bufptr[-1]=='\r')
00661                 nlines++;
00662             if(nlines==4)
00663                 break;
00664         }
00665         *bufptr = '\0';
00666         log(Debug)<<"Received:"<<buffer<<endlog();
00667         sscanf(buffer,"%*s %*s %*s %*s %*s %*s %*s %*s %*f %*f %*f %*f %*f %*f %*f %*s %*s %*s %*s %*s %*s %lf %lf %lf %lf %lf %lf"
00668                ,&initPos[0],&initPos[1],&initPos[2],&initPos[3],&initPos[4],&initPos[5]);
00669         log(Info)<<"Absolute joint position in degrees: "<<initPos[0]<<", "<<initPos[1]<<", "<<initPos[2]
00670                  <<", "<<initPos[3]<<", "<<initPos[4]<<", "<<initPos[5]<<endlog();
00671 
00672         //Adding offsets
00673         initPos[1]+=90.0;
00674         initPos[2]-=90.0;
00675         //Converting to radians
00676         for(unsigned int i=0;i<initPos.size();i++)
00677             initPos[i]*=M_PI/180;
00678 #endif
00679         return true;
00680     }
00681 
00682 
00683 
00684 
00685 }//namespace orocos