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
00028
00029
00030
00031 #ifndef DEVICE_DRIVERS_ANALOGDRIVE_HPP
00032 #define DEVICE_DRIVERS_ANALOGDRIVE_HPP
00033
00034 #include <ocl/OCL.hpp>
00035 #include "rtt/dev/AnalogOutInterface.hpp"
00036 #include "rtt/dev/DigitalOutInterface.hpp"
00037 #include "rtt/dev/DriveInterface.hpp"
00038 #include "rtt/dev/AnalogOutput.hpp"
00039 #include "rtt/dev/DigitalOutput.hpp"
00040 #include <rtt/Event.hpp>
00041
00042 #include <math.h>
00043
00044 extern "C" int isnan(double);
00045
00046 namespace OCL
00047 {
00056 class AnalogDrive
00057 : public DriveInterface
00058 {
00059 typedef double InputStruct;
00060
00061 public:
00081 AnalogDrive( AnalogOutput* an_out,
00082 DigitalOutput* dig_out, double _scale=1.0, double _offset=0.0 )
00083 : analogDevice( an_out ),
00084 enableDevice(dig_out), mySpeed(0.0),
00085 scale(_scale), offset( _offset ),
00086 lowvel( an_out->lowest()*scale),
00087 highvel( an_out->highest()*scale)
00088 {
00089 driveSet(0);
00090 disableDrive();
00091 }
00092
00093 ~AnalogDrive()
00094 {
00095 driveSet(0);
00096 disableDrive();
00097
00098 delete analogDevice;
00099 delete enableDevice;
00100 }
00101
00105 bool enableDrive()
00106 {
00107 enableDevice->switchOn();
00108 return enableDevice->isOn();
00109 }
00110
00114 bool disableDrive()
00115 {
00116 enableDevice->switchOff();
00117 return !enableDevice->isOn();
00118 }
00119
00120 DigitalOutput* getEnable()
00121 {
00122 return enableDevice;
00123 }
00124
00125 bool isEnabled() const { return enableDevice->isOn(); }
00126
00130 void addOffset(double addoffset)
00131 {
00132 offset += addoffset;
00133 }
00134
00135
00139 double getOffset()
00140 {
00141 return offset;
00142 }
00143
00144
00148 void limit(double lower, double higher, const Event<void(std::string)>& ev)
00149 {
00150 lowvel = lower;
00151 highvel = higher;
00152 event = ev;
00153 }
00154
00158 bool driveSet( double v )
00159 {
00160 if ( enableDevice->isOn() == false )
00161 return false;
00162
00163
00164 if ( v+offset > lowvel && v+offset < highvel && !isnan(v) && !isnan(offset) && !isnan(scale)){
00165 mySpeed = v;
00166 analogDevice->value( (mySpeed+offset)/scale );
00167 }
00168 else{
00169 log(Error)<<"Drive value: "<<v<<endlog();
00170 event("Axis drive value exceeds boundaries");
00171 }
00172 return true;
00173 }
00174
00178 double driveGet() const
00179 {
00180 return mySpeed;
00181 }
00182
00186 double maxDriveGet() const
00187 {
00188 return analogDevice->highest()*scale-offset;
00189 }
00190
00194 double minDriveGet() const
00195 {
00196 return analogDevice->lowest()*scale-offset;
00197 }
00198
00199 protected:
00200
00201 AnalogOutput* analogDevice;
00202 DigitalOutput* enableDevice;
00203
00204 double mySpeed;
00205 double scale, offset;
00206 double lowvel, highvel;
00207 Event<void(std::string)> event;
00208 };
00209
00210 }
00211
00212 #endif