Changeset 1742
- Timestamp:
- 02/06/03 11:28:05 (22 years ago)
- Location:
- trunk/MagicSoft/Cosy
- Files:
-
- 1 added
- 8 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/MagicSoft/Cosy/.cosyrc
r1699 r1742 2 2 # -85.0 , +310.0 3 3 # 4 Az_MaxTime2ReachHome[s]: 1 004 Az_MaxTime2ReachHome[s]: 150 5 5 Az_Min[Deg]: -30.0 6 6 Az_Max[Deg]: +375.0 … … 9 9 # 10 10 # 11 Zd_MaxTime2ReachHome[s]: 1 0011 Zd_MaxTime2ReachHome[s]: 150 12 12 Zd_Min[Deg]: -70.0 13 13 Zd_Max[Deg]: +70.0 -
trunk/MagicSoft/Cosy/Changelog
r1727 r1742 1 1 -*-*- END -*-*- 2 3 2003/01/23 - Thomas Bretz: 4 5 * .cosyrc: 6 - changed maximum time to reach home from 100 to 150 7 8 * Makefile: 9 - added devdrv 10 11 * Makefile.rules, candrv/Makefile, devdrv/Makefile: 12 - changed some ordering 13 14 * devdrv/shaftencoder.cc: 15 - changed Guarding from 2x125 to 2x175 16 17 * main/MCosy.cc: 18 - fixed StopWaitingForSDO bug (if this was raised once the 19 'shutdown' never waited correctly for an SDo anymore) 20 - fixed the usage of the bending correction in some places 21 22 * testse.cc: 23 - added 24 2 25 3 26 2003/01/23 - Thomas Bretz: -
trunk/MagicSoft/Cosy/Makefile
r1699 r1742 20 20 # 21 21 22 PROGRAMS = cosy starg22 PROGRAMS = test cosy starg 23 23 SOLIB = 24 24 CINT = M 25 INCLUDES = -I. -Imain -Ibase -Icandrv -Iincl -Igui -Ivideodev -Icatalog 25 INCLUDES = -I. -Imain -Ibase -Icandrv -Iincl -Igui -Ivideodev -Icatalog -Idevdrv 26 26 LIBS = -lpng -lz -L/usr/X11R6/lib -lpthread 27 27 … … 40 40 main \ 41 41 gui \ 42 catalog \ 43 videodev \ 44 devdrv \ 42 45 candrv \ 43 catalog \44 devdrv \45 videodev \46 46 base \ 47 47 slalib … … 72 72 $(PROGRAMS): $(LIBRARIES) $(OBJS) $(HEADERS) $(PROGRAMS:=.o) 73 73 @echo " Linking $@ ..." 74 $(CXX) $(CXXFLAGS) $(LIBS) $ @.o $(OBJS)$(MARS_LIB) $(ROOTGLIBS) -o $@74 $(CXX) $(CXXFLAGS) $(LIBS) $(OBJS) $@.o $(MARS_LIB) $(ROOTGLIBS) -o $@ 75 75 76 76 $(SOLIB): $(LIBRARIES) $(OBJS) $(HEADERS) MCint.o -
trunk/MagicSoft/Cosy/Makefile.rules
r920 r1742 4 4 -f Makefile.depend 2> kk.kk ; cat kk.kk 5 5 6 $(LIB): $( OBJS) $(HEADERS) $(CINT)Cint.o6 $(LIB): $(HEADERS) $(CINT)Cint.o $(OBJS) 7 7 @echo " - Building Library lib$(LIB) ... " 8 8 $(AR) $(LIB) *.o -
trunk/MagicSoft/Cosy/candrv/Makefile
r1109 r1742 31 31 .SUFFIXES: .c .cc .cxx .h .hxx .o 32 32 33 SRCFILES = vmodican.cc \ 33 SRCFILES = nodedrv.cc \ 34 vmodican.cc \ 34 35 sdolist.cc \ 35 36 canopen.cc \ 36 nodedrv.cc \37 37 network.cc 38 38 -
trunk/MagicSoft/Cosy/devdrv/Makefile
r1109 r1742 32 32 .SUFFIXES: .c .cc .cxx .h .hxx .o 33 33 34 SRCFILES = macs.cc \ 35 shaftencoder.cc 34 SRCFILES = shaftencoder.cc macs.cc 36 35 37 36 SRCS = $(SRCFILES) -
trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc
r1727 r1742 291 291 SendNMT(kNMT_START); 292 292 293 StartGuarding(1 25, 2);293 StartGuarding(175, 2); 294 294 } 295 295 -
trunk/MagicSoft/Cosy/main/MCosy.cc
r1703 r1742 187 187 // Wait until the objects are received. 188 188 // 189 fMac2->WaitForSdo(0x6004); 190 fMac1->WaitForSdo(0x6004); 191 189 192 // FIXME, what when waiting times out (Zombie) 190 WaitForSdos();193 // WaitForSdos(); 191 194 192 195 // 193 196 // If waiting was not interrupted everything is ok. return. 194 197 // 195 if (! StopWaitingForSDO() && !HasZombie())198 if (!(Break() || HasError()) && !HasZombie()) 196 199 return kTRUE; 197 200 … … 255 258 { 256 259 fMac1->SetVelocity(vel); 260 fMac2->SetVelocity(vel*ratio); 261 257 262 fMac1->SetAcceleration(acc); 263 fMac2->SetAcceleration(acc*ratio); 264 258 265 fMac1->SetDeceleration(acc); 259 260 fMac2->SetVelocity(vel*ratio);261 fMac2->SetAcceleration(acc*ratio);262 266 fMac2->SetDeceleration(acc*ratio); 263 267 } … … 265 269 { 266 270 fMac1->SetVelocity(vel/ratio); 271 fMac2->SetVelocity(vel); 272 267 273 fMac1->SetAcceleration(acc/ratio); 274 fMac2->SetAcceleration(acc); 275 268 276 fMac1->SetDeceleration(acc/ratio); 269 270 fMac2->SetVelocity(vel);271 fMac2->SetAcceleration(acc);272 277 fMac2->SetDeceleration(acc); 273 278 } … … 285 290 void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) 286 291 { 292 if (HasZombie()) 293 return; 294 287 295 SetStatus(MCosy::kMoving); 288 296 … … 292 300 cout << "Waiting for positioning..." << flush; 293 301 302 if (axe1) fMac2->WaitForSdo(0x6004, 1); 303 if (axe2) fMac1->WaitForSdo(0x6004, 1); 304 294 305 WaitForEndMovement(); 295 306 … … 303 314 int MCosy::StopWaitingForSDO() const 304 315 { 305 return Break() || HasError();316 return 0/*Break() || HasError()*/; 306 317 } 307 318 … … 316 327 { 317 328 // FIXME, what when waiting times out (Zombie) 318 WaitForSdos(); 319 320 while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO() && !HasZombie()) 329 while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && 330 !(Break() || HasError()) && !HasZombie()) 321 331 usleep(1); 322 332 } … … 345 355 SetStatus(MCosy::kError); 346 356 347 // 348 // Now try to handle the error. 349 // 350 fMac1->HandleError(); 351 fMac2->HandleError(); 352 353 // 354 // If the error couldn't get solved return 355 // 356 if (HasError()) 357 return; 358 359 // 360 // Set motor status to stopped 361 // 362 SetStatus(MCosy::kStopped); 357 /* 358 FIXME: HANDLINGE ERROR 359 360 // 361 // Now try to handle the error. 362 // 363 fMac1->HandleError(); 364 fMac2->HandleError(); 365 366 // 367 // If the error couldn't get solved return 368 // 369 if (HasError()) 370 return; 371 372 // 373 // Set motor status to stopped 374 // 375 SetStatus(MCosy::kStopped); 376 */ 363 377 } 364 378 … … 390 404 // MACS has been rebooted from a Zombie state. 391 405 // 392 InitSync();393 if (fMac3->IsZombieNode())394 return false;406 //InitSync(); 407 //if (fMac3->IsZombieNode()) 408 // return false; 395 409 396 410 // 397 411 // Calculate new target position (shortest distance to go) 398 412 // 399 const ZdAz src = GetSePos(); 413 const ZdAz src = GetSePos(); // [se] 400 414 401 415 // … … 405 419 // const ZdAz dest = CorrectTarget(src, dst); 406 420 // 407 fZdAzSoll = fBending(dst);408 const ZdAz dest = fZdAzSoll*16384/2/TMath::Pi();421 const ZdAz dest = fBending(dst)*16384/2/TMath::Pi(); // [se] 422 fZdAzSoll = dst; 409 423 410 424 lout << "Positioning to Target..." << endl; … … 414 428 cout << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; 415 429 416 for (int i=0; i<10 && ! StopWaitingForSDO() && !HasZombie(); i++)430 for (int i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++) 417 431 { 418 432 // … … 500 514 // Wait for the objects to be OKed. 501 515 // 516 517 fMac2->WaitForSdo(0x3006, 1); 518 fMac1->WaitForSdo(0x3006, 1); 519 502 520 // FIXME, what when waiting times out (Zombie) 503 WaitForSdos();521 // WaitForSdos(); 504 522 505 523 // 506 524 // If the waiting for the objects wasn't interrupted return kTRUE 507 525 // 508 if (! StopWaitingForSDO() && !HasZombie())526 if (!(Break() || HasError()) && !HasZombie()) 509 527 return kTRUE; 510 528 … … 693 711 // 694 712 const float dt = 1; // 1 second 695 while (! StopWaitingForSDO() && !HasZombie())713 while (!(Break() || HasError()) && !HasZombie()) 696 714 { 697 715 // … … 706 724 ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); 707 725 dest = CorrectTarget(GetSePos(), dummy); // [se] 726 dest *= kGearRatio; // [re] 727 728 ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min] 708 729 709 730 // … … 719 740 // Shaft- and the rotary encoders 720 741 // 721 dest *= kGearRatio; // [re]722 742 dest -= GetRePos() + fOffset; 723 743 … … 733 753 // 734 754 ZdAz vt = v/4; 735 ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min]736 755 LimitSpeed(&vt, vcalc); 737 756 vt.Round(); … … 804 823 // set deceleration to 50% 805 824 // 806 cout << "Stopping ..." << endl;825 cout << "Stopping movement..." << endl; 807 826 fMac1->SetDeceleration(0.5*fMac1->GetVelRes()); 808 if (!fMac1->IsZombieNode())809 fMac1->SetRpmMode(FALSE);810 811 827 fMac2->SetDeceleration(0.5*fMac2->GetVelRes()); 812 if (!fMac2->IsZombieNode()) 813 fMac2->SetRpmMode(FALSE); 828 829 fMac1->SetRpmMode(FALSE); 830 fMac2->SetRpmMode(FALSE); 814 831 815 832 // … … 828 845 bool MCosy::CheckNetwork() 829 846 { 830 CheckConnections();847 //CheckConnections(); 831 848 if (HasZombie()) 832 849 { … … 835 852 return false; 836 853 } 854 855 /* 856 FIXME HANDLING ERROR 857 */ 858 if (HasError()) 859 { 860 fMac1->HandleError(); 861 fMac2->HandleError(); 862 fMac3->HandleError(); 863 if (HasError() || HasZombie()) 864 return false; 865 } 866 837 867 return true; 838 868 } … … 969 999 cout << "Going Home..." << endl; 970 1000 TEnv env(".cosyrc"); 1001 1002 SetStatus(MCosy::kMoving); 1003 971 1004 fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100)); 972 1005 fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100)); 1006 1007 lout << "SETHOME DONE" << endl; 1008 1009 SetStatus(HasError() ? MCosy::kError : MCosy::kStopped); 973 1010 974 1011 fAz->SetPreset(); … … 978 1015 fMac1->ReqPos(); 979 1016 fMac2->ReqPos(); 1017 fMac3->StopMotor(); 980 1018 } 981 1019 cout << "WM_Home: done. (return 0x403e)" << endl; … … 1038 1076 1039 1077 Double_t resreaz = 0; 1040 if ( !fMac1->IsZombieNode())1078 if (fMac1 && !fMac1->IsZombieNode()) 1041 1079 resreaz = fMac1->GetRes(); 1042 1080 else … … 1047 1085 1048 1086 Double_t resrezd = 0; 1049 if ( !fMac2->IsZombieNode())1087 if (fMac2 && !fMac2->IsZombieNode()) 1050 1088 resrezd = fMac2->GetRes(); 1051 1089 else 1052 1090 resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500); 1053 1091 1054 1092 Double_t ressezd = 0; 1055 if ( !fZd1->IsZombieNode())1093 if (fZd1 && !fZd1->IsZombieNode()) 1056 1094 ressezd = fZd1->GetPhysRes(); 1057 1095 else 1058 if ( !fZd2->IsZombieNode())1096 if (fZd2 && !fZd2->IsZombieNode()) 1059 1097 ressezd = fZd2->GetPhysRes(); 1060 1098 else … … 1062 1100 1063 1101 Double_t resseaz = 0; 1064 if ( !fAz->IsZombieNode())1102 if (fAz && !fAz->IsZombieNode()) 1065 1103 resseaz = fAz->GetPhysRes(); 1066 1104 else … … 1080 1118 { 1081 1119 if (!fMac3) 1120 { 1121 lout << "Unable to Init Sync! Mac3 not available." << endl; 1082 1122 return; 1123 } 1083 1124 1084 1125 const int res = fMac3->GetVelRes(); … … 1095 1136 if (fMac1->IsZombieNode() || fMac2->IsZombieNode()) 1096 1137 return; 1097 */ 1098 fMac1->ReqPos(); 1099 fMac2->ReqPos(); 1138 */ 1139 1140 if (fMac1 && fMac2) 1141 { 1142 fMac1->ReqPos(); 1143 fMac2->ReqPos(); 1144 } 1145 1146 InitSync(); 1147 1148 /*** FOR DEMO MODE ***/ 1149 if (!fZd1 || !fZd2 || !fAz) 1150 return; 1151 /*** FOR DEMO MODE ***/ 1100 1152 1101 1153 if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode()) 1102 1154 return; 1103 1155 1156 if (!fMac1 || !fMac2) 1157 return; 1104 1158 1105 1159 // 1106 1160 // Start the Network 1107 1161 // 1108 //cout << "Reading configuration file..." << flush; 1109 //TEnv env(".cosyrc"); 1110 //cout << "done." << endl; 1111 /* 1112 const int res = fMac3->GetVelRes(); 1113 1114 fMac3->SetVelocity(res); 1115 fMac3->SetAcceleration(res); 1116 fMac3->SetDeceleration(res); 1117 1118 fMac1->ReqPos(); 1119 fMac2->ReqPos(); 1120 1121 //const ZdAz repos=GetRePos(); 1122 //cout << "APOS: " << repos.Zd() << "re, " << repos.Az() << "re" << endl; 1123 1124 //cout << Deg2AzRE(env.GetValue("MinAz[Deg]", -1.0)) << " < Az < " 1125 //</< Deg2AzRE(env.GetValue("MaxAz[Deg]", +1.0)) << "RE" << endl; 1126 //cout << env.GetValue("MinAz[Deg]", -1.0) << " < Az < " 1127 //<< env.GetValue("MaxAz[Deg]", +1.0) << kDEG << endl; 1128 //cout << Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)) << "RE < Zd < " 1129 //<< Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)) << "RE" << endl; 1130 1131 cout << "Setting up software endswitch..." << flush; 1132 fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0))); 1133 fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0))); 1134 1135 fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0))); 1136 fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0))); 1137 cout << "done." << endl; 1138 1139 fMac1->EnableTimeout(kTRUE, 500); 1140 fMac2->EnableTimeout(kTRUE, 500); 1141 1142 //fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0))); 1143 //fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0))); 1144 // fMac3->StartVelSync(); 1162 /* 1163 TEnv env(".cosyrc"); 1164 1165 cout << "Setting up software endswitch..." << flush; 1166 fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0))); 1167 fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0))); 1168 1169 fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0))); 1170 fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0))); 1171 cout << "done." << endl; 1145 1172 */ 1146 1173 SlaStars sla; … … 1220 1247 // FIXME: I cannot take the avarage 1221 1248 // 1222 time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); 1249 if (fZd1->GetMjd()>fZd2->GetMjd()) 1250 time.Zd(fZd1->GetMjd()); 1251 else 1252 time.Zd(fZd2->GetMjd()); 1253 //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); 1223 1254 time.Az(fAz->GetMjd()); 1224 1255 … … 1247 1278 } 1248 1279 1249 fZdAzSoll.Set(sollzd.Zd(), sollaz.Az());1250 fZdAzSoll *= 2*TMath::Pi()/16384;1280 ZdAz soll(sollzd.Zd(), sollaz.Az()); 1281 fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384); 1251 1282 1252 1283 fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(), … … 1268 1299 // Update Gui, foremer MTGui. 1269 1300 // 1270 fZd1->DisplayVal(); 1271 fZd2->DisplayVal(); 1272 fAz->DisplayVal(); 1301 if (fZd1) 1302 fZd1->DisplayVal(); 1303 if (fZd2) 1304 fZd2->DisplayVal(); 1305 if (fAz) 1306 fAz->DisplayVal(); 1273 1307 1274 1308 ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se] … … 1277 1311 Byte_t avail = 0; 1278 1312 1279 avail |= !fMac1->IsZombieNode() ? 0x01 : 0;1280 avail |= !fMac2->IsZombieNode() ? 0x02 : 0;1313 avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0; 1314 avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0; 1281 1315 avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0; 1282 avail |= !fZd1->IsZombieNode() ? 0x08 : 0; 1283 avail |= !fZd2->IsZombieNode() ? 0x10 : 0; 1284 avail |= !fAz->IsZombieNode() ? 0x20 : 0; 1316 avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0; 1317 avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0; 1318 avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0; 1319 1320 if (HasError()) 1321 SetStatus(MCosy::kError); 1322 1323 lout.UpdateGui(); 1285 1324 1286 1325 fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError/kGearRatio2, 1287 1326 fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail); 1327 1288 1328 1289 1329 /* … … 1447 1487 case 0: 1448 1488 lout << "<<Stanard mode>>" << endl; 1489 fBending.Load("bending.txt"); 1449 1490 Constructor(id1, id2, id3, id4, id5, id6); 1450 1491 break; 1451 1492 case 1: 1452 1493 lout << "<<SE mode>>" << endl; 1494 fBending.Load("bending.txt"); 1453 1495 ConstructorSE(id4, id5, id6); 1454 1496 break;
Note:
See TracChangeset
for help on using the changeset viewer.