Ignore:
Timestamp:
01/23/03 13:32:58 (22 years ago)
Author:
tbretz
Message:
*** empty log message ***
Location:
trunk/MagicSoft/Cosy/devdrv
Files:
4 edited

Legend:

Unmodified
Added
Removed

  • trunk/MagicSoft/Cosy/devdrv/macs.cc

    r1703 r1727  
    3333    fPosActive(0), fRpmActive(0)
    3434{
    35     fTimeout = new TTimer(this, 100, kFALSE); // 100ms, asynchronous
     35//    fTimeout = new TTimer(this, 100); //, kFALSE); // 100ms, asynchronous
    3636}
    3737
    3838Macs::~Macs()
    3939{
    40     fTimerOn = kFALSE;
    41     delete fTimeout;
     40    //fTimerOn = kFALSE;
     41  //  delete fTimeout;
    4242}
    4343
    … …  
    6060        fVel = val;
    6161        return;
    62 
    63     case 0x4000:
    64         switch (subidx)
    65         {
    66         case 1:
    67             cout << GetNodeName() << ": Timeout timer is " << (val?"en":"dis") << "abled." << endl;
    68             return;
    69         case 2:
    70             cout << GetNodeName() << ": Actual timeout time: " << dec << val << "ms" << endl;
    71             return;
    72         }
    73         break;
    7462
    7563    case 0x6004:
    … …  
    129117}
    130118
    131 void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx)
     119void Macs::HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
    132120{
    133121    switch (idx)
    … …  
    137125        {
    138126        case 0:
    139             lout << ddev(MLog::eGui);
     127            //lout << ddev(MLog::eGui);
    140128            lout << "- " << GetNodeName() << ": Velocity set." << endl;
    141             lout << edev(MLog::eGui);
     129            //lout << edev(MLog::eGui);
    142130            return;
    143131        }
    144132        break;
     133
    145134    case 0x2003:
    146135        switch (subidx)
    147136        {
    148137        case 0:
    149             lout << ddev(MLog::eGui);
     138            //lout << ddev(MLog::eGui);
    150139            lout << "- " << GetNodeName() << ": Acceleration set." << endl;
    151             lout << edev(MLog::eGui);
     140            //lout << edev(MLog::eGui);
    152141            return;
    153142        case 1:
    154             lout << ddev(MLog::eGui);
     143            //lout << ddev(MLog::eGui);
    155144            lout << "- " << GetNodeName() << ": Decceleration set." << endl;
    156             lout << edev(MLog::eGui);
     145            //lout << edev(MLog::eGui);
    157146            return;
    158147        }
    159148        break;
     149
    160150    case 0x3006:
    161151        switch (subidx)
    162152        {
    163153        case 0:
    164             lout << ddev(MLog::eGui);
     154            //lout << ddev(MLog::eGui);
    165155            lout << "- " << GetNodeName() << ": RPM mode switched." << endl;
    166             lout << edev(MLog::eGui);
     156            //lout << edev(MLog::eGui);
    167157            return;
    168158
    … …  
    176166        }
    177167        break;
     168
    178169    case 0x4000:
    179         if (subidx==0 && fTimerOn)
    180         {
    181             ResetTimeout();
    182             return;
    183         }
    184         break;
     170        HandleNodeguard(tv);
     171        return;
     172
    185173    case 0x6004:
    186174        switch (subidx)
    187175        {
    188176        case 0:
    189             lout << ddev(MLog::eGui);
     177            //lout << ddev(MLog::eGui);
    190178            lout << "- " << GetNodeName() << ": Absolute positioning started." << endl;
    191             lout << edev(MLog::eGui);
     179            //lout << edev(MLog::eGui);
    192180            return;
    193181
    194182        case 1:
    195             lout << ddev(MLog::eGui);
     183            //lout << ddev(MLog::eGui);
    196184            lout << "- " << GetNodeName() << ": Relative positioning started." << endl;
    197             lout << edev(MLog::eGui);
     185            //lout << edev(MLog::eGui);
    198186            return;
    199187        }
    … …  
    202190
    203191    }
    204     NodeDrv::HandleSDOOK(idx, subidx);
     192    NodeDrv::HandleSDOOK(idx, subidx, tv);
    205193}
    206194
    … …  
    231219    RequestSDO(0x100b);
    232220    WaitForSdo(0x100b);
    233 
    234     // FIXME! Not statically linked!
    235     //    if (fSoftVersion<0x00000035)
    236     //        fIsZombie = true;
    237221}
    238222
    … …  
    240224void Macs::Init()
    241225{
     226    if (HasError())
     227    {
     228        lout << "Macs::Init: " << GetNodeName() << " has error --> ZOMBIE!" << endl;
     229        SetZombie();
     230    }
     231
     232    StopHostGuarding();
     233    StopGuarding();
     234
     235    usleep(2000.*GetGuardTime());
     236
    242237    lout << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl;
    243238    RequestSDO(0x100a);
    244239    WaitForSdo(0x100a);
    245240
    246     if (fIsZombie)
     241    if (IsZombieNode())
    247242    {
    248243        lout << GetNodeName() << " - InitDevice failed!" << endl;
    … …  
    250245    }
    251246
    252     EnableTimeout(kFALSE);
     247    // FIXME! Not statically linked!
     248    if (fSoftVersion<0x00000042) // 00.66
     249    {
     250        lout << GetNodeName() << " - Software Version too old!" << endl;
     251        SetZombie();
     252        return;
     253    }
     254
    253255    SetRpmMode(FALSE);
    254256
    … …  
    270272
    271273    SetNoWait(TRUE);
     274
     275    StartGuarding(100, 2);
     276    StartHostGuarding();
    272277}
    273278
    … …  
    283288void Macs::StopDevice()
    284289{
    285     EnableTimeout(kFALSE);
     290    //EnableTimeout(kFALSE);
    286291
    287292    SetNoWait(FALSE);
    … …  
    348353{
    349354    SendSDO(0x2002, vel);     // velocity
    350     WaitForSdo(0x2002);
     355    WaitForSdo(0x2002, 0);
    351356}
    352357
    … …  
    466471    {
    467472        lout << "- " << GetNodeName() << ": reports Error occursion." << endl;
     473        lout << "Macs::HandlePDO2: " << GetNodeName() << " --> ZOMBIE!" << endl;
     474        SetZombie();
    468475        SetError(-1);
    469476        return;
    … …  
    537544
    538545    case 100:
    539         lout << "Connection timed out." << endl;
    540         EnableTimeout(false);
     546        //lout << "Connection timed out." << endl;
     547        //EnableTimeout(false);
    541548        return;
    542549
    … …  
    562569    // FIXME: Timeout???
    563570    //
    564     while (GetError()<0)
    565         usleep(1);
     571//  while (GetError()<0)
     572//      usleep(1);
    566573
    567574    //
    … …  
    569576    // we can go on working 'as usual' Eg. Initialize a Display Update
    570577    //
    571     cout << GetNodeName() << " Handling Error #" << GetError() << endl;
     578    cout << GetNodeName() << " Handling Error #" << dec << GetError() << endl;
    572579    switch (GetError())
    573580    {
    … …  
    576583    case   9: // zero idx
    577584    case  84: // ON TIME
    578         // Stop program?
     585        lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
    579586        return;
    580587
    581588    case 11:  // software endswitch
    582589    case 25:  // hardware endswitch
     590        lout << "- " << GetNodeName() << ": Cannot handle error 'Endswitch!'" << endl;
     591        return;
     592
    583593    case 100: // timeout (movement has been stopped, so we can go on)
    584594        DelError();
    585595        return;
    586 
     596/*
    587597    case 101:
    588         lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
     598        //lout << "Warning: " << GetNodeName() << " didn't respond in timeout window - try again." << endl;
    589599        DelError();
    590600        return;
     601        */
     602    default:
     603        lout << "- " << GetNodeName() << ": Cannot handle error #" << GetError() << endl;
     604 
    591605    }
    592606}
    … …  
    633647}
    634648
    635 void Macs::ReqTimeoutTime()
    636 {
    637     RequestSDO(0x4000, 2);
    638     WaitForSdo(0x4000, 2);
    639 }
    640 
    641 void Macs::EnableTimeout(bool enable, LWORDS_t ms)
    642 {
    643     lout << "- " << GetNodeName() << ": " << (enable?"En":"Dis") << "able timeout, " << dec << ms << "ms." << endl;
    644     if (!enable)
    645     {
    646         SendSDO(0x4000, 1, string('o', 'f', 'f'));
    647         WaitForSdo(0x4000, 1);
    648 
    649         lout << "- " << GetNodeName() << ": Stopping handshake (PC)." << endl;
    650 
    651         fTimeout->Stop(); //kTRUE);
    652 
    653         fTimerOn = kFALSE;
    654     }
    655     else
    656     {
    657         if (ms>0)
    658             SetTimeoutTime(ms);
    659 
    660         ResetTimeout();
    661 
    662         fTimerOn = kTRUE;
    663         fTimeout->Start(fGuardTime/*/3*2*/, kTRUE); //kFALSE); //TRUE);
    664 
    665         //
    666         // Start with kFALSE would be a continous timer, but this
    667         // timer seems to stop it's activity at some stage without
    668         // any reason
    669         //
    670         lout << "- " << GetNodeName() << ": starting handshake." << endl;
    671         SendSDO(0x4000, 1, string('o', 'n'));
    672         WaitForSdo(0x4000, 1);
    673     }
    674     lout << "- " << GetNodeName() << ": Timeout timer turned "
    675         << (enable?"on.":"off.") << endl;
    676 }
    677 
    678 Bool_t Macs::HandleTimer(TTimer *t)
    679 {
    680     /*
    681      Fons:
    682      -----
    683 
    684      timers never trigger at the same time or when in a TTimer::Notify.
    685      Little explanation:
    686 
    687      - there are two types of timers synchronous and a-synchronous.
    688      - synchronous timers are only handled via the ROOT eventloop
    689        (see TUnixSystem::DispatchOneEvent()). If there are no mouse/keyboard
    690        events then the synchronous timer queue is checked. So if the processing
    691        of a mouse/keyboard event takes a long time synchronous timers are not
    692        called for a while. To prevent this from happening one can call in long
    693        procedures gSystem->ProcessEvents(). The system schedules only the
    694        next timer in the queue when the current one's Notify() has finished.
    695      - a-synchronous timers are triggered via SIGALARM, i.e. the program is
    696        interupted and execution jumps to the Notify() function. When the
    697        notify is finished the next a-sync timer is scheduled and the system
    698        resumes from the place where it was initially interrupted. One of the
    699        things to remember when using a-sync timers is don't make any graphics
    700        calls in them. X11 is not re-entrant and it might be that the SIGALARM
    701        signal interrupted the system while being in X11. A-sync timers are best
    702        used to set flags that you can test at a convenient and controlled
    703        time.
    704        */
    705 
    706     //
    707     //  FIXME! Use NMT!
    708     //
    709     // --- lout << ddev(MLog::eGui);
    710     // --- lout << "Send 0x4000: " << GetNodeName() << endl;
    711 
     649void Macs::SendNodeguard()
     650{
    712651    SendSDO(0x4000, 0, (LWORD_t)0, false);
    713 
    714     // --- lout << "Done 0x4000: " << GetNodeName() << endl;
    715 
    716     Timer time;
    717 
    718     // --- lout << "dT " << GetNodeName() << ": " <<  dec <<(int)((fTimeoutTime-time.Now())*1000) << endl;
    719     // --- lout << edev(MLog::eGui);
    720 
    721     if (time.Now() > fTimeoutTime)
    722     {
    723         lout << ddev(MLog::eGui);
    724         lout << "Warning: " << GetNodeName() << " didn't respond in timeout window." << endl;
    725         lout << edev(MLog::eGui);
    726         SetError(101);
    727     }
    728 
    729     //WaitForSdo(0x4000, 0, kDontWait);
    730     //
    731     // Would not be necessary if I would Start the timer with
    732     // kFALSE. This would be a continous timer, but this
    733     // timer seems to stop it's activity at some stage without
    734     // any reason
    735     //
    736     if (fTimerOn)
    737         fTimeout->Start(fGuardTime/*/3*2*/, kTRUE);
    738 
    739     return kTRUE;
    740 }
    741 
    742 void Macs::ResetTimeout()
    743 {
    744     Timer time;
    745 
    746     // --- lout << ddev(MLog::eGui);
    747     // --- lout << "Reset " << GetNodeName() << ": " << dec << (int)((fTimeoutTime-time.Now())*1000) << endl;
    748     // --- lout << edev(MLog::eGui);
    749 
    750     fTimeoutTime = time.Now() + 3.*fGuardTime/1000.;
    751 }
    752 
    753 void Macs::SetTimeoutTime(LWORD_t ms)
    754 {
    755     // FIXME: Is '/2' the best choose?
    756     fGuardTime = ms/3;      // how often do we send/request the handshake
    757 
    758     SendSDO(0x4000, 2, ms*2); // How often do we check for the handshake
    759     WaitForSdo(0x4000, 2);
    760 }
    761 
     652}
     653
     654// --------------------------------------------------------------------------
     655//
     656// This starts the host guarding. The host guarding is only available
     657// if the node guarding is running. The host guarding works with the
     658// guardtime and the lifetimefactor from the nodeguarding.
     659//
     660void Macs::StartHostGuarding()
     661{
     662    SendSDO(0x100c, 0, (LWORD_t)GetGuardTime());
     663    WaitForSdo(0x100c);
     664
     665    SendSDO(0x100d, 0, (LWORD_t)GetLifeTimeFactor());
     666    WaitForSdo(0x100d);
     667
     668    lout << "- " << GetNodeName() << ": Hostguarding started (" << dec;
     669    lout << GetLifeTimeFactor() << "*" << GetGuardTime() << "ms)" << endl;
     670}
     671
     672// --------------------------------------------------------------------------
     673//
     674// Stop the host guarding.
     675//
     676void Macs::StopHostGuarding()
     677{
     678    SendSDO(0x100c, 0, (LWORD_t)0);
     679    WaitForSdo(0x100c);
     680
     681    SendSDO(0x100d, 0, (LWORD_t)0);
     682    WaitForSdo(0x100d);
     683
     684    lout << "- " << GetNodeName() << ": Hostguarding stopped." << endl;
     685}
     686

  • trunk/MagicSoft/Cosy/devdrv/macs.h

    r1703 r1727  
    11#ifndef MACS_H
    22#define MACS_H
    3 
    4 #include <TObject.h>
    53
    64#include "nodedrv.h"
    75#include "base/timer.h"
    86
    9 class Macs : public NodeDrv, public TObject
     7class Macs : public NodeDrv
    108{
    119private:
    … …  
    3028    BYTE_t   fStatus;
    3129
    32     TTimer  *fTimeout;
    33     Bool_t   fTimerOn;
    34     LWORD_t  fGuardTime;
    35     double   fTimeoutTime;
    36 
    3730    LWORD_t string(BYTE_t b0=0, BYTE_t b1=0, BYTE_t b2=0, BYTE_t b3=0)
    3831    {
    … …  
    4033    }
    4134
    42     void ResetTimeout();
    43     Bool_t HandleTimer(TTimer *t);
    44 
    4535    void Init();
    46     //bool Reboot();
    47 
    48     //bool InitDevice(Network *);
    49 
    50     //void StartDevice();
    5136    void StopDevice();
    5237
    5338    void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv);
    54     void HandleSDOOK(WORD_t idx, BYTE_t subidx);
    55     void HandleSDOError(LWORD_t data)          { NodeDrv::HandleSDOError(data); }
     39    void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv);
     40    //void HandleSDOError(LWORD_t data) { NodeDrv::HandleSDOError(data); }
    5641
    5742    void HandlePDO1(BYTE_t *data, timeval_t *tv);
    5843    void HandlePDO2(BYTE_t *data, timeval_t *tv);
     44    //void HandleNodeguard(timeval_t *tv);
     45
     46    void SendNodeguard();
    5947
    6048    void CheckConnection();
    … …  
    10896    void StartAbsPos(LWORDS_t pos);
    10997
    110     void SetTimeoutTime(LWORD_t ms);
    111     void ReqTimeoutTime();
    112 
    113     void EnableTimeout(bool enable=true, LWORDS_t ms=-1);
    114 
    11598    void StopMotor();
    11699
    … …  
    131114    LWORD_t  GetRes()    const { return fRes; }    // Encoder resolution
    132115
     116    void StartHostGuarding();
     117    void StopHostGuarding();
     118
    133119    void HandleError();
    134120

  • trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc

    r1703 r1727  
    11#include "shaftencoder.h"
    22
    3 #include "timer.h"
     3#include "base/timer.h"
    44#include "network.h"
    55
    … …  
    1717ShaftEncoder::ShaftEncoder(const BYTE_t nodeid, const char *name, MLog &out)
    1818    : NodeDrv(nodeid, name, out), fLabel(NULL), fPosHasChanged(false)
    19 {
    20 }
    21 
    22 ShaftEncoder::~ShaftEncoder()
    2319{
    2420}
    … …  
    5551        return;
    5652
     53    case 0x100c:
     54        lout << "- Guardtime: " << dec << val << "ms" << endl;
     55        return;
     56
     57    case 0x100d:
     58        lout << "- Lifetimefactor: " << dec << val << endl;
     59        return;
     60
     61    case 0x100e:
     62        lout << "- CobId for guarding: 0x" << hex << val << endl;
     63        return;
     64
    5765    case 0x6000:
    5866    case 0x6500:
    59         lout << "- Counting: " << (val&1   ?"anti-clockwise":"clockwise") << "  ";
     67        lout << "- Counting: " << (val&1 ?"anti-clockwise":"clockwise") << "  ";
    6068        lout << "HwTest: " <<   (val&2   ?"on":"off") << "  ";
    6169        lout << "Scaling: " <<  (val&4   ?"on":"off") << "  ";
    … …  
    95103void ShaftEncoder::DisplayVal()
    96104{
    97     if (fIsZombie)
     105    if (IsZombieNode())
    98106    {
    99107        fLabel[0]->SetText(new TGString(""));
    … …  
    162170}
    163171
     172//#include <fstream.h>
     173//ofstream fout("log/shaftencoder.log");
     174
    164175void ShaftEncoder::HandlePDOType2(BYTE_t *data, timeval_t *tv)
    165176{
    … …  
    188199    fPos  = pos;
    189200    fTurn = turn;
     201
     202    //fout << dec << (int)GetId() << " " << turn << " " << pos << endl;
     203
    190204    fTime.SetTimer(tv);
    191205    fPosHasChanged=true;
    … …  
    208222    //-----------------------------------------------------------------------
    209223
     224    StopGuarding();
     225
    210226    //
    211227    // Requesting and checking (FIXME) type of encoder
    … …  
    215231    WaitForSdo(0x1000);
    216232
    217     if (fIsZombie)
     233    if (IsZombieNode())
    218234    {
    219235        lout << GetNodeName() << " - Init failed!" << endl;
    … …  
    274290    lout << "- " << GetNodeName() << ": Start Node (NMT)." << endl;
    275291    SendNMT(kNMT_START);
     292
     293    StartGuarding(125, 2);
    276294}
    277295
    … …  
    282300    WaitForSdo(0x100b);
    283301}
    284 /*
    285 bool ShaftEncoder::InitDevice(Network *net)
    286 {
    287     NodeDrv::InitDevice(net);
    288 
    289     Init();
    290 
    291     return !fIsZombie;
    292 }
    293 
    294 // --------------------------------------------------------------------------
    295 //
    296 // This should be called from a master or main thread to get a node out
    297 // of the Zombie-Status.
    298 //
    299 bool ShaftEncoder::Reboot()
    300 {
    301     fIsZombie = false;
    302 
    303     Init();
    304 
    305     return !fIsZombie;
    306 }
    307 */
    308302
    309303void ShaftEncoder::ReqPos()
    … …  
    321315    lout << "- " << GetNodeName() << ": Setting Preset." << endl;
    322316
    323     SendSDO(0x6003, (LWORD_t)fPos);
     317    SendSDO(0x6003, (LWORD_t)pre);
    324318    if (!WaitForSdo(0x6003))
    325319        return;

  • trunk/MagicSoft/Cosy/devdrv/shaftencoder.h

    r1703 r1727  
    22#define SHAFTENCODER_H
    33
    4 #include "timer.h"
     4#include "base/timer.h"
    55#include "nodedrv.h"
    66
    … …  
    1717    WORD_t   fTurns; // Number of possible turns
    1818
    19     TGLabel **fLabel;
    20     LWORDS_t  fUpdPos;   // ticks
    21     WORDS_t   fUpdVel;   // ticks per 5ms
    22     WORDS_t   fUpdAcc;   // ticks per 25ms^2
     19    TGLabel **fLabel;     //
     20    LWORDS_t  fUpdPos;    // ticks
     21    WORDS_t   fUpdVel;    // ticks per 5ms
     22    WORDS_t   fUpdAcc;    // ticks per 25ms^2
    2323
    24     bool fPosHasChanged;
     24    bool fPosHasChanged;  //!
    2525
    2626    Timer fTime;
    … …  
    3737public:
    3838    ShaftEncoder(const BYTE_t nodeid, const char *name=NULL, MLog &out=gLog);
    39     virtual ~ShaftEncoder();
    4039
    41     //bool InitDevice(Network *);
    42     //bool Reboot();
    43 
    44     //void StartDevice();
    4540    void StopDevice();
    4641
    … …  
    4843
    4944    void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv);
    50     void HandleSDOOK(WORD_t idx, BYTE_t subidx) { NodeDrv::HandleSDOOK(idx, subidx); }
    51     void HandleSDOError(LWORD_t data)           { NodeDrv::HandleSDOError(data); }
    52 
     45    /*
     46     void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv) { NodeDrv::HandleSDOOK(idx, subidx, tv); }
     47     void HandleSDOError(LWORD_t data)           { NodeDrv::HandleSDOError(data); }
     48     */
    5349    void HandlePDO1(BYTE_t *data, timeval_t *tv) { HandlePDOType2(data, tv); }
    5450    void HandlePDO2(BYTE_t *data, timeval_t *tv) { HandlePDOType2(data, tv); }
    5551
    56     LWORDS_t GetPos() { return fIsZombie ? 0 : fPos+fTurn*fTicks; } // FIXME? 0?
     52    LWORDS_t GetPos() { return IsZombieNode() ? 0 : fPos+fTurn*fTicks; } // FIXME? 0?
    5753    LWORD_t  GetPhysRes() { return fTicks; }
    5854
Note: See TracChangeset for help on using the changeset viewer.