Changeset 9431 for trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc
- Timestamp:
- 04/22/09 10:36:03 (15 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc
r8869 r9431 15 15 ShaftEncoder::ShaftEncoder(const BYTE_t nodeid, const char *name) 16 16 : 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), 19 18 fLabel(NULL), fUpdPos(0), 20 19 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 37 24 void ShaftEncoder::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, const timeval_t &tv) 38 25 { … … 95 82 gLog << inf2 << "- Reported position: " << dec << (Int_t)val << endl; 96 83 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;105 84 return; 106 85 107 86 108 87 case 0x6501: 109 gLog << inf2 << "- P hys.Ticks/Revolution: " << dec << val << endl;110 f Ticks = val;88 gLog << inf2 << "- Position resolution Ticks/Revolution: " << dec << val << endl; 89 fPosRes = val; 111 90 return; 112 91 … … 179 158 } 180 159 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 200 160 void ShaftEncoder::HandlePDOType0(const BYTE_t *data, const timeval_t &tv) 201 161 { … … 207 167 //if (pos==fPos) 208 168 // 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 else218 fOffset = fMotor ? fMotor->GetPdoPos() : 0;219 169 220 170 fPos = pos; … … 243 193 //if (fPos==pos) 244 194 // 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 else254 fOffset = fMotor ? fMotor->GetPdoPos() : 0;255 195 256 196 //CheckTwin(fPos-pos); … … 284 224 fAcc = data[6] | (data[7]<<8); 285 225 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 298 227 299 228 //if (fPos==pos && fTurn==fTurn) … … 302 231 //CheckTwin(fPos-pos); 303 232 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 else315 {316 fDirHasChanged = false;317 }318 319 if (multipos>GetPos())320 fDirection = kForward;321 else322 if (multipos<GetPos())323 fDirection = kBackward;324 else325 fDirection = kUndefined;326 327 328 233 fPos = pos; 329 fTurn = turn;234 //fTurn = turn; 330 235 331 236 fTime.Set(tv); … … 338 243 fReport->Lock("ShaftEncoder::HandlePDOType2"); 339 244 *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; 341 246 fReport->UnLock("ShaftEncoder::HandlePDOType2"); 342 247 } … … 390 295 391 296 // 392 // Set logic ticks/revolution = physical ticks/revolution => scale factor = 1393 //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 Value407 //408 gLog << inf2 << "- " << GetNodeName() << ": Delete preset value (0x6003)." << endl;409 SendSDO(0x6003, (LWORD_t)0xffffffff);410 WaitForSdo(0x6003);411 412 //413 297 // Configure PDOs 414 298 // … … 428 312 gLog << inf2 << "- " << GetNodeName() << ": Start Node (NMT)." << endl; 429 313 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); // 175440 // StartGuarding(10*GetId(), 2); // 175441 314 } 442 315 … … 458 331 } 459 332 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 479 333 void ShaftEncoder::StopDevice() 480 334 { … … 482 336 SendNMT(kNMT_STOP); 483 337 } 484
Note:
See TracChangeset
for help on using the changeset viewer.