Index: /trunk/MagicSoft/Cosy/.cosyrc =================================================================== --- /trunk/MagicSoft/Cosy/.cosyrc (revision 1741) +++ /trunk/MagicSoft/Cosy/.cosyrc (revision 1742) @@ -2,5 +2,5 @@ # -85.0 , +310.0 # -Az_MaxTime2ReachHome[s]: 100 +Az_MaxTime2ReachHome[s]: 150 Az_Min[Deg]: -30.0 Az_Max[Deg]: +375.0 @@ -9,5 +9,5 @@ # # -Zd_MaxTime2ReachHome[s]: 100 +Zd_MaxTime2ReachHome[s]: 150 Zd_Min[Deg]: -70.0 Zd_Max[Deg]: +70.0 Index: /trunk/MagicSoft/Cosy/Changelog =================================================================== --- /trunk/MagicSoft/Cosy/Changelog (revision 1741) +++ /trunk/MagicSoft/Cosy/Changelog (revision 1742) @@ -1,3 +1,26 @@ -*-*- END -*-*- + + 2003/01/23 - Thomas Bretz: + + * .cosyrc: + - changed maximum time to reach home from 100 to 150 + + * Makefile: + - added devdrv + + * Makefile.rules, candrv/Makefile, devdrv/Makefile: + - changed some ordering + + * devdrv/shaftencoder.cc: + - changed Guarding from 2x125 to 2x175 + + * main/MCosy.cc: + - fixed StopWaitingForSDO bug (if this was raised once the + 'shutdown' never waited correctly for an SDo anymore) + - fixed the usage of the bending correction in some places + + * testse.cc: + - added + 2003/01/23 - Thomas Bretz: Index: /trunk/MagicSoft/Cosy/Makefile =================================================================== --- /trunk/MagicSoft/Cosy/Makefile (revision 1741) +++ /trunk/MagicSoft/Cosy/Makefile (revision 1742) @@ -20,8 +20,8 @@ # -PROGRAMS = cosy starg +PROGRAMS = test cosy starg SOLIB = CINT = M -INCLUDES = -I. -Imain -Ibase -Icandrv -Iincl -Igui -Ivideodev -Icatalog +INCLUDES = -I. -Imain -Ibase -Icandrv -Iincl -Igui -Ivideodev -Icatalog -Idevdrv LIBS = -lpng -lz -L/usr/X11R6/lib -lpthread @@ -40,8 +40,8 @@ main \ gui \ + catalog \ + videodev \ + devdrv \ candrv \ - catalog \ - devdrv \ - videodev \ base \ slalib @@ -72,5 +72,5 @@ $(PROGRAMS): $(LIBRARIES) $(OBJS) $(HEADERS) $(PROGRAMS:=.o) @echo " Linking $@ ..." - $(CXX) $(CXXFLAGS) $(LIBS) $@.o $(OBJS) $(MARS_LIB) $(ROOTGLIBS) -o $@ + $(CXX) $(CXXFLAGS) $(LIBS) $(OBJS) $@.o $(MARS_LIB) $(ROOTGLIBS) -o $@ $(SOLIB): $(LIBRARIES) $(OBJS) $(HEADERS) MCint.o Index: /trunk/MagicSoft/Cosy/Makefile.rules =================================================================== --- /trunk/MagicSoft/Cosy/Makefile.rules (revision 1741) +++ /trunk/MagicSoft/Cosy/Makefile.rules (revision 1742) @@ -4,5 +4,5 @@ -f Makefile.depend 2> kk.kk ; cat kk.kk -$(LIB): $(OBJS) $(HEADERS) $(CINT)Cint.o +$(LIB): $(HEADERS) $(CINT)Cint.o $(OBJS) @echo " - Building Library lib$(LIB) ... " $(AR) $(LIB) *.o Index: /trunk/MagicSoft/Cosy/candrv/Makefile =================================================================== --- /trunk/MagicSoft/Cosy/candrv/Makefile (revision 1741) +++ /trunk/MagicSoft/Cosy/candrv/Makefile (revision 1742) @@ -31,8 +31,8 @@ .SUFFIXES: .c .cc .cxx .h .hxx .o -SRCFILES = vmodican.cc \ +SRCFILES = nodedrv.cc \ + vmodican.cc \ sdolist.cc \ canopen.cc \ - nodedrv.cc \ network.cc Index: /trunk/MagicSoft/Cosy/devdrv/Makefile =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/Makefile (revision 1741) +++ /trunk/MagicSoft/Cosy/devdrv/Makefile (revision 1742) @@ -32,6 +32,5 @@ .SUFFIXES: .c .cc .cxx .h .hxx .o -SRCFILES = macs.cc \ - shaftencoder.cc +SRCFILES = shaftencoder.cc macs.cc SRCS = $(SRCFILES) Index: /trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc =================================================================== --- /trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (revision 1741) +++ /trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc (revision 1742) @@ -291,5 +291,5 @@ SendNMT(kNMT_START); - StartGuarding(125, 2); + StartGuarding(175, 2); } Index: /trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- /trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1741) +++ /trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1742) @@ -187,11 +187,14 @@ // Wait until the objects are received. // + fMac2->WaitForSdo(0x6004); + fMac1->WaitForSdo(0x6004); + // FIXME, what when waiting times out (Zombie) - WaitForSdos(); + // WaitForSdos(); // // If waiting was not interrupted everything is ok. return. // - if (!StopWaitingForSDO() && !HasZombie()) + if (!(Break() || HasError()) && !HasZombie()) return kTRUE; @@ -255,9 +258,10 @@ { fMac1->SetVelocity(vel); + fMac2->SetVelocity(vel*ratio); + fMac1->SetAcceleration(acc); + fMac2->SetAcceleration(acc*ratio); + fMac1->SetDeceleration(acc); - - fMac2->SetVelocity(vel*ratio); - fMac2->SetAcceleration(acc*ratio); fMac2->SetDeceleration(acc*ratio); } @@ -265,9 +269,10 @@ { fMac1->SetVelocity(vel/ratio); + fMac2->SetVelocity(vel); + fMac1->SetAcceleration(acc/ratio); + fMac2->SetAcceleration(acc); + fMac1->SetDeceleration(acc/ratio); - - fMac2->SetVelocity(vel); - fMac2->SetAcceleration(acc); fMac2->SetDeceleration(acc); } @@ -285,4 +290,7 @@ void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) { + if (HasZombie()) + return; + SetStatus(MCosy::kMoving); @@ -292,4 +300,7 @@ cout << "Waiting for positioning..." << flush; + if (axe1) fMac2->WaitForSdo(0x6004, 1); + if (axe2) fMac1->WaitForSdo(0x6004, 1); + WaitForEndMovement(); @@ -303,5 +314,5 @@ int MCosy::StopWaitingForSDO() const { - return Break() || HasError(); + return 0/*Break() || HasError()*/; } @@ -316,7 +327,6 @@ { // FIXME, what when waiting times out (Zombie) - WaitForSdos(); - - while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO() && !HasZombie()) + while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && + !(Break() || HasError()) && !HasZombie()) usleep(1); } @@ -345,20 +355,24 @@ SetStatus(MCosy::kError); - // - // Now try to handle the error. - // - fMac1->HandleError(); - fMac2->HandleError(); - - // - // If the error couldn't get solved return - // - if (HasError()) - return; - - // - // Set motor status to stopped - // - SetStatus(MCosy::kStopped); + /* + FIXME: HANDLINGE ERROR + + // + // Now try to handle the error. + // + fMac1->HandleError(); + fMac2->HandleError(); + + // + // If the error couldn't get solved return + // + if (HasError()) + return; + + // + // Set motor status to stopped + // + SetStatus(MCosy::kStopped); + */ } @@ -390,12 +404,12 @@ // MACS has been rebooted from a Zombie state. // - InitSync(); - if (fMac3->IsZombieNode()) - return false; + //InitSync(); + //if (fMac3->IsZombieNode()) + // return false; // // Calculate new target position (shortest distance to go) // - const ZdAz src = GetSePos(); + const ZdAz src = GetSePos(); // [se] // @@ -405,6 +419,6 @@ // const ZdAz dest = CorrectTarget(src, dst); // - fZdAzSoll = fBending(dst); - const ZdAz dest = fZdAzSoll*16384/2/TMath::Pi(); + const ZdAz dest = fBending(dst)*16384/2/TMath::Pi(); // [se] + fZdAzSoll = dst; lout << "Positioning to Target..." << endl; @@ -414,5 +428,5 @@ cout << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; - for (int i=0; i<10 && !StopWaitingForSDO() && !HasZombie(); i++) + for (int i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++) { // @@ -500,11 +514,15 @@ // Wait for the objects to be OKed. // + + fMac2->WaitForSdo(0x3006, 1); + fMac1->WaitForSdo(0x3006, 1); + // FIXME, what when waiting times out (Zombie) - WaitForSdos(); + // WaitForSdos(); // // If the waiting for the objects wasn't interrupted return kTRUE // - if (!StopWaitingForSDO() && !HasZombie()) + if (!(Break() || HasError()) && !HasZombie()) return kTRUE; @@ -693,5 +711,5 @@ // const float dt = 1; // 1 second - while (!StopWaitingForSDO() && !HasZombie()) + while (!(Break() || HasError()) && !HasZombie()) { // @@ -706,4 +724,7 @@ ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); dest = CorrectTarget(GetSePos(), dummy); // [se] + dest *= kGearRatio; // [re] + + ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min] // @@ -719,5 +740,4 @@ // Shaft- and the rotary encoders // - dest *= kGearRatio; // [re] dest -= GetRePos() + fOffset; @@ -733,5 +753,4 @@ // ZdAz vt = v/4; - ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min] LimitSpeed(&vt, vcalc); vt.Round(); @@ -804,12 +823,10 @@ // set deceleration to 50% // - cout << "Stopping..." << endl; + cout << "Stopping movement..." << endl; fMac1->SetDeceleration(0.5*fMac1->GetVelRes()); - if (!fMac1->IsZombieNode()) - fMac1->SetRpmMode(FALSE); - fMac2->SetDeceleration(0.5*fMac2->GetVelRes()); - if (!fMac2->IsZombieNode()) - fMac2->SetRpmMode(FALSE); + + fMac1->SetRpmMode(FALSE); + fMac2->SetRpmMode(FALSE); // @@ -828,5 +845,5 @@ bool MCosy::CheckNetwork() { - CheckConnections(); + //CheckConnections(); if (HasZombie()) { @@ -835,4 +852,17 @@ return false; } + + /* + FIXME HANDLING ERROR + */ + if (HasError()) + { + fMac1->HandleError(); + fMac2->HandleError(); + fMac3->HandleError(); + if (HasError() || HasZombie()) + return false; + } + return true; } @@ -969,6 +999,13 @@ cout << "Going Home..." << endl; TEnv env(".cosyrc"); + + SetStatus(MCosy::kMoving); + fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100)); fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100)); + + lout << "SETHOME DONE" << endl; + + SetStatus(HasError() ? MCosy::kError : MCosy::kStopped); fAz->SetPreset(); @@ -978,4 +1015,5 @@ fMac1->ReqPos(); fMac2->ReqPos(); + fMac3->StopMotor(); } cout << "WM_Home: done. (return 0x403e)" << endl; @@ -1038,5 +1076,5 @@ Double_t resreaz = 0; - if (!fMac1->IsZombieNode()) + if (fMac1 && !fMac1->IsZombieNode()) resreaz = fMac1->GetRes(); else @@ -1047,14 +1085,14 @@ Double_t resrezd = 0; - if (!fMac2->IsZombieNode()) + if (fMac2 && !fMac2->IsZombieNode()) resrezd = fMac2->GetRes(); else resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500); - + Double_t ressezd = 0; - if (!fZd1->IsZombieNode()) + if (fZd1 && !fZd1->IsZombieNode()) ressezd = fZd1->GetPhysRes(); else - if (!fZd2->IsZombieNode()) + if (fZd2 && !fZd2->IsZombieNode()) ressezd = fZd2->GetPhysRes(); else @@ -1062,5 +1100,5 @@ Double_t resseaz = 0; - if (!fAz->IsZombieNode()) + if (fAz && !fAz->IsZombieNode()) resseaz = fAz->GetPhysRes(); else @@ -1080,5 +1118,8 @@ { if (!fMac3) + { + lout << "Unable to Init Sync! Mac3 not available." << endl; return; + } const int res = fMac3->GetVelRes(); @@ -1095,52 +1136,38 @@ if (fMac1->IsZombieNode() || fMac2->IsZombieNode()) return; - */ - fMac1->ReqPos(); - fMac2->ReqPos(); + */ + + if (fMac1 && fMac2) + { + fMac1->ReqPos(); + fMac2->ReqPos(); + } + + InitSync(); + + /*** FOR DEMO MODE ***/ + if (!fZd1 || !fZd2 || !fAz) + return; + /*** FOR DEMO MODE ***/ if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode()) return; + if (!fMac1 || !fMac2) + return; // // Start the Network // - //cout << "Reading configuration file..." << flush; - //TEnv env(".cosyrc"); - //cout << "done." << endl; -/* - const int res = fMac3->GetVelRes(); - - fMac3->SetVelocity(res); - fMac3->SetAcceleration(res); - fMac3->SetDeceleration(res); - - fMac1->ReqPos(); - fMac2->ReqPos(); - - //const ZdAz repos=GetRePos(); - //cout << "APOS: " << repos.Zd() << "re, " << repos.Az() << "re" << endl; - - //cout << Deg2AzRE(env.GetValue("MinAz[Deg]", -1.0)) << " < Az < " - //SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0))); - fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0))); - - fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0))); - fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0))); - cout << "done." << endl; - - fMac1->EnableTimeout(kTRUE, 500); - fMac2->EnableTimeout(kTRUE, 500); - - //fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0))); - //fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0))); - // fMac3->StartVelSync(); + /* + TEnv env(".cosyrc"); + + cout << "Setting up software endswitch..." << flush; + fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0))); + fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0))); + + fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0))); + fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0))); + cout << "done." << endl; */ SlaStars sla; @@ -1220,5 +1247,9 @@ // FIXME: I cannot take the avarage // - time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); + if (fZd1->GetMjd()>fZd2->GetMjd()) + time.Zd(fZd1->GetMjd()); + else + time.Zd(fZd2->GetMjd()); + //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0); time.Az(fAz->GetMjd()); @@ -1247,6 +1278,6 @@ } - fZdAzSoll.Set(sollzd.Zd(), sollaz.Az()); - fZdAzSoll *= 2*TMath::Pi()/16384; + ZdAz soll(sollzd.Zd(), sollaz.Az()); + fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384); fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(), @@ -1268,7 +1299,10 @@ // Update Gui, foremer MTGui. // - fZd1->DisplayVal(); - fZd2->DisplayVal(); - fAz->DisplayVal(); + if (fZd1) + fZd1->DisplayVal(); + if (fZd2) + fZd2->DisplayVal(); + if (fAz) + fAz->DisplayVal(); ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se] @@ -1277,13 +1311,19 @@ Byte_t avail = 0; - avail |= !fMac1->IsZombieNode() ? 0x01 : 0; - avail |= !fMac2->IsZombieNode() ? 0x02 : 0; + avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0; + avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0; avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0; - avail |= !fZd1->IsZombieNode() ? 0x08 : 0; - avail |= !fZd2->IsZombieNode() ? 0x10 : 0; - avail |= !fAz->IsZombieNode() ? 0x20 : 0; + avail |= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0; + avail |= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0; + avail |= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0; + + if (HasError()) + SetStatus(MCosy::kError); + + lout.UpdateGui(); fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError/kGearRatio2, fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail); + /* @@ -1447,8 +1487,10 @@ case 0: lout << "<>" << endl; + fBending.Load("bending.txt"); Constructor(id1, id2, id3, id4, id5, id6); break; case 1: lout << "<>" << endl; + fBending.Load("bending.txt"); ConstructorSE(id4, id5, id6); break; Index: /trunk/MagicSoft/Cosy/testse.cc =================================================================== --- /trunk/MagicSoft/Cosy/testse.cc (revision 1742) +++ /trunk/MagicSoft/Cosy/testse.cc (revision 1742) @@ -0,0 +1,73 @@ +#include +/*#include +#include + +#include +#include +#include + +*/ + +#include "network.h" +#include "shaftencoder.h" + +TROOT root("Cosy", "Magic Control System"); + +int main(int argc, char **argv) +{ +// TApplication* app = new TApplication("App", &argc, argv); +// MLog *l = new MLog("log/cosy.log", kTRUE); +// MLog &lout = *l; +// gLog.EnableOutputDevice(MLog::eStdout); + + cout << "Creating Network..." << endl; + const int baud = 500; + Network net("/dev/dpm_00", baud); + + cout << "Creating SE..." << endl; + const int nodeid = 6; + ShaftEncoder se(nodeid, "SE/Zd1"); + + cout << "Adding SE..." << endl; + net.SetNode(&se); + + // Don't call this function twice! + cout << "Start Network..." << endl; + net.Start(); + + if (se.IsZombieNode()) + { + cout << " /-------------------------------------\\" << endl; + cout << " >>>>> Initialization of ShaftEncoder failed <<<<<" << endl; + cout << " \\-------------------------------------/" << endl; + } + else + { +#ifndef TESTPDO + while (1) + { + se.RequestSDO(0x6004); + se.WaitForSdo(0x6004); + if (se.IsZombieNode()) + { + cout << " /------------------------------\\" << endl; + cout << " >>>>> Reading of ShaftEncoder failed <<<<<" << endl; + cout << " \\------------------------------/" << endl; + break; + } + usleep(100000); + } +#else + for (int i=0; i<50; i++) + { + cout << se.GetPos() << endl; + usleep(100000); + } +#endif + } + + cout << "Stop Network..." << endl; + net.Stop(); + + cout << "The End." << endl; +}