#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; }