00001 #ifndef KUKA361_NAXES_TORQUE_CONTROLLER_HPP
00002 #define KUKA361_NAXES_TORQUE_CONTROLLER_HPP
00003
00004 #include <rtt/RTT.hpp>
00005
00006 #include <vector>
00007 #include <rtt/RTT.hpp>
00008
00009 #include <rtt/TaskContext.hpp>
00010 #include <rtt/Ports.hpp>
00011 #include <rtt/Event.hpp>
00012 #include <rtt/Properties.hpp>
00013 #include <rtt/Command.hpp>
00014 #include <rtt/Method.hpp>
00015
00016 #if (defined (OROPKG_OS_LXRT))
00017 #include "dev/SwitchDigitalInapci1032.hpp"
00018 #include "dev/RelayCardapci2200.hpp"
00019 #include "dev/EncoderSSIapci1710.hpp"
00020 #include "dev/ComediDevice.hpp"
00021 #include "dev/ComediSubDeviceAOut.hpp"
00022 #include "dev/ComediSubDeviceAIn.hpp"
00023 #include "dev/ComediSubDeviceDOut.hpp"
00024 #include "dev/ComediSubDeviceDIn.hpp"
00025 #include "dev/AbsoluteEncoderSensor.hpp"
00026 #include "dev/AnalogDrive.hpp"
00027 #include "dev/AnalogSensor.hpp"
00028 #include "dev/Axis.hpp"
00029
00030 #include <rtt/dev/AnalogOutput.hpp>
00031 #include <rtt/dev/AnalogInput.hpp>
00032 #include <rtt/dev/DigitalOutput.hpp>
00033 #include <rtt/dev/DigitalInput.hpp>
00034
00035
00036 #endif
00037
00038 #include "dev/SimulationAxis.hpp"
00039 #include "dev/TorqueSimulationAxis.hpp"
00040 #include <rtt/dev/AxisInterface.hpp>
00041 #include "Kuka361TorqueSimulator.hpp"
00042
00043 #include <ocl/OCL.hpp>
00044
00045 namespace OCL
00046 {
00057 class Kuka361nAxesTorqueController : public RTT::TaskContext
00058 {
00059 public:
00068 Kuka361nAxesTorqueController(std::string name);
00069 virtual ~Kuka361nAxesTorqueController();
00070
00071 protected:
00077 RTT::DataPort<std::vector<double> > driveValues;
00078 std::vector<double> driveValues_local;
00085 RTT::DataPort<std::vector<double> > positionValues;
00086 std::vector<double> positionValues_local;
00087
00093 RTT::DataPort<std::vector<double> > velocityValues;
00094 std::vector<double> velocityValues_local;
00100 RTT::DataPort<std::vector<double> > torqueValues;
00101 std::vector<double> torqueValues_local;
00106 RTT::WriteDataPort< double > delta_time_Port;
00107
00108
00114 RTT::Property<std::vector <double> > velocityLimits;
00115
00121 RTT::Property<std::vector <double> > currentLimits;
00122
00123
00127 RTT::Property<std::vector <double> > lowerPositionLimits;
00128
00132 RTT::Property<std::vector <double> > upperPositionLimits;
00133
00138 RTT::Property<std::vector <double> > initialPosition;
00139
00144 RTT::Property<std::vector <double> > velDriveOffset;
00145
00150 RTT::Property<bool> simulation;
00151
00155 RTT::Constant<unsigned int> num_axes;
00156
00162 RTT::Event< void(std::string) > velocityOutOfRange;
00163
00169 RTT::Event< void(std::string) > currentOutOfRange;
00170
00177 RTT::Event< void(std::string) > positionOutOfRange;
00178
00179 private:
00180
00181 virtual bool startAllAxes();
00182
00183 virtual bool stopAllAxes();
00184
00185 virtual bool lockAllAxes();
00186
00187 virtual bool unlockAllAxes();
00188
00189 virtual bool addDriveOffset(const std::vector<double>& offset);
00190
00191 virtual bool prepareForUse();
00192 virtual bool prepareForUseCompleted() const;
00193 virtual bool prepareForShutdown();
00194 virtual bool prepareForShutdownCompleted() const;
00195
00196
00200 RTT::Property<bool> velresolved;
00201
00205 bool TorqueMode;
00206
00207
00211 bool activated;
00212
00218 std::vector<double> positionConvertFactor;
00219
00220
00226 std::vector<double> driveConvertFactor;
00227
00233 std::vector<double> tachoConvertScale;
00234 std::vector<double> tachoConvertOffset;
00235
00241 std::vector<double> curReg_a;
00242 std::vector<double> curReg_b;
00243 std::vector<double> shunt_R;
00244 std::vector<double> shunt_c;
00245
00251 std::vector<double> Km;
00252
00256 RTT::Property<std::vector<std::string> > EmergencyEvents_prop;
00257
00258
00259 public:
00260 virtual bool configureHook();
00261
00266 virtual bool startHook();
00267
00271 virtual void updateHook();
00272
00276 virtual void stopHook();
00277
00278 virtual void cleanupHook();
00279
00280 private:
00281
00282 void EmergencyStop(std::string message)
00283 {
00284 log(Error) << "---------------------------------------------" << endlog();
00285 log(Error) << "--------- EMERGENCY STOP --------------------" << endlog();
00286 log(Error) << "---------------------------------------------" << endlog();
00287 log(Error) << message << endlog();
00288 this->fatal();
00289 };
00290 std::vector<RTT::Handle> EmergencyEventHandlers;
00291
00292
00293
00294
00295 #if (defined (OROPKG_OS_LXRT))
00296 std::vector<OCL::Axis*> axes_hardware;
00297
00298 ComediDevice* comediDev;
00299 ComediSubDeviceAOut* comediSubdevAOut;
00300 EncoderSSI_apci1710_board* apci1710;
00301 RelayCardapci2200* apci2200;
00302 SwitchDigitalInapci1032* apci1032;
00303 ComediDevice* comediDev_NI6024;
00304 ComediSubDeviceAIn* comediSubdevAIn_NI6024;
00305 ComediSubDeviceDIn* comediSubdevDIn_NI6024;
00306 ComediSubDeviceDOut* comediSubdevDOut_NI6713;
00307
00308 std::vector<RTT::EncoderInterface*> encoderInterface;
00309 std::vector<OCL::AbsoluteEncoderSensor*> encoder;
00310 std::vector<RTT::AnalogOutput*> ref;
00311 std::vector<RTT::DigitalOutput*> enable;
00312 std::vector<OCL::AnalogDrive*> drive;
00313 std::vector<RTT::DigitalOutput*> brake;
00314 std::vector<RTT::AnalogInput*> tachoInput;
00315 std::vector<OCL::AnalogSensor*> tachometer;
00316 std::vector<RTT::AnalogInput*> currentInput;
00317 std::vector<OCL::AnalogSensor*> currentSensor;
00318 RTT::DigitalOutput* torqueModeSwitch;
00319 std::vector<RTT::DigitalInput*> torqueModeCheck;
00320
00321
00322 #endif
00323 std::vector<RTT::AxisInterface*> axes;
00324 std::vector<RTT::AxisInterface*> axes_simulation;
00325 OCL::Kuka361TorqueSimulator* torqueSimulator;
00326 std::vector<double> tau_sim;
00327 std::vector<double> pos_sim;
00328 std::vector<TimeService::ticks> previous_time;
00329 TimeService::Seconds delta_time;
00330
00331 };
00332 }
00333 #endif // Kuka361nAxesTorqueController
