Index: unk/MagicSoft/Cosy/devdrv/macs.cc =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/macs.cc (revision 9436) +++ (revision ) @@ -1,1052 +1,0 @@ -#include "macs.h" - -#include // timeval->tv_sec - -#include "network.h" - -#include "MLogManip.h" - -#include "MString.h" - -ClassImp(Macs); - -using namespace std; - -//#define EXPERT - -/* - --------------------------- - For test purposes - --------------------------- - -class MyTimer : public TTimer -{ -public: - MyTimer(TObject *obj, Long_t ms, Bool_t mode) : TTimer(obj, ms, mode) {} - Bool_t Notify() - { - cout << "Notify" << endl; - TTimer::Notify(); - return kTRUE; - } -}; -*/ - -Macs::Macs(const BYTE_t nodeid, const char *name) - : NodeDrv(nodeid, name), fMacId(2*nodeid+1), - fPos(0), fPdoPos(0), fPosActive(0), fRpmActive(0), - fStatusPdo3(0xff), fArmed(false) -{ -// fTimeout = new TTimer(this, 100); //, kFALSE); // 100ms, asynchronous -} - -Macs::~Macs() -{ - //fTimerOn = kFALSE; - // delete fTimeout; -} - -TString Macs::EvalStatusDKC(UInt_t stat) const -{ - switch (stat) - { - case 0: return "offline"; - case 0xa000: - case 0xa001: - case 0xa002: - case 0xa003: return MString::Format("Communication phase %d", stat&0xf); - case 0xa010: return "Drive HALT"; - case 0xa012: return "Control and power section ready for operation"; - case 0xa013: return "Ready for power on"; - case 0xa100: return "Drive in Torque mode"; - case 0xa101: return "Drive in Velocity mode"; - case 0xa102: return "Position control mode with encoder 1"; - case 0xa103: return "Position control mode with encoder 2"; - case 0xa104: return "Lagless position control mode with encoder 1"; - case 0xa105: return "Lagless position control mode with encoder 2"; - case 0xa106: return "Drive controlled interpolated positioning with encoder 1"; - case 0xa107: return "Drive controlled interpolated positioning with encoder 2"; - case 0xa108: return "Drive controlled interpolated absolute positioning lagless with encoder 1"; - case 0xa109: return "Drive controlled interpolated absolute positioning lagless with encoder 2"; - case 0xa146: return "Drive controlled interpolated relative positioning with encoder 1"; - case 0xa147: return "Drive controlled interpolated relative positioning with encoder 2"; - case 0xa148: return "Drive controlled interpolated relative positioning lagless with encoder 1"; - case 0xa149: return "Drive controlled interpolated relative positioning lagless with encoder 2"; - case 0xa150: return "Drive controlled positioning with encoder 1"; - case 0xa151: return "Drive controlled positioning with encoder 1, lagless"; - case 0xa208: return "Jog mode positive"; - case 0xa218: return "Jog mode negative"; - case 0xa400: return "Automatic drive check and adjustment"; - case 0xa401: return "Drive decelerating to standstill"; - case 0xa800: return "Unknown operation mode"; - case 0xc217: return "Motor encoder reading error"; - case 0xc218: return "Shaft encoder reading error"; - case 0xc220: return "Motor encoder initialization error"; - case 0xc221: return "Shaft encoder initialization error"; - case 0xc400: return "Switching to parameter mode"; - case 0xc500: return "Error reset"; - case 0xe225: return "Motor overload"; - case 0xe249: return "Positioning command velocity exceeds limit bipolar"; - case 0xe250: return "Drive overtemp warning"; - case 0xe251: return "Motor overtemp warning"; - case 0xe252: return "Bleeder overtemp warning"; - case 0xe257: return "Continous current limit active"; - case 0xe264: return "Target position out of numerical range"; - case 0xe834: return "Emergency-Stop"; - case 0xe842: return "Both end-switches activated"; - case 0xe843: return "Positive end-switch activated"; - case 0xe844: return "Negative end-switch activated"; - case 0xf218: return "Amplifier overtemp shutdown"; - case 0xf219: return "Motor overtemp shutdown"; - case 0xf220: return "Bleeder overload shutdown"; - case 0xf221: return "Motor temperature surveillance defective"; - case 0xf224: return "Maximum breaking time exceeded"; - case 0xf228: return "Excessive control deviation"; - case 0xf250: return "Overflow of target position preset memory"; - case 0xf269: return "Error during release of the motor holding brake"; - case 0xf276: return "Absolute encoder out of allowed window"; - case 0xf409: return "Bus error on Profibus interface"; - case 0xf434: return "Emergency-Stop"; - case 0xf629: return "Positive sw end-switch"; - case 0xf630: return "Negative sw end-switch"; - case 0xf634: return "Emergency-Stop"; - case 0xf643: return "Positive hardware end-switch activated"; - case 0xf644: return "Negative hardware end-switch activated"; - case 0xf870: return "24V DC error"; - case 0xf878: return "Velocity loop error"; - } - return "unknown"; -} - -Bool_t Macs::EvalStatus(LWORD_t val) const -{ - const Int_t errnum = val&0xffff; - const Int_t errinf = val>>16; - - if (errnum!=0xff) - return errnum==0; - - const Int_t type = errinf&0xf000; - - gLog << all << MTime(-1) << ": " << GetNodeName() << " DKC "; - - switch (type) - { - case 0xf000: gLog << "ERROR"; break; - case 0xe000: gLog << "WARNING"; break; - case 0xa000: gLog << "Status"; break; - case 0xc000: - case 0xd000: gLog << "Message"; break; - default: gLog << "Unknown"; break; - } - - gLog << " (" << MString::Format("%X", errinf) << "): " << EvalStatusDKC(errinf); - - gLog << (type==0xf000 || type==0xe000 ? "!" : ".") << endl; - - return type!=0xf000; -} - -void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv) -{ - // cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx; - // cout << ", val=0x" << val << endl; - switch (idx) - { - case 0x1000: - if (subidx==1) - { - gLog << inf2 << "- " << GetNodeName() << ": Node is" << (val?" ":" not ") << "armed." << endl; - fArmed = val==1; - } - return; - - case 0x1003: - // FIXME, see Init - if (subidx!=2) - return; - gLog << inf2 << "- " << GetNodeName() << ": Error[0]=" << hex << val << dec << endl; - CheckErrorDKC(val); - return; - - case 0x100a: - gLog << inf2 << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl; - fSoftVersion = val; - return; - - case 0x100b: - // Do not display, this is used for CheckConnection - // lout << "Node ID: " << dec << val << endl; - return; - - case 0x100c: - gLog << inf2 << "- " << GetNodeName() << ": Guard time:" << dec << val << endl; - return; - - case 0x100d: - gLog << inf2 << "- " << GetNodeName() << ": Life time factor:" << dec << val << endl; - return; - - case 0x2002: - gLog << inf2 << GetNodeName() << ": Current velocity: " << dec << val << endl; - fVel = val; - return; - - case 0x6004: - if (subidx) - return; - -// lout << "Actual Position: " << dec << (signed long)val << endl; - fPos = (LWORDS_t)val; - fPosTime.Set(tv); - return; -/* - case 0x2001: - cout << "Axe Status: 0x" << hex << val << endl; - cout << " - Motor " << (val&0x01?"standing":"moving") << endl; - cout << " - Positioning " << (val&0x02?"active":"inactive") << endl; - cout << " - Rotary mode " << (val&0x04?"active":"inactive") << endl; - cout << " - Attitude control: " << (val&0x40?"off":"on") << endl; - cout << " - Axe resetted: " << (val&0x80?"yes":"no") << endl; - fPosActive = val&0x02; - fRpmActive = val&0x04; - return; - case 0x2003: - if (!subidx) - { - cout << "Input State: "; - for (int i=0; i<8; i++) - cout << (int)(val&(1<>8)&0xff) << endl; - cout << " - Attitude control: " << (val&0x04?"off":"on") << endl; - cout << " - Startswitch active: " << (val&0x20?"yes":"no") << endl; - cout << " - Referenceswitch active: " << (val&0x40?"yes":"no") << endl; - cout << " - Endswitch active: " << (val&0x80?"yes":"no") << endl; - return; -*/ - - case 0x6002: - gLog << inf2 << "- " << GetNodeName() << ": Velocity resolution = " << dec << val << " (100%)" << endl; - fVelRes = val; - return; - - case 0x6501: - gLog << inf2 << "- " << GetNodeName() << ": Encoder resolution = " << dec << val << " ticks" << endl; - fRes = val; - return; - } - - NodeDrv::HandleSDO(idx, subidx, val, tv); - -// cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx; -// cout << ", val=0x"< ZOMBIE!" << endl; - SetZombie(); - } - if (IsZombieNode()) - return; - - StopHostGuarding(); - StopGuarding(); - - gLog << inf2 << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl; - RequestSDO(0x100a); - WaitForSdo(0x100a); - // FIXME! Not statically linked! - //if (fSoftVersion<0x00000044) // 00.68 - if (fSoftVersion<0x00000046) // 00.69 - { - gLog << err << GetNodeName() << " - Software Version " << 0.01*fSoftVersion << " too old!" << endl; - SetZombie(); - return; - } - - SetRpmMode(FALSE); - - ReqRes(); // Init fRes - ReqVelRes(); // Init fVelRes - - /* Should not be necessary anymore. This is done by the MACS itself. - lout << "- " << GetNodeName() << ": Motor on." << endl; - SendSDO(0x3000, string('o', 'n')); - WaitForSdo(0x3000); - */ - - SetPDO1On(FALSE); // this is a workaround for the Macs - SetPDO1On(TRUE); - - //This is now standard in the MACS - //SetNoWait(TRUE); - - //StartGuarding(400, 1, kFALSE); // Using PDO1 @ 100ms - //StartGuarding(250, 4); - //StartHostGuarding(); - -#ifdef EXPERT - StartNode(); -#endif - - gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl; - RequestSDO(0x1000, 1); - WaitForSdo(0x1000, 1); -} -/* -void Macs::StopMotor() -{ - // - // Stop the motor and switch off the position control unit - // - SendSDO(0x3000, string('s','t','o','p')); - WaitForSdo(0x3000); -} -*/ -void Macs::StopDevice() -{ - //EnableTimeout(kFALSE); - - //No need to switch it off. - //SetNoWait(FALSE); - - StopHostGuarding(); - StopGuarding(); - - // - // FIXME: This isn't called if the initialization isn't done completely! - // - - SetRpmMode(FALSE); - - SetPDO1On(FALSE); - - /* - lout << "- " << GetNodeName() << ": Motor off." << endl; - SendSDO(0x3000, string('o', 'f', 'f')); - WaitForSdo(0x3000); - */ - - /* - lout << "- Stopping Program of " << (int)GetId() << endl; - SendSDO(0x4000, (LWORD_t)0xaffe); - WaitForSdo(); - */ -} - -void Macs::ReqPos() -{ - gLog << inf2 << "- " << GetNodeName() << ": Requesting Position." << endl; - RequestSDO(0x6004); - WaitForSdo(0x6004); -} - -void Macs::ReqVel() -{ - gLog << inf2 << "- " << GetNodeName() << ": Requesting Velocity." << endl; - RequestSDO(0x2002); - WaitForSdo(0x2002); -} -/* -void Macs::SetHome(LWORDS_t pos, WORD_t maxtime) -{ - StopHostGuarding(); - StopGuarding(); - - lout << "- " << GetNodeName() << ": Driving to home position, Offset=" << dec << pos << endl; - SendSDO(0x6003, 2, (LWORD_t)pos); // home - WaitForSdo(0x6003, 2); - - // home also defines the zero point of the system - // maximum time allowd for home drive: 25.000ms - SendSDO(0x3001, string('h','o','m','e')); // home - WaitForSdo(0x3001, 0, maxtime*1000); - lout << "- " << GetNodeName() << ": Home position reached. " << endl; - - SendSDO(0x6003, 0, string('s','e','t')); // home - WaitForSdo(0x6003, 0); - - StartGuarding(); - StartHostGuarding(); -} -*/ -void Macs::SetVelocity(LWORD_t vel) -{ - gLog << dbg << "- Setting velocity to: " << vel << endl; - SendSDO(0x2002, vel); // velocity - WaitForSdo(0x2002, 0); -} - -void Macs::SetAcceleration(LWORD_t acc) -{ - gLog << dbg << "- Setting acceleration to: " << acc << endl; - SendSDO(0x2003, 0, acc); // acceleration - WaitForSdo(0x2003, 0); -} - -void Macs::SetDeceleration(LWORD_t dec) -{ - gLog << dbg << "- Setting deceleration to: " << dec << endl; - SendSDO(0x2003, 1, dec); - WaitForSdo(0x2003, 1); -} - -void Macs::SetRpmMode(BYTE_t mode) -{ - // - // SetRpmMode(FALSE) stop the motor, but lets the position control unit on - // - SendSDO(0x3006, 0, mode ? string('s','t','r','t') : string('s','t','o','p')); - WaitForSdo(0x3006, 0); -} - -void Macs::SetRpmVelocity(LWORDS_t cvel) -{ - SendSDO(0x3006, 1, (LWORD_t)cvel); - WaitForSdo(0x3006, 1); -} - -void Macs::StartRelPos(LWORDS_t pos) -{ - if (!fArmed) - { - gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl; - SetZombie(); - return; - } - - gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl; - SendSDO(0x6004, 1, (LWORD_t)pos); - fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO -} - -void Macs::StartAbsPos(LWORDS_t pos) -{ - if (!fArmed) - { - gLog << err << GetNodeName() << ": ERROR - Moving without being armed is not allowed." << endl; - SetZombie(); - return; - } - - gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (LWORDS_t)pos << " ticks." << endl; - SendSDO(0x6004, 0, (LWORD_t)pos); - fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO -} - -/* -void Macs::SetNoWait(BYTE_t flag) -{ - lout << "- " << GetNodeName() << ": Setting NOWAIT " << (flag?"ON":"OFF") << "." << endl; - SendSDO(0x3008, flag ? string('o', 'n') : string('o', 'f', 'f')); - WaitForSdo(0x3008); -} -*/ - -void Macs::StartVelSync() -{ - // - // The syncronization mode is disabled by a 'MOTOR STOP' - // or by a positioning command (POSA, ...) - // - gLog << inf2 << "- " << GetNodeName() << ": Starting RPM Sync Mode." << endl; - SendSDO(0x3007, 0, string('s', 'y', 'n', 'c')); - WaitForSdo(0x3007, 0); -} - -void Macs::StartPosSync() -{ - // - // The syncronization mode is disabled by a 'MOTOR STOP' - // or by a positioning command (POSA, ...) - // - gLog << inf2 << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl; - SendSDO(0x3007, 1, string('s', 'y', 'n', 'c')); - WaitForSdo(0x3007, 1); -} -/* -void Macs::ReqAxEnd() -{ - RequestSDO(0x2001); - WaitForSdo(0x2001); -} -*/ -void Macs::SendMsg(BYTE_t data[6]) -{ - GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]); -} - -void Macs::SendMsg(BYTE_t d0, BYTE_t d1, BYTE_t d2, - BYTE_t d3, BYTE_t d4, BYTE_t d5) -{ - GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5); -} - -void Macs::HandlePDO1(const BYTE_t *data, const timeval_t &tv) -{ - // FIXME!!!! Only 0x4000 should do this to be - // CanOpen conform - HandleNodeguard(tv); - - fPdoPos = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7]; - - // data[3]&0x01; // motor not moving - fPosActive = data[3]&kPosActive; // positioning active - fRpmActive = data[3]&kRpmActive; // RPM mode switched on - // data[3]&0x08; // - unused - - // data[3]&0x10; // - unused - - // data[3]&0x20; // - unused - - fInControl = data[3]&0x40; // motor uncontrolled - // data[3]&0x80; // axis resetted (after errclr, motor stop, motor on) - - fArmed = data[2]&kIsArmed==kIsArmed; - - fStatus = data[3]; - - fPdoTime.Set(tv); -} - -void Macs::CheckErrorDKC(LWORD_t val) -{ - Bool_t rc = EvalStatus(val); - SetError(rc ? 0 : val); - if (!rc) - SetZombie(); -} - -void Macs::HandlePDO2(const BYTE_t *data, const timeval_t &tv) -{ - LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3]; - LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7]; - - // Check if the DKC changed its status message - if (errnum==0xff && (errinf&0xf000)<=0xe000) - { - CheckErrorDKC(errnum, errinf); - return; - } - - // Check if MACS report error occursion. - // errnum==0 gives a sudden information that something happened. Now the - // microcontroller is running inside its interrupt procedure which - // stopped the normal program. The interrupt procedure should try to clear - // the error state of the hardware. This should never create a new error! - // - if (!errnum) - { - gLog << err << "- " << GetNodeName() << ": reports Error occursion." << endl; - gLog << "Macs::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl; - SetZombie(); - SetError(-1); - return; - } - - // - // Now the error is handled by the hardware now it is the software part - // to react on it. The Error flag now is set to the correct value. - // - if (GetError()>0) - { - gLog << warn << GetNodeName() << ": WARNING! Previous error #" << GetError() << " unhandled (not cleared) by software." << endl; - - // - // If the error is unhadled and/or not cleared, don't try it again. - // - if (GetError()==errnum) - return; - } - - SetError(errnum); - - gLog << err << GetNodeName() << " reports: "; - switch (errnum) - { - case 3: - gLog << "Axis does not existing." << endl; - return; - case 5: - gLog << "Error not cleared (while trying to move axis)" << endl; - return; - case 6: - // - // Report the error to the user. All possible movements should have - // been stopped anyhow. Now delete the error to prevent the system - // from reporting this error a thousands of times. - // - gLog << "Home position not the first positioning command." << endl; - SetError(0); - return; - case 8: - gLog << "Control deviation overflow." << endl; - return; - case 9: - gLog << "Zero index not found." << endl; - return; - case 10: - gLog << "Unknown command, syntax error." << endl; - gLog << "Please recompile and reload program." << endl; - return; - case 11: - case 25: - switch (errinf) - { - case -1: - gLog << "Negative"; - break; - case 1: - gLog << "Positive"; - break; - default: - gLog << "-unknown-"; - } - switch (errnum) - { - case 11: - gLog << " software"; - break; - case 25: - gLog << " hardware"; - break; - } - gLog << " endswitch activated." << endl; - return; - case 12: - gLog << "Wrong parameter number used in SET command." << endl; - return; - case 14: - gLog << " Too many LOOP calls." << endl; - return; - case 16: - gLog << "Parameter in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl; - gLog << "Please use APOSS to 'Reset' the MACS and reload the parameters." << endl; - return; - case 17: - gLog << "Program in EEPROM broken (means: EEPROM broken, or saving not finished)" << endl; - gLog << "Please use APOSS to delete all Programs restore the programs." << endl; - return; - case 18: - gLog << "Reset by CPU (reset called by Watch-dog cause of CPU halted)" << endl; - gLog << "Possible reasons: short under-/overvoltage or shortcut." << endl; - return; - case 19: - gLog << "User break (autostart program stopped by user)" << endl; - return; - case 51: - gLog << "Too many (>=10) GOSUB calls." << endl; - return; - case 52: - gLog << "Too many RETURN calls." << endl; - return; - case 62: - gLog << "Error veryfiing EEPROM after access (Try again savaing parameters or program)" << endl; - return; - case 70: - gLog << "Error in DIM call (call to DIM doesn't fit existing DIM call)" << endl; - return; - case 71: - gLog << "Array out of bound." << endl; - return; - case 79: - gLog << "Timeout waiting for index (WAITNDX)." << endl; - return; - case 84: - gLog << "Too many (>12) ON TIME calls." << endl; - return; - case 87: - gLog << "Out of memory for variables - Check APOSS predifined number of" << endl; - gLog << "variables and try deleting the array by doing a 'Reset' from APOSS." << endl; - return; - case 89: - gLog << "CAN I/O error (REOPEN=" << dec << errinf << " " << (errinf==0?"OK":"ERR") << ")" << endl; - return; - - case 100: - //lout << "Connection timed out." << endl; - //EnableTimeout(false); - return; - - case 0xff: - EvalStatus(errnum, errinf); - return; - - default: - gLog << "Error Nr. " << dec << errnum << ", " << errinf << endl; - } -} - -void Macs::HandlePDO3(const BYTE_t *data, const timeval_t &tv) -{ - // 3 5 7 9 - // 1100 1010 1110 1001 - if (fStatusPdo3 == data[3]) - return; - - MTime time; - time.Now(); - - gLog << inf << time << ": " << GetNodeName() << " - PDO3 = "; - const Bool_t ready = data[3]&0x01; - const Bool_t fuse = data[3]&0x02; - const Bool_t emcy = data[3]&0x04; - const Bool_t vltg = data[3]&0x08; - const Bool_t mode = data[3]&0x10; - const Bool_t rf = data[3]&0x20; - const Bool_t brake = data[3]&0x40; - if (ready) gLog << "DKC-Ready "; - if (fuse) gLog << "FuseOk "; - if (emcy) gLog << "EmcyOk "; - if (vltg) gLog << "OvervoltOk "; - if (mode) gLog << "SwitchToManualMode "; - if (rf) gLog << "RF "; - if (brake) gLog << "BrakeOpen "; - gLog << endl; - - fStatusPdo3 = data[3]; -} - -// FIXME? Handling of fIsZombie? -void Macs::HandleError() -{ - // - // If there is no error we must not handle anything - // - if (!HasError()) - return; - - // - // If the program got into the: HandleError state before the hardware - // has finished handeling the error we have to wait for the hardware - // handeling the error - // - // FIXME: Timeout??? - // -// while (GetError()<0) -// usleep(1); - - // - // After this software and hardware should be in a state so that - // we can go on working 'as usual' Eg. Initialize a Display Update - // - gLog << inf << GetNodeName() << " Handling Error #" << dec << GetError() << endl; - switch (GetError()) - { - case 6: // home - case 8: // control dev - case 9: // zero idx - case 84: // ON TIME - gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl; - return; - - case 11: // software endswitch - case 25: // hardware endswitch - gLog << "- " << GetNodeName() << ": Cannot handle error 'Endswitch!'" << endl; - return; - - case 100: // timeout (movement has been stopped, so we can go on) - DelError(); - return; - - case 0xff: - gLog << err << "DKC error! Go and check what is going on!" << endl; - //DelError(); - return; -/* - case 101: - //lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl; - DelError(); - return; - */ - default: - gLog << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl; - - } -} - -/* 0x2000 0 rw Maximum positioning error */ -/* 1 rw Negative Software Endswitch */ -/* 2 rw Positive Software Endswitch */ -void Macs::SetNegEndswitch(LWORDS_t val) -{ - SendSDO(0x2000, 1, (LWORD_t)val); - WaitForSdo(0x2000, 1); -} - -void Macs::SetPosEndswitch(LWORDS_t val) -{ - SendSDO(0x2000, 2, (LWORD_t)val); - WaitForSdo(0x2000, 2); -} - -void Macs::EnableEndswitches(bool neg, bool pos) -{ - SendSDO(0x2000, 3, (LWORD_t)(neg|(pos<<1))); - WaitForSdo(0x2000, 3); -} - -void Macs::SendNodeguard() -{ - SendSDO(0x4000, 0, (LWORD_t)0, false); -} - -// -------------------------------------------------------------------------- -// -// This starts the host guarding. The host guarding is only available -// if the node guarding is running. The host guarding works with the -// guardtime and the lifetimefactor from the nodeguarding. -// -void Macs::StartHostGuarding() -{ - SendSDO(0x100c, 0, (LWORD_t)GetGuardTime()); - WaitForSdo(0x100c); - - SendSDO(0x100d, 0, (LWORD_t)GetLifeTimeFactor()); - WaitForSdo(0x100d); - - gLog << inf2 << "- " << GetNodeName() << ": Hostguarding started (" << dec; - gLog << GetLifeTimeFactor() << "*" << GetGuardTime() << "ms)" << endl; -} - -// -------------------------------------------------------------------------- -// -// Stop the host guarding. -// -void Macs::StopHostGuarding() -{ - SendSDO(0x100c, 0, (LWORD_t)0); - WaitForSdo(0x100c); - - SendSDO(0x100d, 0, (LWORD_t)0); - WaitForSdo(0x100d); - - gLog << inf2 << "- " << GetNodeName() << ": Hostguarding stopped." << endl; -} Index: unk/MagicSoft/Cosy/devdrv/macs.h =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/macs.h (revision 9436) +++ (revision ) @@ -1,152 +1,0 @@ -#ifndef COSY_Macs -#define COSY_Macs - -#ifndef COSY_NodeDrv -#include "nodedrv.h" -#endif - -#ifndef MARS_MTime -#include "MTime.h" -#endif - -class Macs : public NodeDrv -{ -private: - BYTE_t fMacId; - - LWORD_t fSoftVersion; - - LWORD_t fVelRes; - LWORDS_t fVel; - - LWORDS_t fPos; - MTime fPosTime; - - LWORD_t fRes; // Encoder resolution - - LWORDS_t fPdoPos; - MTime fPdoTime; - - BYTE_t fPosActive; - BYTE_t fRpmActive; - BYTE_t fInControl; - BYTE_t fStatus; - - BYTE_t fStatusPdo3; - - bool fArmed; - - void Init(); - void StopDevice(); - - TString EvalStatusDKC(UInt_t stat) const; - Bool_t EvalStatus(LWORD_t val) const; - Bool_t EvalStatus(UInt_t errnum, UInt_t errinf) const - { - return EvalStatus(errnum|(errinf<<16)); - } - void CheckErrorDKC(LWORD_t val); - void CheckErrorDKC(UInt_t errnum, UInt_t errinf) - { - CheckErrorDKC(errnum|(errinf<<16)); - } - - - void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv); - void HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv); - //void HandleSDOError(LWORD_t data) { NodeDrv::HandleSDOError(data); } - - void HandlePDO1(const BYTE_t *data, const timeval_t &tv); - void HandlePDO2(const BYTE_t *data, const timeval_t &tv); - void HandlePDO3(const BYTE_t *data, const timeval_t &tv); - //void HandleNodeguard(timeval_t *tv); - - void SendNodeguard(); - - void CheckConnection(); - -public: - enum - { - kNoSync = BIT(0), - kPosSync = BIT(1), - kVelSync = BIT(2) - }; - enum - { - kIsArmed = BIT(0), // Macs will accept movement commands - kNotMoving = BIT(0), // motor not moving - kPosActive = BIT(1), // positioning active - kRpmActive = BIT(2), // RPM mode switched on - // BIT(3-5) unsused - kOutOfControl = BIT(6), // motor uncontrolled - kAxisReset = BIT(7) // axis resetted (after errclr, motor stop, motor on) - }; - - Macs(const BYTE_t nodeid, const char *name=NULL); - virtual ~Macs(); - - static LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0) - { - return (LWORD_t)(b0<<24 | b1<<16 | b2<<8 | b3); - } - - void SendMsg(BYTE_t data[6]); - void SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0, - BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0); - - void ReqPos(); - void ReqVel(); - //void ReqAxEnd(); - void ReqVelRes(); - void ReqRes(); - //void SetHome(LWORDS_t pos=0, WORD_t maxtime=25); - void SetAcceleration(LWORD_t acc); - void SetDeceleration(LWORD_t dec); - void SetVelocity(LWORD_t vel); - //void SetNoWait(BYTE_t flag=TRUE); - void SetRpmMode(BYTE_t mode=TRUE); - void SetRpmVelocity(LWORDS_t cvel); - void SetPDO1On(BYTE_t flag=TRUE); - void SetPosEndswitch(LWORDS_t val); - void SetNegEndswitch(LWORDS_t val); - - void EnableEndswitches(bool neg=true, bool pos=true); - - void StartVelSync(); - void StartPosSync(); - - void StartRelPos(LWORDS_t pos); - void StartAbsPos(LWORDS_t pos); - - //void StopMotor(); - - void StartNode(); - - bool IsArmed() const { return fArmed; } - void Arm(); - void Disarm(); - - int IsPositioning() const { return fPosActive; } - BYTE_t GetStatus() const { return fStatus; } - - LWORDS_t GetPdoPos() const { return fPdoPos; } - - LWORDS_t GetPos() const { return fPos; } - LWORDS_t GetVel() const { return fVel; } - LWORD_t GetVelRes() const { return fVelRes; } // Velocity units (would be 100 for %) - LWORD_t GetRes() const { return fRes; } // Encoder resolution - - void StartHostGuarding(); - void StopHostGuarding(); - - void HandleError(); - - Double_t GetPosTime() const { return fPosTime; } - Double_t GetMjd() const { return fPosTime.GetMjd(); } - Double_t GetPdoMjd() const { return fPdoTime.GetMjd(); } - - ClassDef(Macs, 0) -}; - -#endif Index: unk/MagicSoft/Cosy/devdrv/shaftencoder.cc =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (revision 9436) +++ (revision ) @@ -1,337 +1,0 @@ -#include "shaftencoder.h" - -#include // TGLabel->SetText - -#include "MLog.h" -#include "MLogManip.h" - -#include "macs.h" -#include "network.h" - -ClassImp(ShaftEncoder); - -using namespace std; - -ShaftEncoder::ShaftEncoder(const BYTE_t nodeid, const char *name) - : NodeDrv(nodeid, name), - fPos(0), fVel(0), fAcc(0), fPosRes(0), fTurns(0), - fLabel(NULL), fUpdPos(0), - fPosHasChanged(false), fDirHasChanged(false), - fReport(NULL) -{ -} - -void ShaftEncoder::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv) -{ - switch (idx) - { - case 0x1000: - gLog << inf2 << "- Model: "; - switch (val&0xffff) - { - case 0x0196: - gLog << inf2 << "Shaft Encoder Type: "; - switch ((val>>16)&0xff) - { - case 0x01: - gLog << "Singleturn" << endl; - return; - case 0x02: - gLog << "Multiturn" << endl; - return; - default: - gLog << err << "?" << endl; - SetZombie(); - return; - } - default: - gLog << err << "???" << endl; - SetZombie(); - return; - } - case 0x100b: - // Do not display, this is used for CheckConnection - // lout << "Node ID: " << dec << val << endl; - return; - - case 0x100c: - gLog << inf2 << "- Guardtime: " << dec << val << "ms" << endl; - return; - - case 0x100d: - gLog << inf2 << "- Lifetimefactor: " << dec << val << endl; - return; - - case 0x100e: - gLog << inf2 << "- CobId for guarding: 0x" << hex << val << endl; - return; - - case 0x6000: - case 0x6500: - gLog << inf2 << "- Counting: " << (val&1 ?"anti-clockwise":"clockwise") << " "; - gLog << "HwTest: " << (val&2 ?"on":"off") << " "; - gLog << "Scaling: " << (val&4 ?"on":"off") << " "; - gLog << "Modulo: " << (val&4096?"on":"off") << endl; - return; - - case 0x6001: - gLog << inf2 << "- Logical Ticks/Revolution: " << dec << val << endl; - return; - - case 0x6004: - gLog << inf2 << "- Reported position: " << dec << (Int_t)val << endl; - fPos = val; - return; - - - case 0x6501: - gLog << inf2 << "- Position resolution Ticks/Revolution: " << dec << val << endl; - fPosRes = val; - return; - - case 0x6502: - //if (val==0) - // val = 1; // Single Turn = Multiturn with one turn - gLog << inf2 << "- Number of Revolutions: " << dec << val << endl; - fTurns = val; - return; - - - } - - NodeDrv::HandleSDO(idx, subidx, val, tv); - -// gLog << err << hex << setfill('0'); -// gLog << "Sdo=" << idx << "/" << (int)subidx << ": 0x" << setw(8) << val; -// gLog << endl; -} - -void ShaftEncoder::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv) -{ - switch (idx) - { - case 0x1802: - switch (subidx) - { - case 1: - //lout << ddev(MLog::eGui); - gLog << inf2 << "- " << GetNodeName() << ": PDOs configured." << endl; - //lout << edev(MLog::eGui); - return; - } - break; - - case 0x6001: - switch (subidx) - { - case 0: - //lout << ddev(MLog::eGui); - gLog << inf2 << "- " << GetNodeName() << ": Log.ticks/revolution set." << endl; - //lout << edev(MLog::eGui); - return; - } - break; - - case 0x6002: - switch (subidx) - { - case 0: - //lout << ddev(MLog::eGui); - gLog << inf2 << "- " << GetNodeName() << ": Max number of ticks set." << endl; - //lout << edev(MLog::eGui); - return; - } - break; - - case 0x6003: - switch (subidx) - { - case 0: - //lout << ddev(MLog::eGui); - gLog << inf2 << "- " << GetNodeName() << ": Preset value set." << endl; - //lout << edev(MLog::eGui); - return; - } - break; - } - NodeDrv::HandleSDOOK(idx, subidx, data, tv); -} - -void ShaftEncoder::HandlePDOType0(const BYTE_t *data, const timeval_t &tv) -{ - // - // Decode information, we have a 14bit only - // - LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16); // | (data[3]<<24); - - //if (pos==fPos) - // return; - - fPos = pos; - fTime.Set(tv); - fPosHasChanged = true; - - //CheckTwin(fPos-pos); - //fIsUpdated=kTRUE; - - if (fReport) - { - fReport->Lock("ShaftEncoder::HandlePDOType0"); - *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO0 " << pos << " " << GetNodeName() << endl; - fReport->UnLock("ShaftEncoder::HandlePDOType0"); - } -} - -void ShaftEncoder::HandlePDOType1(const BYTE_t *data, const timeval_t &tv) -{ - // - // Decode information, we have a 14bit only - // - LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24); - BYTE_t flag = data[4]; - - //if (fPos==pos) - // return; - - //CheckTwin(fPos-pos); - fPos=pos; - fTime.Set(tv); - fPosHasChanged=true; - //fIsUpdated=kTRUE; - //fOffset = 0; - - flag=flag; - - if (fReport) - { - fReport->Lock("ShaftEncoder::HandlePDOType1"); - *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO1 " << pos << " " << (int)flag << " " << GetNodeName() << endl; - fReport->UnLock("ShaftEncoder::HandlePDOType1"); - } -} - -//#include -//ofstream fout("log/shaftencoder.log"); - -void ShaftEncoder::HandlePDOType2(const BYTE_t *data, const timeval_t &tv) -{ - // - // Decode information, we have a 14bit only - // - LWORDS_t pos = data[0] | (data[1]<<8) | (data[2]<<16) | (data[3]<<24); - - fVel = data[4] | (data[5]<<8); - fAcc = data[6] | (data[7]<<8); - -// LWORDS_t multipos = pos+turn*fTicks; // SE position of multiturn shaftencoder - - //if (fPos==pos && fTurn==fTurn) - // return; - - //CheckTwin(fPos-pos); - - fPos = pos; - //fTurn = turn; - - fTime.Set(tv); - fPosHasChanged=true; - //fIsUpdated=kTRUE; - //fOffset = 0; - - if (fReport) - { - fReport->Lock("ShaftEncoder::HandlePDOType2"); - *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO2 " << pos << " " << fVel << " " << fAcc << " " << GetNodeName() << endl; - //*fReport << "DIR-REPORT " << (int)GetId() << " " << (Int_t)fDirHasChanged << " " << (Int_t)fDirChangedPos << endl; - fReport->UnLock("ShaftEncoder::HandlePDOType2"); - } -} - -double ShaftEncoder::GetMjd() -{ - return fTime.GetMjd(); -} - -void ShaftEncoder::Init() -{ - //----------------------------------------------------------------------- - // Start Setup of the Shaft Encoder - //----------------------------------------------------------------------- - - StopGuarding(); - - // - // Requesting and checking (FIXME) type of encoder - // - gLog << inf2 << "- " << GetNodeName() << ": Requesting Hardware Type (0x1000)." << endl; - RequestSDO(0x1000); - WaitForSdo(0x1000); - if (IsZombieNode()) - return; - - // - // Read physical ticks per revolution - // - gLog << inf2 << "- " << GetNodeName() << ": Requesting physical ticks/revolution (SDO 0x6501)." << endl; - RequestSDO(0x6501); - WaitForSdo(0x6501); - - // - // Read number of possible ticks per revolution - // - gLog << inf2 << "- " << GetNodeName() << ": Requesting possible ticks/revolution (SDO 0x6502)." << endl; - RequestSDO(0x6502); - WaitForSdo(0x6502); - - // - // Request Lifetimefactor for unknown reason to make guarding - // working in SE/Az... (FIXME) - // - // lout << "- " << GetNodeName() << ": Requesting Lifetimefactor (Workaround, FIXME!) (SDO 0x100d)." << endl; - // RequestSDO(0x100c); - // WaitForSdo(0x100c); - // RequestSDO(0x100d); - // WaitForSdo(0x100d); - - // - // Configure PDOs - // - gLog << inf2 << "- " << GetNodeName() << ": Configuring PDOs (0x1802)." << endl; - SendSDO(0x1802, 1, (LWORD_t)0x281); - WaitForSdo(0x1802, 1); - - // - // Request Parameter - // - gLog << inf2 << "- " << GetNodeName() << ": Requesting SDO 0x6000." << endl; - RequestSDO(0x6000); - WaitForSdo(0x6000); - - ReqPos(); - - gLog << inf2 << "- " << GetNodeName() << ": Start Node (NMT)." << endl; - SendNMT(kNMT_START); -} - -void ShaftEncoder::CheckConnection() -{ - // Request Node number - RequestSDO(0x100b); - WaitForSdo(0x100b); -} - -void ShaftEncoder::ReqPos() -{ - // - // Request Position - // - gLog << inf2 << "- " << GetNodeName() << ": Requesting Position." << endl; - RequestSDO(0x6004); - WaitForSdo(0x6004); -} - -void ShaftEncoder::StopDevice() -{ - gLog << inf2 << "- " << GetNodeName() << ": Stop Node (NMT)." << endl; - SendNMT(kNMT_STOP); -} Index: unk/MagicSoft/Cosy/devdrv/shaftencoder.h =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/shaftencoder.h (revision 9436) +++ (revision ) @@ -1,61 +1,0 @@ -#ifndef COSY_ShafTEncoder -#define COSY_ShafTEncoder - -#ifndef COSY_NodeDrv -#include "nodedrv.h" -#endif - -#ifndef MARS_MTime -#include "MTime.h" -#endif - -class Macs; -class TGLabel; - -enum Direction_t { kUndefined, kForward, kBackward }; - -class ShaftEncoder : public NodeDrv -{ -private: - LWORDS_t fPos; // ticks - WORDS_t fVel; // ticks per 5ms - WORDS_t fAcc; // ticks per 25ms^2 - LWORD_t fPosRes; // Number of ticks per revolution (resolution) - WORD_t fTurns; // Number of possible turns - - TGLabel *fLabel; // - LWORDS_t fUpdPos; // ticks - - bool fPosHasChanged; //! - bool fDirHasChanged; //! - - MTime fTime; - MLog *fReport; - - void HandlePDOType0(const BYTE_t *data, const timeval_t &tv); - void HandlePDOType1(const BYTE_t *data, const timeval_t &tv); - void HandlePDOType2(const BYTE_t *data, const timeval_t &tv); - - void ReqPos(); - - void Init(); - void CheckConnection(); - // void CheckTwin(Int_t diff) const; - -public: - ShaftEncoder(const BYTE_t nodeid, const char *name=NULL); - - void StopDevice(); - - void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv); - void HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv); - - void HandlePDO1(const BYTE_t *data, const timeval_t &tv) { HandlePDOType2(data, tv); } - void HandlePDO2(const BYTE_t *data, const timeval_t &tv) { HandlePDOType2(data, tv); } - - double GetMjd(); - - ClassDef(ShaftEncoder, 0) -}; - -#endif