Index: trunk/MagicSoft/Cosy/main/MBending.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MBending.cc (revision 1691) +++ trunk/MagicSoft/Cosy/main/MBending.cc (revision 1699) @@ -7,4 +7,22 @@ ClassImp(MBending); + +void MBending::Reset() +{ + fIa = 0; + fIe = 0; + fCa = 0; + fAn = 0; + fAw = 0; + fNrx = 0; + fNry = 0; + fCrx = 0; + fCry = 0; + fNpae = 0; + fEces = 0; + fAces = 0; + fEcec = 0; + fAcec = 0; +} void MBending::Load(const char *name) @@ -141,6 +159,44 @@ p += NPAE; - const AltAz I(fIe*kDeg2Rad, fIa*kDeg2Rad); + const AltAz I(fIe/**kDeg2Rad*/, fIa/**kDeg2Rad*/); p += I; + + return p; +} + +AltAz MBending::CorrectBack(const AltAz &aa) const +{ + // Correct [rad] + // zdaz [rad] + AltAz p = aa; + + const AltAz CES(-fEces*kDeg2Rad*sin(p.Alt()), -fAces*kDeg2Rad*sin(p.Az())); + const AltAz CEC(-fEcec*kDeg2Rad*cos(p.Alt()), -fAcec*kDeg2Rad*cos(p.Az())); + p -= CES; + p -= CEC; + + const AltAz CRX(-fCrx*kDeg2Rad*sin(p.Az()-p.Alt()), fCrx*kDeg2Rad*cos(p.Az()-p.Alt())/cos(p.Alt())); + const AltAz CRY(-fCry*kDeg2Rad*cos(p.Az()-p.Alt()), -fCry*kDeg2Rad*sin(p.Az()-p.Alt())/cos(p.Alt())); + p -= CRX; + p -= CRY; + + const AltAz NRX(fNrx*kDeg2Rad*sin(p.Alt()), -fNrx*kDeg2Rad); + const AltAz NRY(fNry*kDeg2Rad*cos(p.Alt()), -fNry*kDeg2Rad*tan(p.Alt())); + p -= NRX; + p -= NRY; + + const AltAz AW( fAw*kDeg2Rad*sin(p.Az()), -fAw*kDeg2Rad*cos(p.Az())*tan(p.Alt())); + const AltAz AN(-fAn*kDeg2Rad*cos(p.Az()), -fAn*kDeg2Rad*sin(p.Az())*tan(p.Alt())); + p -= AW; + p -= AN; + + const AltAz CA(0, -fCa*kDeg2Rad/cos(p.Alt())); + p -= CA; + + const AltAz NPAE(0, -fNpae*kDeg2Rad*tan(p.Alt())); + p -= NPAE; + + const AltAz I(fIe/**kDeg2Rad*/, fIa/**kDeg2Rad*/); + p -= I; return p; @@ -156,4 +212,13 @@ } +ZdAz MBending::CorrectBack(const ZdAz &zdaz) const +{ + // Correct [rad] + // zdaz [rad] + AltAz p(TMath::Pi()/2-zdaz.Zd(), zdaz.Az()); + AltAz c = CorrectBack(p); + return ZdAz(TMath::Pi()/2-c.Alt(), c.Az()); +} + void MBending::SetParameters(const Double_t *par, Int_t n=14) { Index: trunk/MagicSoft/Cosy/main/MBending.h =================================================================== --- trunk/MagicSoft/Cosy/main/MBending.h (revision 1691) +++ trunk/MagicSoft/Cosy/main/MBending.h (revision 1699) @@ -52,6 +52,11 @@ void Save(const char *name); + void Reset(); + ZdAz Correct(const ZdAz &zdaz) const; AltAz Correct(const AltAz &aaz) const; + + ZdAz CorrectBack(const ZdAz &zdaz) const; + AltAz CorrectBack(const AltAz &aaz) const; ZdAz operator()(const ZdAz &zdaz) const { return Correct(zdaz); } Index: trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1691) +++ trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1699) @@ -378,5 +378,5 @@ // FIXME: Correct by fOffset ? - if (!(fMac1 && fMac2 && fMac3)) + if (!(fMac1 && fMac2)) { cout << "SetPosition: No MACS!" << endl; @@ -395,9 +395,11 @@ // const ZdAz dest = CorrectTarget(src, dst); // - const ZdAz dest = fBending(dst)*16384/2/TMath::Pi(); + const ZdAz bend = fBending(dst); + const ZdAz dest = bend*16384/2/TMath::Pi(); lout << "Positioning to Target..." << endl; - //cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl; - //cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl; + cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl; + cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl; + cout << "Bend'd Dest Zd: " << Rad2SE(bend.Zd()) << "se Az:" << Rad2SE(bend.Az()) << "se" << endl; cout << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; @@ -450,9 +452,10 @@ /* - cout << " + " << cdzd << " " << cdaz << endl; + cout << " + " << (int)cdzd << " " << (int)cdaz << endl; cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl; cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl; cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl; - */ + cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl; + */ // @@ -653,5 +656,5 @@ fRaDec = dst; - if (!(fMac1 && fMac2 && fMac3)) + if (!(fMac1 && fMac2)) return; @@ -855,5 +858,5 @@ fAz->SetPreset(za.Az()); - cout << "WM_Polaris: done. (return 0xaffe)" << endl; + cout << "WM_Calib: done. (return 0xaffe)" << endl; } return (void*)0xaffe; @@ -914,4 +917,10 @@ cout << "WM_LoadBending: done. (return 0xbe0d)" << endl; return (void*)0xbe0d; + + case WM_RESETBENDING: + cout << "WM_ResetBending: START" << endl; + fBending.Reset(); + cout << "WM_ResetBending: done. (return 0xbe0e)" << endl; + return (void*)0xbe0e; case WM_CALCALTAZ: @@ -995,16 +1004,49 @@ resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384); + /* WORKAROUND !!!!!!!!!!!!! FIXME !!!!!!!!!!! */ + resrezd = 500; + resreaz = 500; + kGearRatio.Set (gzd*resrezd/ressezd, gaz*resreaz/resseaz); //[re/se] kGearRatio2.Set(gzd*resrezd/360.0, gaz*resreaz/360.0); //[re/deg] cout << "done." << endl; + + cout << "Setting Gear Ratios:" << endl; + cout << "--------------------" << endl; + cout << " X: " << gzd << "*" << resrezd << "/" << ressezd << "=" << kGearRatio.X() << endl; + cout << " Y: " << gaz << "*" << resreaz << "/" << resseaz << "=" << kGearRatio.Y() << endl; } void MCosy::TalkThread() { - if (!(fMac1 && fMac2 && fMac3)) + if (!(fMac1 && fMac2)) return; fMac1->ReqPos(); fMac2->ReqPos(); + + if (fMac3) + { + const int res = fMac3->GetVelRes(); + + fMac3->SetVelocity(res); + fMac3->SetAcceleration(res); + fMac3->SetDeceleration(res); + fMac3->StartPosSync(); + } + + fMac1->EnableTimeout(kTRUE, 500); + fMac2->EnableTimeout(kTRUE, 500); + + if (!(fZd1 && fZd2 && fAz)) + return; + + /* + // + // Start the Network + // + cout << "Reading configuration file..." << flush; + TEnv env(".cosyrc"); + cout << "done." << endl; const int res = fMac3->GetVelRes(); @@ -1013,26 +1055,4 @@ fMac3->SetAcceleration(res); fMac3->SetDeceleration(res); - - fMac1->EnableTimeout(kTRUE, 500); - fMac2->EnableTimeout(kTRUE, 500); - - if (!(fZd1 && fZd2 && fAz)) - 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); - - fMac3->StartPosSync(); cout << "Going Home..." << endl; @@ -1224,7 +1244,8 @@ if (fAz) fAz->DisplayVal(); - ZdAz ist = GetSePos()*(360.0/16384.0); // [se] - - fWin->Update(ist, fTrackingError/kGearRatio2, + ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se] + ZdAz bendist = fBending.CorrectBack(seist); + + fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError/kGearRatio2, fVelocity, fOffset/*/kGearRatio2*/, fStatus); @@ -1297,5 +1318,10 @@ } -void MCosy::Constructor() +// -------------------------------------------------------------------------- +// +// Disable the synchronization by using a negative CAN Id for id2. +// +void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3, + Int_t id4, Int_t id5, Int_t id6) { // @@ -1304,11 +1330,12 @@ lout << "- Setting up network." << endl; - fMac1=new Macs(1, "Mac.1/Az", lout); - fMac3=new Macs(2, "Mac.3/Az-Sync", lout); - fMac2=new Macs(3, "Mac.2/Zd", lout); - - fZd1=new ShaftEncoder(5, "SE.5/Zd1", lout); - fZd2=new ShaftEncoder(6, "SE.6/Zd2", lout); - fAz =new ShaftEncoder(7, "SE.7/Az", lout); + fMac1=new Macs(id1, "Mac/Az", lout); + fMac2=new Macs(id3, "Mac/Zd", lout); + if (id2>=0) + fMac3=new Macs(id2, "Mac/Az-Sync", lout); + + fZd1=new ShaftEncoder(id4, "SE/Zd1", lout); + fZd2=new ShaftEncoder(id5, "SE/Zd2", lout); + fAz =new ShaftEncoder(id6, "SE/Az", lout); lout << "- Connecting devices to network." << endl; @@ -1319,5 +1346,6 @@ SetNode(fMac1); SetNode(fMac2); - SetNode(fMac3); + if (id2>=0) + SetNode(fMac3); SetNode(fZd1); SetNode(fZd2); @@ -1340,5 +1368,5 @@ } -void MCosy::ConstructorSE() +void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6) { // @@ -1347,7 +1375,7 @@ lout << "- Setting up network." << endl; - fZd1=new ShaftEncoder(5, "SE.5/Zd1", lout); - fZd2=new ShaftEncoder(6, "SE.6/Zd2", lout); - fAz =new ShaftEncoder(7, "SE.7/Az", lout); + fZd1=new ShaftEncoder(id4, "SE/Zd1", lout); + fZd2=new ShaftEncoder(id5, "SE/Zd2", lout); + fAz =new ShaftEncoder(id6, "SE/Az", lout); lout << "- Connecting devices to network." << endl; @@ -1383,25 +1411,4 @@ lout << "- Setting up network." << endl; - /* - fMac1=new Macs(1, "Mac.1/Az", lout); - fMac2=new Macs(2, "Mac.2/Zd", lout); - fMac3=new Macs(3, "Mac.3/Az-Sync", lout); - - fZd1=new ShaftEncoder(4, "SE.4/Zd1", lout); - fZd2=new ShaftEncoder(5, "SE.5/Zd2", lout); - fAz =new ShaftEncoder(6, "SE.6/Az", lout); - - lout << "- Connecting devices to network." << endl; - - // - // Connect the devices to the network - // - SetNode(fMac1); - SetNode(fMac2); - SetNode(fMac3); - SetNode(fZd1); - SetNode(fZd2); - SetNode(fAz); - */ // // Create Gui Event timer and Gui @@ -1412,15 +1419,19 @@ lout << "- Starting GUI." << endl; fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); - /* - fAz->SetDisplay(fWin->GetLabel1()); - fZd1->SetDisplay(fWin->GetLabel2()); - fZd2->SetDisplay(fWin->GetLabel3()); - */ + lout.SetOutputGui(fWin->GetLog(), kTRUE); } MCosy::MCosy(int mode, const char *dev, const int baud, MLog &out) -: Network(dev, baud, out), fTracking(kFALSE) -{ +: Network(dev, baud, out), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fTracking(kFALSE) +{ + TEnv env(".cosyrc"); + const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1 + const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2 + const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3 + const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4 + const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5 + const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6 + lout << "- Program in "; switch (mode) @@ -1428,9 +1439,9 @@ case 0: lout << "<>" << endl; - Constructor(); + Constructor(id1, id2, id3, id4, id5, id6); break; case 1: lout << "<>" << endl; - ConstructorSE(); + ConstructorSE(id4, id5, id6); break; default: Index: trunk/MagicSoft/Cosy/main/MCosy.h =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.h (revision 1691) +++ trunk/MagicSoft/Cosy/main/MCosy.h (revision 1699) @@ -10,14 +10,15 @@ #define kDEG ((char)0x9c) // Linux 'ΓΈ' -#define WM_WAIT WM_NULL -#define WM_PRESET 0x1000 -#define WM_POSITION 0x1001 -#define WM_TRACK 0x1002 -#define WM_STOP 0x1003 -#define WM_CALIB 0x1004 -#define WM_TPOINT 0x1005 -#define WM_NEWTRACK 0x1006 -#define WM_LOADBENDING 0x1007 -#define WM_CALCALTAZ 0x1008 +#define WM_WAIT WM_NULL +#define WM_PRESET 0x1000 +#define WM_POSITION 0x1001 +#define WM_TRACK 0x1002 +#define WM_STOP 0x1003 +#define WM_CALIB 0x1004 +#define WM_TPOINT 0x1005 +#define WM_NEWTRACK 0x1006 +#define WM_LOADBENDING 0x1007 +#define WM_RESETBENDING 0x1008 +#define WM_CALCALTAZ 0x1009 class ShaftEncoder; @@ -125,6 +126,6 @@ void WaitForEndMovement(); - void Constructor(); - void ConstructorSE(); + void Constructor(Int_t id1, Int_t id2, Int_t id3, Int_t id4, Int_t id5, Int_t id6); + void ConstructorSE(Int_t id1, Int_t id2, Int_t id3); void ConstructorDemo();