OrocosComponentLibrary: PerformernAxesVelocityController.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 #include "PerformernAxesVelocityController.hpp"
00023 
00024 #include <ocl/ComponentLoader.hpp>
00025 
00026 #include <rtt/Logger.hpp>
00027 #include <sstream>
00028 
00029 namespace OCL
00030 {
00031 using namespace RTT;
00032 using namespace KDL;
00033 using namespace std;
00034 
00035 #define PERFORMERMK2_NUM_AXES 5
00036 
00037 #define PERFORMERMK2_ENCODEROFFSETS { 0, 0, 0, 0, 0 }
00038 
00039 //Encoder counts for pi/2 radians
00040 #define PERFORMERMK2_CONV1  60000
00041 #define PERFORMERMK2_CONV2  80000
00042 #define PERFORMERMK2_CONV3  72000
00043 #define PERFORMERMK2_CONV4  60000
00044 #define PERFORMERMK2_CONV5  44000
00045 
00046 // Conversion from encoder ticks to radiants
00047 #define PERFORMERMK2_TICKS2RAD { M_PI_2 / PERFORMERMK2_CONV1 ,M_PI_2 / PERFORMERMK2_CONV2 ,M_PI_2 / PERFORMERMK2_CONV3 ,M_PI_2 / PERFORMERMK2_CONV4 ,M_PI_2 / PERFORMERMK2_CONV5 }
00048 
00049 // Conversion from angular speed to voltage
00050 #define PERFORMERMK2_RADproSEC2VOLT { 5.25, 12.84, 12.22, 4.4, 3.1}
00051 
00052 // Velocity estimator parameters
00053 
00054 // maximum number of backward steps
00055 # define m 10
00056 // relative accuracy
00057 # define rj 0.02
00058 # define sj 2/rj
00059 # define pi 3.14159265359
00060 # define R1 pi/2/PERFORMERMK2_CONV1
00061 # define R2 pi/2/PERFORMERMK2_CONV2
00062 # define R3 pi/2/PERFORMERMK2_CONV3
00063 # define R4 pi/2/PERFORMERMK2_CONV4
00064 # define R5 pi/2/PERFORMERMK2_CONV5
00065 
00066 typedef PerformerMK2nAxesVelocityController MyType;
00067 
00068 PerformerMK2nAxesVelocityController::PerformerMK2nAxesVelocityController(string name,string _propertyfile)
00069 : TaskContext(name,PreOperational),
00070 resetController_mtd( "resetController", &MyType::resetController, this),
00071 startAllAxes_mtd( "startAllAxes", &MyType::startAllAxes, this),
00072 stopAllAxes_mtd( "stopAllAxes", &MyType::stopAllAxes, this),
00073 unlockAllAxes_mtd( "unlockAllAxes", &MyType::unlockAllAxes, this),
00074 lockAllAxes_mtd( "lockAllAxes", &MyType::lockAllAxes, this),
00075 prepareForUse_cmd( "prepareForUse", &MyType::prepareForUse,&MyType::prepareForUseCompleted, this),
00076 prepareForShutdown_cmd( "prepareForShutdown", &MyType::prepareForShutdown,&MyType::prepareForShutdownCompleted, this),
00077 addDriveOffset_mtd( "addDriveOffset", &MyType::addDriveOffset, this),
00078 _initPosition( "initPosition", &MyType::initPosition, this),
00079 driveValues_port("nAxesOutputVelocity"),
00080 servoValues_port("nAxesServoVelocity"),
00081 positionValues_port("nAxesSensorPosition"),
00082 velocityValues_port("nAxesSensorVelocity"),
00083 jValues_port("nAxesjValues"),
00084 homingSwitchValues_port("nAxesSensorHomingSwitch"),
00085 deltaTime_port("DeltaTime"),
00086 driveLimits_prop("driveLimits","velocity limits of the axes, (rad/s)",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00087 lowerPositionLimits_prop("LowerPositionLimits","Lower position limits (rad)",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00088 upperPositionLimits_prop("UpperPositionLimits","Upper position limits (rad)",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00089 velocityLimits_prop("velocityLimits","velocity limits of the axes, (rad/s)",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00090 initialPosition_prop("initialPosition","Initial position (rad) for simulation or hardware",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00091 driveOffset_prop("driveOffset","offset (in rad/s) to the drive value.",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00092 simulation_prop("simulation","true if simulationAxes should be used",true),
00093 simulation(true),
00094 //servoIntegrationFactor_prop("ServoIntegrationFactor","Inverse of Integration time for servoloop",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00095 //servoGain_prop("ServoGain","Feedback Gain for servoloop",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00096 servoFFScale_prop("ServoFFScale","Feedforward scale for servoloop",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00097 PIDkp_prop("PIDkp","PID proportional gain",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00098 PIDTi_prop("PIDTi","PID integration time",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00099 PIDTd_prop("PIDTd","PID differentiation time",vector<double>(PERFORMERMK2_NUM_AXES,0)),
00100 num_axes_attr("NUM_AXES",PERFORMERMK2_NUM_AXES),
00101 chain_attr("Kinematics"),
00102 driveOutOfRange_evt("driveOutOfRange"),
00103 positionOutOfRange_evt("positionOutOfRange"),
00104 velocityOutOfRange_evt("velocityOutOfRange"),
00105 propertyfile(_propertyfile),
00106 activated(false),
00107 positionConvertFactor(PERFORMERMK2_NUM_AXES,0),
00108 driveConvertFactor(PERFORMERMK2_NUM_AXES,0),
00109 positionValues(PERFORMERMK2_NUM_AXES,0),
00110 driveValues(PERFORMERMK2_NUM_AXES,0),
00111 velocityValues(PERFORMERMK2_NUM_AXES,0),
00112 jValues(PERFORMERMK2_NUM_AXES,0),
00113 positionDeque_axis1(m,0),
00114 positionDeque_axis2(m,0),
00115 positionDeque_axis3(m,0),
00116 positionDeque_axis4(m,0),
00117 positionDeque_axis5(m,0),
00118 timeDeque(m,0),
00119 homingSwitchValues(PERFORMERMK2_NUM_AXES,0),
00120 outputvel(PERFORMERMK2_NUM_AXES,0),
00121 outputvel_kmin1(PERFORMERMK2_NUM_AXES,0),
00122 outputvel_kmin2(PERFORMERMK2_NUM_AXES,0),
00123 velocity_error_k(PERFORMERMK2_NUM_AXES,0),
00124 velocity_error_kmin1(PERFORMERMK2_NUM_AXES,0),
00125 velocity_error_kmin2(PERFORMERMK2_NUM_AXES,0),
00126 position_error_k(PERFORMERMK2_NUM_AXES,0),
00127 position_desired_k(PERFORMERMK2_NUM_AXES,0),
00128 #if (defined OROPKG_OS_LXRT)
00129 axes_hardware(PERFORMERMK2_NUM_AXES),
00130 encoderInterface(PERFORMERMK2_NUM_AXES),
00131 encoder(PERFORMERMK2_NUM_AXES),
00132 vref(PERFORMERMK2_NUM_AXES),
00133 enable(PERFORMERMK2_NUM_AXES),
00134 drive(PERFORMERMK2_NUM_AXES),
00135 homingSwitch(PERFORMERMK2_NUM_AXES),
00136 #endif
00137 axes(PERFORMERMK2_NUM_AXES)
00138 //_previous_time(PERFORMERMK2_NUM_AXES)
00139 {
00140 Logger::In in(this->getName().data());
00141 double ticks2rad[PERFORMERMK2_NUM_AXES] = PERFORMERMK2_TICKS2RAD;
00142 double vel2volt[PERFORMERMK2_NUM_AXES] = PERFORMERMK2_RADproSEC2VOLT;
00143 for(unsigned int i = 0;i<PERFORMERMK2_NUM_AXES;i++){
00144 positionConvertFactor[i] = ticks2rad[i];
00145 driveConvertFactor[i] = vel2volt[i];
00146 }
00147 
00148 #if (defined OROPKG_OS_LXRT)
00149     double encoderOffsets[PERFORMERMK2_NUM_AXES] = PERFORMERMK2_ENCODEROFFSETS;
00150 
00151     log(Info)<<"Creating Comedi Devices."<<endlog();
00152     NI6713 = new ComediDevice(0);
00153     NI6602 = new ComediDevice(2);
00154 
00155     SubAOut_NI6713 = new ComediSubDeviceAOut(NI6713,"DriveValues",1);
00156     SubDIn_NI6713 = new ComediSubDeviceDIn(NI6713,"DigitalIn",2);
00157     SubDOut_NI6602 = new ComediSubDeviceDOut(NI6602,"DigitalOut",1);
00158 
00159     brakeAxis2 = new DigitalOutput(SubDOut_NI6602,0,true);
00160     brakeAxis2->switchOn();
00161     brakeAxis3 = new DigitalOutput(SubDOut_NI6602,1,true);
00162     brakeAxis3->switchOn();
00163 
00164     armPowerOn = new DigitalInput(SubDIn_NI6713,6);
00165     armPowerEnable = new DigitalOutput(SubDOut_NI6602,7);
00166     armPowerEnable->switchOff();
00167 
00168 
00169     for (unsigned int i = 0; i < PERFORMERMK2_NUM_AXES; i++){
00170     log(Info)<<"Creating Hardware axis "<<i<<endlog();
00171     //Setting up encoders
00172     log(Info)<<"Setting up encoder ..."<<endlog();
00173     encoderInterface[i] = new ComediEncoder(NI6602,0,i);
00174     encoder[i] = new IncrementalEncoderSensor( encoderInterface[i], 1.0 / ticks2rad[i],
00175                         encoderOffsets[i],
00176                         -10, 10,0 );
00177     //Setting up drive
00178     log(Info)<<"Setting up drive ..."<<endlog();
00179     //      if(i==2)
00180     //        vref[i]   = new AnalogOutput(SubAOut_NI6713, 5 );
00181     //else
00182     vref[i]   = new AnalogOutput(SubAOut_NI6713, i );
00183     enable[i] = new DigitalOutput( SubDOut_NI6602, 2+i );
00184     enable[i]->switchOff();
00185     drive[i]  = new AnalogDrive( vref[i], enable[i], 1.0 / vel2volt[i], 0.0);
00186 
00187     axes_hardware[i] = new Axis( drive[i] );
00188     axes_hardware[i]->setSensor( "Position", encoder[i] );
00189     if(i==1)
00190     axes_hardware[i]->setBrake(brakeAxis2);
00191     if(i==2)
00192     axes_hardware[i]->setBrake(brakeAxis3);
00193 
00194     //Setting up homeswitches
00195     homingSwitch[i] = new DigitalInput(SubDIn_NI6713,i);
00196     axes_hardware[i]->setSwitch("HomingSwitch",homingSwitch[i]);
00197 
00198     }
00199 
00200 #endif
00201     //Definition of kinematics for the PerformerMK2
00202     kinematics.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.100,0.0,0.360))));
00203     kinematics.addSegment(Segment(Joint(Joint::RotY),Frame(Vector(0.0,0.0,0.270))));
00204     kinematics.addSegment(Segment(Joint(Joint::RotY),Frame(Vector(0.0,0.0,0.230))));
00205     kinematics.addSegment(Segment(Joint(Joint::RotY),Frame(Vector(0.0,0.0,0.100))));
00206     kinematics.addSegment(Segment(Joint(Joint::RotZ)));
00207 
00208     chain_attr.set(kinematics);
00209 
00210     /*
00211     *  Execution Interface
00212     */
00213     methods()->addMethod( &resetController_mtd, "reset Velocity Controller","axis_id","axis to reset");
00214     methods()->addMethod( &startAllAxes_mtd, "start all axes"  );
00215     methods()->addMethod( &stopAllAxes_mtd, "stops all axes"  );
00216     methods()->addMethod( &lockAllAxes_mtd, "locks all axes"  );
00217     methods()->addMethod( &unlockAllAxes_mtd, "unlock all axes"  );
00218     commands()->addCommand( &prepareForUse_cmd, "prepares the robot for use"  );
00219     commands()->addCommand( &prepareForShutdown_cmd,"prepares the robot for shutdown"  );
00220     methods()->addMethod( &addDriveOffset_mtd,"adds an offset to the drive value of axis","offset","offset values in rad/s" );
00221     methods()->addMethod( &_initPosition,"changes position value to the initial position","axis","axis to initialize" );
00222     events()->addEvent( &driveOutOfRange_evt, "Drive value of an Axis is out of range","message","Information about event" );
00223     events()->addEvent( &positionOutOfRange_evt, "Position of an Axis is out of range","message","Information about event");
00224     events()->addEvent( &velocityOutOfRange_evt, "Velocity of an Axis is out of range","message","Information about event");
00225 
00230     ports()->addPort(&driveValues_port);
00231     ports()->addPort(&servoValues_port);
00232     ports()->addPort(&positionValues_port);
00233     ports()->addPort(&velocityValues_port);
00234     ports()->addPort(&jValues_port);
00235     ports()->addPort(&homingSwitchValues_port);
00236     ports()->addPort(&deltaTime_port);
00237 
00241     properties()->addProperty( &driveLimits_prop);
00242     properties()->addProperty( &lowerPositionLimits_prop);
00243     properties()->addProperty( &upperPositionLimits_prop);
00244     properties()->addProperty( &velocityLimits_prop);
00245     properties()->addProperty( &initialPosition_prop);
00246     properties()->addProperty( &driveOffset_prop);
00247     properties()->addProperty( &simulation_prop);
00248     //properties()->addProperty( &servoIntegrationFactor_prop);
00249     //properties()->addProperty( &servoGain_prop);
00250     properties()->addProperty( &servoFFScale_prop);
00251     properties()->addProperty( &PIDkp_prop);
00252     properties()->addProperty( &PIDTi_prop);
00253     properties()->addProperty( &PIDTd_prop);
00254     attributes()->addConstant( &num_axes_attr);
00255     attributes()->addAttribute(&chain_attr);
00256 
00257 }
00258 
00259 PerformerMK2nAxesVelocityController::~PerformerMK2nAxesVelocityController()
00260 {
00261     // make sure robot is shut down
00262     prepareForShutdown_cmd();
00263 
00264     // brake, drive, sensors and switches are deleted by each axis
00265     if(simulation)
00266     for (unsigned int i = 0; i < PERFORMERMK2_NUM_AXES; i++)
00267         delete axes[i];
00268 
00269 #if (defined OROPKG_OS_LXRT)
00270     for (unsigned int i = 0; i < PERFORMERMK2_NUM_AXES; i++){
00271     delete axes_hardware[i];
00272     }
00273     delete armPowerOn;
00274     delete armPowerEnable;
00275 
00276     delete SubAOut_NI6713;
00277     delete SubDIn_NI6713;
00278     delete SubDOut_NI6602;
00279 
00280     delete NI6602;
00281     delete NI6713;
00282 #endif
00283 }
00284 
00285 
00286 bool PerformerMK2nAxesVelocityController::configureHook()
00287 {
00288 Logger::In in(this->getName().data());
00289 
00290 if (!marshalling()->readProperties(propertyfile)) {
00291     log(Error) << "Failed to read the property file, continueing with default values." << endlog();
00292     return false;
00293     }
00294     simulation=simulation_prop.value();
00295 
00296     if(!(driveLimits_prop.value().size()==PERFORMERMK2_NUM_AXES&&
00297     lowerPositionLimits_prop.value().size()==PERFORMERMK2_NUM_AXES&&
00298     upperPositionLimits_prop.value().size()==PERFORMERMK2_NUM_AXES&&
00299     velocityLimits_prop.value().size()==PERFORMERMK2_NUM_AXES&&
00300     initialPosition_prop.value().size()==PERFORMERMK2_NUM_AXES&&
00301     driveOffset_prop.value().size()==PERFORMERMK2_NUM_AXES)){
00302     log(Error) << "Properties of invalid size" << endlog();
00303     return false;
00304     }
00305 
00306 #if (defined OROPKG_OS_LXRT)
00307     if(!simulation){
00308 
00309     for (unsigned int i = 0; i <PERFORMERMK2_NUM_AXES; i++){
00310     axes_hardware[i]->limitDrive(-driveLimits_prop.value()[i], driveLimits_prop.value()[i], driveOutOfRange_evt);
00311     axes[i] = axes_hardware[i];
00312     ((Axis*)(axes[i]))->getDrive()->addOffset(driveOffset_prop.value()[i]);
00313     log(Info) << "Hardware version of PerformerMK2nAxesVelocityController has started" << endlog();
00314     }
00315 
00316     }
00317     else{
00318 #endif
00319     for (unsigned int i = 0; i <PERFORMERMK2_NUM_AXES; i++)
00320         axes[i] = new SimulationAxis(initialPosition_prop.value()[i],
00321                     lowerPositionLimits_prop.value()[i],
00322                     upperPositionLimits_prop.value()[i]);
00323     log(Info) << "Simulation version of PerformerMK2nAxesVelocityController has started" << endlog();
00324 #if (defined OROPKG_OS_LXRT)
00325     }
00326 #endif
00327 
00330     //servoGain = servoGain_prop.value();
00331     //servoIntegrationFactor = servoIntegrationFactor_prop.value();
00332     servoFFScale = servoFFScale_prop.value();
00333 
00338     PIDkp = PIDkp_prop.value();
00339     PIDTi = PIDTi_prop.value();
00340     PIDTd = PIDTd_prop.value();
00341 
00342     return true;
00343 }
00344 
00345 bool PerformerMK2nAxesVelocityController::startHook()
00346 {
00347   previous_time = TimeService::Instance()->getTicks();
00348 
00349   for (int axis=0;axis<PERFORMERMK2_NUM_AXES;axis++) {
00350     positionValues[axis] = axes[axis]->getSensor("Position")->readSensor();
00351     position_desired_k[axis] = position_desired_k[axis];
00352     if(!simulation)
00353       homingSwitchValues[axis] = axes[axis]->getSwitch("HomingSwitch")->isOn();
00354     else
00355       homingSwitchValues[axis] = true;
00356 
00357   }
00358 
00359     for (int i=0;i<m;i++) {
00360     positionDeque_axis1[i] = positionValues[0];
00361     positionDeque_axis2[i] = positionValues[1];
00362     positionDeque_axis3[i] = positionValues[2];
00363     positionDeque_axis4[i] = positionValues[3];
00364     positionDeque_axis5[i] = positionValues[4];
00365     timeDeque[i] = 0.0;
00366     }
00367 
00368 
00369     positionValues_port.Set(positionValues);
00370     homingSwitchValues_port.Set(homingSwitchValues);
00371     return true;
00372 }
00373 
00374 void PerformerMK2nAxesVelocityController::updateHook()
00375 {
00376 delta_time = TimeService::Instance()->secondsSince(previous_time);
00377 total_time = TimeService::Instance()->secondsSince(initial_time);
00378 previous_time = TimeService::Instance()->getTicks();
00379 deltaTime_port.Set(delta_time);
00380 
00381 for (int axis=0;axis<PERFORMERMK2_NUM_AXES;axis++) {
00382     // Euler approximation velocity estimator
00383     // velocityValues[axis] = (axes[axis]->getSensor("Position")->readSensor() - positionValues[axis])/delta_time;
00384     positionValues[axis] = axes[axis]->getSensor("Position")->readSensor();
00385     if(!simulation)
00386       homingSwitchValues[axis] = axes[axis]->getSwitch("HomingSwitch")->isOn();
00387     else
00388       homingSwitchValues[axis] = true;
00389         }
00390 
00391     positionDeque_axis1.push_front(positionValues[0]);
00392     positionDeque_axis1.pop_back();
00393     positionDeque_axis2.push_front(positionValues[1]);
00394     positionDeque_axis2.pop_back();
00395     positionDeque_axis3.push_front(positionValues[2]);
00396     positionDeque_axis3.pop_back();
00397     positionDeque_axis4.push_front(positionValues[3]);
00398     positionDeque_axis4.pop_back();
00399     positionDeque_axis5.push_front(positionValues[4]);
00400     positionDeque_axis5.pop_back();
00401     timeDeque.push_front(total_time);
00402     timeDeque.pop_back();
00403 
00404     for (int j=1;j<m;j++) {
00405         velocityValues[0] = (positionDeque_axis1[0]-positionDeque_axis1[m-1])/(timeDeque[0]-timeDeque[m-1]);
00406         if(abs(positionDeque_axis1[0]-positionDeque_axis1[j])>sj*R1){
00407             velocityValues[0] = (positionDeque_axis1[0]-positionDeque_axis1[j])/(timeDeque[0]-timeDeque[j]);
00408             jValues[0] = j;
00409             break;
00410         }
00411     }
00412 
00413     for (int j=1;j<m;j++) {
00414         velocityValues[1] = (positionDeque_axis2[0]-positionDeque_axis2[m-1])/(timeDeque[0]-timeDeque[m-1]);
00415         if(abs(positionDeque_axis2[0]-positionDeque_axis2[j])>sj*R2){
00416             velocityValues[1] = (positionDeque_axis2[0]-positionDeque_axis2[j])/(timeDeque[0]-timeDeque[j]);
00417             jValues[1] = j;
00418             break;
00419         }
00420     }
00421 
00422     for (int j=1;j<m;j++) {
00423         velocityValues[2] = (positionDeque_axis3[0]-positionDeque_axis3[m-1])/(timeDeque[0]-timeDeque[m-1]);
00424         if(abs(positionDeque_axis3[0]-positionDeque_axis3[j])>sj*R3){
00425             velocityValues[2] = (positionDeque_axis3[0]-positionDeque_axis3[j])/(timeDeque[0]-timeDeque[j]);
00426             jValues[2] = j;
00427             break;
00428         }
00429     }
00430 
00431     for (int j=1;j<m;j++) {
00432         velocityValues[3] = (positionDeque_axis4[0]-positionDeque_axis4[m-1])/(timeDeque[0]-timeDeque[m-1]);
00433         if(abs(positionDeque_axis4[0]-positionDeque_axis4[j])>sj*R4) {
00434             velocityValues[3] = (positionDeque_axis4[0]-positionDeque_axis4[j])/(timeDeque[0]-timeDeque[j]);
00435             jValues[3] = j;
00436             break;
00437         }
00438     }
00439 
00440     for (int j=1;j<m;j++) {
00441         velocityValues[4] = (positionDeque_axis5[0]-positionDeque_axis5[m-1])/(timeDeque[0]-timeDeque[m-1]);
00442         if(abs(positionDeque_axis5[0]-positionDeque_axis5[j])>sj*R5){
00443             velocityValues[4] = (positionDeque_axis5[0]-positionDeque_axis5[j])/(timeDeque[0]-timeDeque[j]);
00444             jValues[4] = j;
00445             break;
00446         }
00447     }
00448 
00449 
00450     positionValues_port.Set(positionValues);
00451     velocityValues_port.Set(velocityValues);
00452     jValues_port.Set(jValues);
00453     homingSwitchValues_port.Set(homingSwitchValues);
00454 
00455 #if defined OROPKG_OS_LXRT
00456     //double dt;
00457     // Determine sampling time :
00458     //if (servoInitialized) {
00459     //  dt              = TimeService::Instance()->secondsSince(previousTime);
00460     //  previousTime    = TimeService::Instance()->getTicks();
00461     //} else {
00462     //  dt = 0.0;
00463     //  for (unsigned int i=0;i<PERFORMERMK2_NUM_AXES;i++) {
00464     //    servoIntError[i] = 0.0;
00465     //  }
00466     //  previousTime = TimeService::Instance()->getTicks();
00467     //  servoInitialized = true;
00468     //}
00469 #endif
00470     driveValues_port.Get(driveValues);
00471 
00472     for (unsigned int i=0;i<PERFORMERMK2_NUM_AXES;i++) {
00473     // emit event when velocity is out of range
00474     if( (velocityValues[i] < -velocityLimits_prop.value()[i]) ||
00475     (velocityValues[i] > velocityLimits_prop.value()[i]) )
00476     {
00477     stringstream msg;
00478     msg<<"Velocity of PerformerMK2 Axis "<< i <<" is out of range: "<<velocityValues[i];
00479     log(Warning)<<msg.str()<<endlog();
00480     velocityOutOfRange_evt(msg.str()+"\n");
00481     }
00482     // emit event when position is out of range
00483     if( (positionValues[i] < lowerPositionLimits_prop.value()[i]) ||
00484     (positionValues[i] > upperPositionLimits_prop.value()[i]) ){
00485     stringstream msg;
00486     msg<<"Position of PerformerMK2 Axis "<<i<<" is out of range: "<<positionValues[i];
00487     log(Warning)<<msg.str()<<endlog();
00488     positionOutOfRange_evt(msg.str()+"\n");
00489     }
00490 
00491     // send the drive value to hw and performs checks
00492     if (axes[i]->isDriven()){
00493 #if defined OROPKG_OS_LXRT
00494       // perform control action ( dt is zero the first time !) :
00495       //   double error        = driveValues[i] - velocityValues[i];
00496       // servoIntError[i]    += dt*error;
00497       //outputvel[i] = servoGain[i]*(error + servoIntegrationFactor[i]*servoIntError[i]) + servoFFScale[i]*driveValues[i];
00498       if ((i==0 || i==3 || i==4)&&!simulation ){
00499         velocity_error_kmin2[i] = velocity_error_kmin1[i];
00500         velocity_error_kmin1[i] = velocity_error_k[i];
00501         velocity_error_k[i] = driveValues[i]-velocityValues[i];
00502         position_desired_k[i] = position_desired_k[i] + driveValues[i]*delta_time;
00503         position_error_k[i] = position_desired_k[i]-positionValues[i];
00504         outputvel_kmin2[i] =  outputvel_kmin1[i];
00505         outputvel_kmin1[i] =  outputvel[i];
00506 
00507         // outputvel by discretizing PID controller (design by emulation)
00508         //outputvel[i] = PIDkp[i]*(velocity_error_k[i]-velocity_error_kmin2[i])+(1/2)*delta_time*velocity_error_k[i]*PIDkp[i]/PIDTi[i]+delta_time*velocity_error_kmin1[i]*PIDkp[i]/PIDTi[i]+(1/2)*delta_time*velocity_error_kmin2[i]*PIDkp[i]/(PIDTi[i])+2*velocity_error_k[i]*PIDkp[i]*PIDTd[i]/delta_time-4*velocity_error_kmin1[i]*PIDkp[i]*PIDTd[i]/delta_time+2*velocity_error_kmin2[i]*PIDkp[i]*PIDTd[i]/delta_time+outputvel_kmin2[i];
00509 
00510         // outputvel for PI controller:
00511         outputvel[i] = PIDkp[i]*(velocity_error_k[i]+1.0/PIDTi[i]*position_error_k[i]);
00512 
00513       }
00514       else{
00515         outputvel[i] = 0.0;
00516       }
00517 
00518       driveValues[i] = outputvel[i]+servoFFScale[i]*driveValues[i];
00519     }
00520     else {
00521       outputvel[i] = 0.0;
00522       driveValues[i] = outputvel[i];
00523     }
00524     axes[i]->drive(driveValues[i]);
00525 #else
00526     axes[i]->drive(driveValues[i]);
00527     }
00528 
00529 #endif
00530 }
00531 servoValues_port.Set(driveValues);
00532 }
00533 
00534 
00535 
00536 void PerformerMK2nAxesVelocityController::stopHook()
00537 {
00538     //Make sure machine is shut down
00539     prepareForShutdown();
00540 }
00541 
00542 void PerformerMK2nAxesVelocityController::cleanupHook()
00543 {
00544     //Write properties back to file
00545     marshalling()->writeProperties(propertyfile);
00546 }
00547 
00548 bool PerformerMK2nAxesVelocityController::resetController(const int& axis_id)
00549 {
00550     bool succes = false;
00551         if(axis_id < 5 && axis_id >= 0){
00552             succes = true;
00553         velocity_error_kmin2[axis_id] = 0.0;
00554         velocity_error_kmin1[axis_id] = 0.0;
00555         velocity_error_k[axis_id] = 0.0;
00556             outputvel_kmin1[axis_id] = 0.0;
00557             outputvel_kmin2[axis_id] = 0.0;
00558         positionValues[axis_id] = axes[axis_id]->getSensor("Position")->readSensor();
00559         position_desired_k[axis_id] = positionValues[axis_id];
00560 
00561         if (axis_id == 0){
00562             for (int i=0;i<m;i++) {
00563                 positionDeque_axis1[i] = positionValues[0];
00564                 }
00565         }
00566         if (axis_id == 1){
00567             for (int i=0;i<m;i++) {
00568                 positionDeque_axis2[i] = positionValues[1];
00569                 }
00570         }
00571             if (axis_id == 2){
00572             for (int i=0;i<m;i++) {
00573                 positionDeque_axis3[i] = positionValues[2];
00574                 }
00575         }
00576         if (axis_id == 3){
00577             for (int i=0;i<m;i++) {
00578                 positionDeque_axis4[i] = positionValues[3];
00579                 }
00580         }
00581             if (axis_id == 4){
00582             for (int i=0;i<m;i++) {
00583                 positionDeque_axis5[i] = positionValues[4];
00584                 }
00585         }
00586     }
00587 
00588         return succes;
00589 }
00590 
00591 bool PerformerMK2nAxesVelocityController::prepareForUse()
00592 {
00593 #if (defined OROPKG_OS_LXRT)
00594 if(!simulation){
00595     armPowerEnable->switchOn();
00596     log(Warning) <<"Release Emergency stop and push button to start ...."<<endlog();
00597 }
00598 #endif
00599 activated = true;
00600 return true;
00601 }
00602 
00603 bool PerformerMK2nAxesVelocityController::prepareForUseCompleted()const
00604 {
00605 #if (defined OROPKG_OS_LXRT)
00606     if(!simulation)
00607     return armPowerOn->isOn();
00608     else
00609 #endif
00610     {
00611 
00612     return true;
00613     }
00614 
00615 }
00616 
00617 bool PerformerMK2nAxesVelocityController::prepareForShutdown()
00618 {
00619   //make sure all axes are stopped and locked
00620   stopAllAxes();
00621   lockAllAxes();
00622 #if (defined OROPKG_OS_LXRT)
00623   armPowerEnable->switchOff();
00624 #endif
00625   activated = false;
00626   return true;
00627 }
00628 
00629 bool PerformerMK2nAxesVelocityController::prepareForShutdownCompleted()const
00630 {
00631     return true;
00632 }
00633 
00634 bool PerformerMK2nAxesVelocityController::stopAllAxes()
00635 {
00636     bool succes = true;
00637     for(unsigned int i = 0;i<PERFORMERMK2_NUM_AXES;i++)
00638     succes &= axes[i]->stop();
00639 
00640     return succes;
00641 }
00642 
00643 
00644 
00645 bool PerformerMK2nAxesVelocityController::startAllAxes()
00646 {
00647     bool succes = true;
00648     for(unsigned int i = 0;i<PERFORMERMK2_NUM_AXES;i++)
00649     succes &= axes[i]->drive(0.0);
00650     if(!succes)
00651     stopAllAxes();
00652 
00653     return succes;
00654 }
00655 
00656 bool PerformerMK2nAxesVelocityController::unlockAllAxes()
00657 {
00658     if(!activated)
00659     return false;
00660 
00661     bool succes = true;
00662     for(unsigned int i = 0;i<PERFORMERMK2_NUM_AXES;i++)
00663     succes &= axes[i]->unlock();
00664     if(!succes)
00665     lockAllAxes();
00666 
00667     return succes;
00668 }
00669 
00670 bool PerformerMK2nAxesVelocityController::lockAllAxes()
00671 {
00672     bool succes = true;
00673     for(unsigned int i = 0;i<PERFORMERMK2_NUM_AXES;i++){
00674     succes &= axes[i]->lock();
00675     }
00676     return succes;
00677 }
00678 
00679 bool PerformerMK2nAxesVelocityController::addDriveOffset(const vector<double>& offset)
00680 {
00681     if(offset.size()!=PERFORMERMK2_NUM_AXES)
00682     return false;
00683 
00684     for(unsigned int i=0;i<PERFORMERMK2_NUM_AXES;i++){
00685     driveOffset_prop.value()[i] += offset[i];
00686 #if (defined OROPKG_OS_LXRT)
00687     if (!simulation)
00688         ((Axis*)(axes[i]))->getDrive()->addOffset(offset[i]);
00689 #endif
00690     }
00691     return true;
00692 }
00693     bool PerformerMK2nAxesVelocityController::initPosition(int axis)
00694     {
00695 #if (defined OROPKG_OS_LXRT)
00696         if(!simulation)
00697             ((IncrementalEncoderSensor*)axes[axis]->getSensor("Position"))->writeSensor(initialPosition_prop.value()[axis]);
00698 #endif
00699         return true;
00700     }
00701 
00702     bool PerformerMK2nAxesVelocityController::initPositionCompleted(int axis)const
00703     {
00704         return true;
00705     }
00706 
00707 
00708 }//namespace orocos
00709 ORO_CREATE_COMPONENT(OCL::PerformerMK2nAxesVelocityController)