00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #ifndef KMLMOTBASE_H
00014 #define KMLMOTBASE_H
00015
00016 #include "common/exception.h"
00017 #include "common/dllexport.h"
00018
00019 #include "KNI/kmlCommon.h"
00020 #include "KNI/cplBase.h"
00021
00022 #include <vector>
00023
00024 class CKatBase;
00025 class CMotBase;
00026
00027
00028
00029
00030
00031
00034 struct TMotDesc {
00035 byte slvID;
00036 };
00037
00040 struct TKatMOT {
00041 short cnt;
00042 CMotBase* arr;
00043 TMotDesc* desc;
00044 };
00045
00048 enum TMotCmdFlg {
00049 MCF_OFF = 0,
00050 MCF_CALIB = 4,
00051 MCF_FREEZE = 8,
00052 MCF_ON = 24
00053 };
00054
00057 enum TMotStsFlg {
00058 MSF_MECHSTOP = 1,
00059 MSF_MAXPOS = 2,
00060 MSF_MINPOS = 4,
00061 MSF_DESPOS = 8,
00062 MSF_NORMOPSTAT = 16,
00063 MSF_MOTCRASHED = 40,
00064 MSF_NLINMOV = 88,
00065 MSF_LINMOV = 152,
00066 MSF_NOTVALID = 128
00067 };
00068 enum TSearchDir {
00069 DIR_POSITIVE,
00070 DIR_NEGATIVE
00071 };
00072
00073
00074
00075
00078 struct TMotGNL {
00079 CKatBase* own;
00080 byte SID;
00081 };
00082
00085 struct TMotSFW {
00086 byte version;
00087 byte subversion;
00088 byte revision;
00089 byte type;
00090 byte subtype;
00091 };
00092
00095 struct TMotAPS {
00096 TMotCmdFlg mcfAPS;
00097 short actpos;
00098 };
00099
00102 struct TMotTPS {
00103 TMotCmdFlg mcfTPS;
00104 short tarpos;
00105 };
00106
00109 struct TMotSCP {
00110
00111
00112
00113 byte maxppwm;
00114 byte maxnpwm;
00115 byte kP;
00116 byte kI;
00117 byte kD;
00118 byte kARW;
00119
00120 byte kP_speed;
00121 byte kI_speed;
00122 byte kD_speed;
00123
00124
00125
00126 byte maxppwm_nmp;
00127 byte maxnpwm_nmp;
00128 byte kspeed_nmp;
00129 byte kpos_nmp;
00130 byte kI_nmp;
00131 int crash_limit_nmp;
00132 int crash_limit_lin_nmp;
00133 };
00134
00137 struct TMotDYL {
00138
00139
00140
00141 byte maxaccel;
00142 byte maxdecel;
00143 short minpos;
00144 short maxpspeed;
00145 short maxnspeed;
00146
00147
00148 byte maxcurr;
00149 byte actcurr;
00150
00151
00152
00153 byte maxaccel_nmp;
00154 short maxpspeed_nmp;
00155 short maxnspeed_nmp;
00156 byte maxcurr_nmp;
00157 };
00158
00161 struct TMotPVP {
00162 TMotStsFlg msf;
00163 short pos;
00164 short vel;
00165 byte pwm;
00166 };
00167
00170 struct TMotENL {
00171 int enc_range;
00172 int enc_minpos;
00173 int enc_maxpos;
00174 int enc_per_cycle;
00175 int enc_tolerance;
00176 };
00177
00178
00181 struct TMotCLB {
00182 bool enable;
00183 short order;
00184
00185 TSearchDir dir;
00186 TMotCmdFlg mcf;
00187
00188 int encoderPositionAfter;
00189 bool isCalibrated;
00190
00191 TMotDYL dyl;
00192 TMotSCP scp;
00193 };
00194
00195
00198 struct TMotInit {
00199 int encoderOffset;
00200 int encodersPerCycle;
00201 double angleOffset;
00202 double angleRange;
00203 int rotationDirection;
00204
00205
00206 double angleStop;
00207 };
00208
00209
00210
00219 class DLLDIR CMotBase {
00220
00221 friend class CKatBase;
00222
00223
00224 protected:
00225 TMotGNL gnl;
00226 TMotAPS aps;
00227 TMotTPS tps;
00228 TMotSCP scp;
00229 TMotDYL dyl;
00230 TMotPVP pvp;
00231 TMotSFW sfw;
00232 TMotCLB _calibrationParameters;
00233 TMotENL _encoderLimits;
00234 TMotInit _initialParameters;
00235 bool freedom;
00236 bool nmp;
00237 bool blocked;
00238
00239
00240 public:
00241 const TMotGNL* GetGNL() { return &gnl; }
00242 const TMotAPS* GetAPS() { return &aps; }
00243 const TMotTPS* GetTPS() { return &tps; }
00244 const TMotSCP* GetSCP() { return &scp; }
00245 const TMotDYL* GetDYL() { return &dyl; }
00246 const TMotPVP* GetPVP() { return &pvp; }
00247 const TMotSFW* GetSFW() { return &sfw; }
00248 const TMotCLB* GetCLB() { return &_calibrationParameters; }
00249
00250 const TMotInit* GetInitialParameters() { return &_initialParameters; }
00251 const int GetEncoderTolerance() { return _encoderLimits.enc_tolerance; }
00252 const int GetEncoderMinPos() { return _encoderLimits.enc_minpos; }
00253 const int GetEncoderMaxPos() { return _encoderLimits.enc_maxpos; }
00254 const int GetEncoderRange() { return _encoderLimits.enc_range; }
00255 const bool GetFreedom() { return freedom; }const bool GetBlocked() { return blocked; }const bool GetNmp() { return nmp; }
00262
00263 protected:
00264 CCplBase* protocol;
00265
00266 public:
00267 virtual ~CMotBase() {}
00268
00269 bool init(CKatBase* _own, const TMotDesc _motDesc, CCplBase* protocol);
00270 void sendAPS(const TMotAPS* _aps); void sendTPS(const TMotTPS* _tps); void sendSCP(const TMotSCP* _scp); void sendDYL(const TMotDYL* _dyl);
00279 void recvPVP(); void recvSCP(); void recvDYL(); void recvSFW();
00288
00289 void setSCP(TMotSCP _scp) { scp = _scp; }
00290 void setDYL(TMotDYL _dyl) { dyl = _dyl; }
00291
00296 void setTPSP(int tar);
00297 void setTPSPDegrees(double tar); void resetTPSP();
00302
00303
00304 void setInitialParameters(double angleOffset, double angleRange, int encodersPerCycle, int encoderOffset, int rotationDirection);
00305 void setCalibrationParameters(bool doCalibration, short order, TSearchDir direction, TMotCmdFlg motorFlagAfter, int encoderPositionAfter);
00306 void setCalibrated(bool calibrated);
00307
00308 void setTolerance(int tolerance);
00309
00312 bool checkAngleInRange(double angle);
00313 bool checkEncoderInRange(int encoder);
00314
00317 void inc(int dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS);
00320 void dec(int dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS);
00323 void mov(int tar, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS);
00324
00327 void waitForMotor(int tar, int encTolerance = 100, short mode = 0, int waitTimeout = TM_ENDLESS);
00328
00331 void incDegrees(double dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS);
00334 void decDegrees(double dif, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS);
00337 void movDegrees(double tar, bool wait = false, int tolerance = 100, long timeout = TM_ENDLESS);
00338
00341 void resetBlocked();
00342
00343
00347 void sendSpline(short targetPosition, short duration, short p1, short p2, short p3, short p4);
00348
00353 void sendFourSplines(short targetPosition, short duration, std::vector<short>& coefficients);
00354
00355
00358 void setSpeedLimits(short positiveVelocity, short negativeVelocity);
00359 void setSpeedLimit(short velocity) { setSpeedLimits(velocity, velocity); }
00360
00363 void setAccelerationLimit( short acceleration );
00364
00367 void setPwmLimits(byte maxppwm, byte maxnpwm);
00368
00371 void setControllerParameters(byte kSpeed, byte kPos, byte kI);
00372
00375 void setCrashLimit(int limit);
00377 void setCrashLimitLinear(int limit_lin);
00379 void setSpeedCollisionLimit(int limit);
00381 void setPositionCollisionLimit(int limit);
00382
00389 void getParameterOrLimit(int subcommand, byte* R1, byte* R2, byte* R3);
00390 };
00391
00392
00393 #endif