Changeset 9431 for trunk


Ignore:
Timestamp:
04/22/09 10:36:03 (15 years ago)
Author:
tbretz
Message:
*** empty log message ***
Location:
trunk/MagicSoft/Cosy/devdrv
Files:
3 edited

Legend:

Unmodified
Added
Removed

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

    r9132 r9431  
    2020    : NodeDrv(nodeid, name), fMacId(2*nodeid+1),
    2121    fPdoPos1(0), fPdoPos2(0), fPosActive(0), fRpmActive(0),
    22     fStatusDKC(0), fStatusPdo3(0xff), fArmed(false),
    23     fReport(NULL),fLabel(NULL)
     22    fStatus(0), fStatusDKC(0), fStatusPdo3(0xff), fArmed(false),
     23    fReport(NULL),fLabel(NULL),fUpdPos(0)
    2424{
    2525}
    … …  
    7979    case 0xe252: return "Bleeder overtemp warning";
    8080    case 0xe257: return "Continous current limit active";
     81    case 0xe259: return "Command velocity limit active";
    8182    case 0xe264: return "Target position out of numerical range";
    8283    case 0xe829: return "Positive position limit exceeded";

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

    r8869 r9431  
    1515ShaftEncoder::ShaftEncoder(const BYTE_t nodeid, const char *name)
    1616    : NodeDrv(nodeid, name),
    17     fPos(0), fVel(0), fAcc(0), fTurn(0), fTicks(0), fTurns(0),
    18     fDirection(kUndefined), fHysteresisPos(0), fHysteresisNeg(0),
     17    fPos(0), fVel(0), fAcc(0), fPosRes(0), fTurns(0),
    1918    fLabel(NULL), fUpdPos(0),
    2019    fPosHasChanged(false), fDirHasChanged(false),
    21     fReport(NULL), fMotor(NULL),
    22     fOffset(0), fDirChangedPos(0), fDirChangedOffset(0)
    23 {
    24 }
    25 /*
    26 void ShaftEncoder::CheckTwin(Int_t diff) const
    27 {
    28     if (!fTwin)
    29         return;
    30 
    31     if (fTwin->fIsUpdated)
    32         fTwin->fIsUpdated = kFALSE;
    33     else
    34         fTwin->fOffset += diff;
    35 }
    36 */
     20    fReport(NULL)
     21{
     22}
     23
    3724void ShaftEncoder::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv)
    3825{
    … …  
    9582        gLog << inf2 << "- Reported position: " << dec << (Int_t)val << endl;
    9683        fPos  = val;
    97         fTurn = 0;
    98         fDirChangedPos = val;
    99         fOffset = fMotor ? fMotor->GetPdoPos() : 0;
    100         fPosHasChanged = true;
    101         fDirHasChanged = true;
    102         fDirection = kUndefined;
    103         //fIsUpdated=kTRUE;
    104         //fOffset = 0;
    10584        return;
    10685
    10786
    10887    case 0x6501:
    109         gLog << inf2 << "- Phys. Ticks/Revolution: " << dec << val << endl;
    110         fTicks = val;
     88        gLog << inf2 << "- Position resolution Ticks/Revolution: " << dec << val << endl;
     89        fPosRes = val;
    11190        return;
    11291
    … …  
    179158}
    180159
    181 void ShaftEncoder::DisplayVal()
    182 {
    183     const LWORDS_t pos = GetPos();
    184     if (IsZombieNode())
    185     {
    186         fLabel->SetText(new TGString(""));
    187         fUpdPos = ~pos;
    188         return;
    189     }
    190 
    191     char text[21]="";
    192     if (pos!=fUpdPos && fLabel)
    193     {
    194         sprintf(text, "%ld", pos);
    195         fLabel->SetText(new TGString(text));
    196         fUpdPos = pos;
    197     }
    198 }
    199 
    200160void ShaftEncoder::HandlePDOType0(const BYTE_t *data, const timeval_t &tv)
    201161{
    … …  
    207167    //if (pos==fPos)
    208168    //    return;
    209 
    210     // Warning: A multiturn shaftencoder is assumed!
    211     if ((pos>fDirChangedPos && pos<fPos) ||
    212         (pos<fDirChangedPos && pos>fPos))
    213     {
    214         fDirChangedPos = pos;
    215         fDirHasChanged = true;
    216     }
    217     else
    218         fOffset = fMotor ? fMotor->GetPdoPos() : 0;
    219169
    220170    fPos = pos;
    … …  
    243193    //if (fPos==pos)
    244194    //    return;
    245 
    246     // Warning: A multiturn shaftencoder is assumed!
    247     if ((pos>fDirChangedPos && pos<fPos) ||
    248         (pos<fDirChangedPos && pos>fPos))
    249     {
    250         fDirChangedPos = pos;
    251         fDirHasChanged = true;
    252     }
    253     else
    254         fOffset = fMotor ? fMotor->GetPdoPos() : 0;
    255195
    256196    //CheckTwin(fPos-pos);
    … …  
    284224    fAcc = data[6] | (data[7]<<8);
    285225
    286     const int dnlim = fTicks/10;
    287     const int uplim = fTurns*fTicks-dnlim;
    288 
    289     int turn = fTurn;
    290 
    291     if (fPos > uplim && pos < dnlim)
    292         turn++;
    293 
    294     if (fPos < dnlim && pos > uplim)
    295         turn--;
    296 
    297     LWORDS_t multipos = pos+turn*fTicks; // SE position of multiturn shaftencoder
     226//    LWORDS_t multipos = pos+turn*fTicks; // SE position of multiturn shaftencoder
    298227
    299228    //if (fPos==pos && fTurn==fTurn)
    … …  
    302231    //CheckTwin(fPos-pos);
    303232
    304     fOffset = fMotor ? fMotor->GetPdoPos() : 0;
    305 
    306     // Warning: A multiturn shaftencoder is assumed!
    307     if ((multipos>fDirChangedPos && multipos<fPos) ||
    308         (multipos<fDirChangedPos && multipos>fPos))
    309     {
    310         fDirChangedPos = GetPos();
    311         fDirHasChanged = true;
    312         fDirChangedOffset = fOffset;
    313     }
    314     else
    315     {
    316         fDirHasChanged = false;
    317     }
    318 
    319     if (multipos>GetPos())
    320         fDirection = kForward;
    321     else
    322         if (multipos<GetPos())
    323             fDirection = kBackward;
    324         else
    325             fDirection = kUndefined;
    326 
    327 
    328233    fPos  = pos;
    329     fTurn = turn;
     234    //fTurn = turn;
    330235
    331236    fTime.Set(tv);
    … …  
    338243        fReport->Lock("ShaftEncoder::HandlePDOType2");
    339244        *fReport << "SE-REPORT " << (int)GetId() << " " << fTime << " PDO2 " << pos << " " << fVel << " " << fAcc << " " << GetNodeName() << endl;
    340         *fReport << "DIR-REPORT " << (int)GetId() << " " << (Int_t)fDirHasChanged << " " << (Int_t)fDirChangedPos << endl;
     245        //*fReport << "DIR-REPORT " << (int)GetId() << " " << (Int_t)fDirHasChanged << " " << (Int_t)fDirChangedPos << endl;
    341246        fReport->UnLock("ShaftEncoder::HandlePDOType2");
    342247    }
    … …  
    390295
    391296    //
    392     // Set logic ticks/revolution = physical ticks/revolution => scale factor = 1
    393     //
    394     gLog << inf2 << "- " << GetNodeName() << ": Configuring log. tick/rev (0x6001)." << endl;
    395     SendSDO(0x6001, fTicks);
    396     WaitForSdo(0x6001);
    397 
    398     //
    399     // Set maximum number of ticks (ticks * turns)
    400     //
    401     gLog << inf2 << "- " << GetNodeName() << ": Configuring max number of ticks (0x6002)." << endl;
    402     SendSDO(0x6002, (LWORD_t)(fTicks*fTurns));
    403     WaitForSdo(0x6002);
    404 
    405     //
    406     // Delete preset Value
    407     //
    408     gLog << inf2 << "- " << GetNodeName() << ": Delete preset value (0x6003)." << endl;
    409     SendSDO(0x6003, (LWORD_t)0xffffffff);
    410     WaitForSdo(0x6003);
    411 
    412     //
    413297    // Configure PDOs
    414298    //
    … …  
    428312    gLog << inf2 << "- " << GetNodeName() << ": Start Node (NMT)." << endl;
    429313    SendNMT(kNMT_START);
    430 
    431     /*
    432      cout << "---1---" << endl;
    433      MTimeout t(1000);
    434      while (!t.HasTimedOut())
    435       usleep(1);
    436      cout << "---2---" << endl;
    437      */
    438 
    439     // StartGuarding(200, 1, kTRUE); // 175
    440     // StartGuarding(10*GetId(), 2); // 175
    441314}
    442315
    … …  
    458331}
    459332
    460 void ShaftEncoder::SetPreset(LWORD_t pre)
    461 {
    462     gLog << inf2 << "- " << GetNodeName() << ": Setting Preset." << endl;
    463 
    464     SendSDO(0x6003, (LWORD_t)pre);
    465     if (!WaitForSdo(0x6003))
    466         return;
    467 
    468     fPos  = pre%fTicks;
    469     fTurn = pre/fTicks;
    470     fDirChangedPos = fPos;
    471     fOffset = fMotor ? fMotor->GetPdoPos() : 0;
    472 
    473     fPosHasChanged = true;
    474     fDirHasChanged = true;
    475 
    476     fDirection = kUndefined;
    477 }
    478 
    479333void ShaftEncoder::StopDevice()
    480334{
    … …  
    482336    SendNMT(kNMT_STOP);
    483337}
    484 

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

    r8862 r9431  
    1818{
    1919private:
    20     LWORDS_t fPos;   // ticks
    21     WORDS_t  fVel;   // ticks per 5ms
    22     WORDS_t  fAcc;   // ticks per 25ms^2
    23     WORDS_t  fTurn;  // Number of turn
    24     LWORD_t  fTicks; // Number of ticks per revolution (resolution)
    25     WORD_t   fTurns; // Number of possible turns
    26 
    27     Direction_t fDirection;
    28  
    29     Float_t fHysteresisPos;
    30     Float_t fHysteresisNeg;
     20    LWORDS_t fPos;    // ticks
     21    WORDS_t  fVel;    // ticks per 5ms
     22    WORDS_t  fAcc;    // ticks per 25ms^2
     23    LWORD_t  fPosRes; // Number of ticks per revolution (resolution)
     24    WORD_t   fTurns;  // Number of possible turns
    3125
    3226    TGLabel  *fLabel;     //
    … …  
    3832    MTime fTime;
    3933    MLog *fReport;
    40 
    41     Macs *fMotor;     // Corresponding Motor/MACS
    42     Int_t fOffset;    // offset between SE and Motor
    43     Int_t fDirChangedPos; // Last position at which the SE changed its moving direction
    44     Int_t fDirChangedOffset; // Offset between SE and Motor when SE changed its moving direction last
    45 
    4634
    4735    void HandlePDOType0(const BYTE_t *data, const timeval_t &tv);
    … …  
    6048    void StopDevice();
    6149
    62     void SetDisplay(TGLabel *label) { fLabel = label; }
    63     void SetMotor(Macs *m) { fMotor = m; }
    64     //void SetTwin(ShaftEncoder *se) { fTwin = se; }
    65 
    6650    void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv);
    6751    void HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv);
    68     /*
    69      void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv) { NodeDrv::HandleSDOOK(idx, subidx, tv); }
    70      void HandleSDOError(LWORD_t data)           { NodeDrv::HandleSDOError(data); }
    71      */
     52
    7253    void HandlePDO1(const BYTE_t *data, const timeval_t &tv) { HandlePDOType2(data, tv); }
    7354    void HandlePDO2(const BYTE_t *data, const timeval_t &tv) { HandlePDOType2(data, tv); }
    7455
    75     LWORDS_t GetPos() const       { return IsZombieNode() ? 0 : fPos+fTurn*fTicks; } // FIXME? 0?
    76     Int_t    GetDirection() const { return IsZombieNode() ? 0 : fPos-fDirChangedPos; } // FIXME? 0?
    77     Int_t    GetDirChangedPos() const { return IsZombieNode() ? 0 : fDirChangedPos; } // FIXME? 0?
    78     LWORD_t  GetPhysRes() const { return fTicks; }
    79     Int_t    GetOffset() const { return fOffset; }
    80     Int_t    GetDirChangedOffset() const { return fDirChangedOffset; }
    81 
    82     Int_t GetPosDirCorrected() const
    83     {
    84         return DirHasChanged() ? GetDirChangedPos() : GetPos();
    85     }
    86     Int_t GetOffsetDirCorrected() const
    87     {
    88         return DirHasChanged() ? GetDirChangedOffset() : GetOffset();
    89     }
    90 
    91     void     SetOffset(Int_t off) { fOffset = off; }
    92 
    9356    double GetMjd();
    94 
    95     void SetPreset(LWORD_t pre=0);
    96 
    97     void DisplayVal();
    98 
    99     bool PosHasChanged() const { return fPosHasChanged; }
    100     bool DirHasChanged() const { return fDirHasChanged; }
    101     void ResetPosHasChanged() { fPosHasChanged = false; }
    102     void ResetDirHasChanged() { fDirHasChanged = false; }
    103 
    104     void SetReport(MLog *log) { fReport = log; }
    105 
    106     void SetHysteresisNeg(Float_t f) { fHysteresisNeg = f; }
    107     void SetHysteresisPos(Float_t f) { fHysteresisPos = f; }
    108 
    109     Float_t GetPosCorrected() const
    110     {
    111         switch (fDirection)
    112         {
    113         case kUndefined:
    114             return GetPos();
    115         case kForward:
    116             return GetPos()-fHysteresisPos;
    117         case kBackward:
    118             return GetPos()+fHysteresisNeg;
    119         }
    120          return 0;
    121     }
    12257
    12358    ClassDef(ShaftEncoder, 0)
Note: See TracChangeset for help on using the changeset viewer.