Index: trunk/MagicSoft/Cosy/Changelog =================================================================== --- trunk/MagicSoft/Cosy/Changelog (revision 1816) +++ trunk/MagicSoft/Cosy/Changelog (revision 1817) @@ -5,4 +5,23 @@ - changed output file names to contain mjd - fixed a cast warning + + * main/MCosy.[h,cc]: + - Do not change acceleration and deceleration for positioning + (SetPosVelocity) + - some output if WintForEnDMovement aborted + - some output to correct for the elevation gear asymmetry + - changed handling of acceleration and velocity + - changed velocities + - call StopMovement only if positioning failed + - changed acceleration/deceleration from 0.9 to 0.2 in + InitTracking + - added some workarounds to be able to reach 290deg + - changed control time in tracking from 1 to 3 sec + - added a flag to SetPosition indicating tracking mode + (not yet used) + - added some workarounds to be able to track 290deg + - changed deceleration from 0.5 to 0.3 in StopMovement + - added StopTracking (not yet used) + - changed tpoint output (+mjd, +ra/dec) Index: trunk/MagicSoft/Cosy/main/MCosy.cc =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1816) +++ trunk/MagicSoft/Cosy/main/MCosy.cc (revision 1817) @@ -248,4 +248,6 @@ } + + // -------------------------------------------------------------------------- // @@ -258,5 +260,5 @@ // target position at the same time. // -void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc) +void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel) { // @@ -266,5 +268,4 @@ vel *= vr; - acc *= vr; if (ratio <1) @@ -272,10 +273,4 @@ fMac1->SetVelocity(vel); fMac2->SetVelocity(vel*ratio); - - fMac1->SetAcceleration(acc); - fMac2->SetAcceleration(acc*ratio); - - fMac1->SetDeceleration(acc); - fMac2->SetDeceleration(acc*ratio); } else @@ -283,10 +278,4 @@ fMac1->SetVelocity(vel/ratio); fMac2->SetVelocity(vel); - - fMac1->SetAcceleration(acc/ratio); - fMac2->SetAcceleration(acc); - - fMac1->SetDeceleration(acc/ratio); - fMac2->SetDeceleration(acc); } } @@ -305,4 +294,5 @@ if (HasZombie()) return; + SetStatus(MCosy::kMoving); @@ -342,4 +332,7 @@ !(Break() || HasError()) && !HasZombie()) usleep(1); + + if (Break() || HasError() || HasZombie()) + lout << "WaitForEndMovement aborted..." << endl; } @@ -400,7 +393,19 @@ // As a reference the shaftencoder values are used. // -int MCosy::SetPosition(const ZdAz &dst) // [rad] +int MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad] { // FIXME: Correct by fOffset ? +/* + ZdAz + + if (dst.Az()<-TMath::Pi()/2) + dst.Az(dst.Az() + TMath::Pi()*2); + + if (dst.Az()>3*TMath::Pi()/2) + dst.Az(dst.Az() -TMath::Pi()*2); +*/ + // track=kFALSE; + + // const ZdAz d1 = track ? CorrectTarget(src, dst) : dst*16384/TMath::Pi()/2; // [se] const ZdAz d = dst*kRad2Deg; @@ -422,4 +427,8 @@ } + // + // Calculate new target position (shortest distance to go) + // + const ZdAz src = GetSePos(); // [se] /* @@ -439,8 +448,8 @@ // return false; - // - // Calculate new target position (shortest distance to go) - // - const ZdAz src = GetSePos(); // [se] + // ==================================== + RequestRePos(); + ZdAz before = GetRePos(); + // ==================================== // @@ -458,4 +467,11 @@ cout << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; + // + // Set velocities + // + const int vr = fMac1->GetVelRes(); + + cout << "Mac1: Velres=" << dec << vr << endl; + int i; for (i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++) @@ -472,8 +488,39 @@ // ZdAz rd = dest-p; // [se] +/* + if (rd.Az()>0 && rd.Az()<0.5) + rd.Az(1); + if (rd.Az()<0 && rd.Az()>-0.5) + rd.Az(-1); +*/ + cout << "Deviation: " << rd.Zd() << "se Az:" << rd.Az() << "se" << endl; + + + // =========================================== + Float_t k1 = dest.Zd()*360/16384 - 14.3979; //[deg] + Float_t k0 = p.Zd()*360/16384 - 14.3979; //[deg] + + // + // -28re/se*cos((x/deg-14.4)*pi/180) + 443.2re/se x + // 28re/se*2*pi/360*sin((x/deg-14.4)*pi/180)*deg + 443.2re/se*x/deg + // + + Float_t dk = 443.152*(dest.Zd()-p.Zd()); + Float_t dx = 28.0148/2/TMath::Pi()*(sin(k1/kRad2Deg)-sin(k0/kRad2Deg)); + + cout << "Using: " << rd.Zd()*kGearRatio.X() << endl; // [re] + cout << "Estimated1: " << 16384*dx + dk << endl; + cout << "Estimated2: " << (16384*dx + dk)/1.085949688 << endl; + // =========================================== + ZdAz cd = rd; // [se] cd.Round(); - +/* + if (rd.Az()>0 && rd.Az()<0.5) + rd.Az(0.5); + if (rd.Az()<0 && rd.Az()>-0.5) + rd.Az(-0.5); +*/ // // Check if there is a control deviation on the axis @@ -489,4 +536,13 @@ lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl; SetStatus(MCosy::kStopped); + + // ==================================== + RequestRePos(); + ZdAz after = GetRePos(); + // ==================================== + + after -= before; + + cout << "Drove: " << after.Zd() << " " << after.Az() << endl; return TRUE; } @@ -501,10 +557,36 @@ // at the same time // - lout << "SetVelocity" << endl; if (i) - SetPosVelocity(1.0, 0.05, 0.1); - else // vel(0.6) acc(0.5) - SetPosVelocity(fabs(rd.Ratio()), 0.15, 0.4); -// SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3); + { + fMac1->SetAcceleration(0.1*vr); + fMac2->SetAcceleration(0.1*vr); + + fMac1->SetDeceleration(0.1*vr); + fMac2->SetDeceleration(0.1*vr); + + SetPosVelocity(1.0, 0.05); + } + else + { + if (rd.Az()>-15*kGearRatio.Y() && rd.Az()<15*kGearRatio.Y()) + { + cout << " -------------- LO ---------------- " << endl; + fMac1->SetAcceleration(0.05*vr); + fMac1->SetDeceleration(0.05*vr); + } + else + { + cout << " -------------- HI ---------------- " << endl; + fMac1->SetAcceleration(0.2*vr); + fMac1->SetDeceleration(0.1*vr); + } + + fMac2->SetAcceleration(0.2*vr); + fMac2->SetDeceleration(0.1*vr); + + // vel(0.6) acc(0.5) + SetPosVelocity(fabs(rd.Ratio()), 0.175); + // SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3); + } rd.Round(); @@ -529,5 +611,7 @@ } - StopMovement(); + if (i<10) + StopMovement(); + lout << "Warning: Requested position not reached (i=" << i << ")" << endl; return FALSE; @@ -591,11 +675,11 @@ // Start revolution mode // - fMac2->SetAcceleration(0.90*fMac2->GetVelRes()); - fMac2->SetDeceleration(0.90*fMac2->GetVelRes()); + fMac2->SetAcceleration(0.2*fMac2->GetVelRes()); + fMac2->SetDeceleration(0.2*fMac2->GetVelRes()); if (fMac2->IsZombieNode()) return false; - fMac1->SetAcceleration(0.90*fMac1->GetVelRes()); - fMac1->SetDeceleration(0.90*fMac1->GetVelRes()); + fMac1->SetAcceleration(0.2*fMac1->GetVelRes()); + fMac1->SetDeceleration(0.2*fMac1->GetVelRes()); if (fMac1->IsZombieNode()) return false; @@ -695,5 +779,11 @@ ZdAz dest = sla.CalcZdAz(dst); - if (!SetPosition(dest)) + if (dest.Az()<-TMath::Pi()/2) + dest.Az(dest.Az() + TMath::Pi()*2); + + if (dest.Az()>3*TMath::Pi()/2) + dest.Az(dest.Az() -TMath::Pi()*2); + + if (!SetPosition(dest, kTRUE)) { lout << "Error: Cannot start tracking, positioning failed." << endl; @@ -751,5 +841,5 @@ // // *OLD*const float dt = 1; // 1 second - const float dt = 3; // 1 second + const float dt = 3; // 2 second while (!(Break() || HasError()) && !HasZombie()) { @@ -763,5 +853,12 @@ // sla.SetMjd(sla.GetMjd()+dt/(60*60*24)); - ZdAz dummy = fBending(sla.CalcZdAz(fRaDec)); + ZdAz dummy1 = sla.CalcZdAz(fRaDec); + + if (dummy1.Az()<-TMath::Pi()/2) + dummy1.Az(dummy1.Az() + TMath::Pi()*2); + if (dummy1.Az()>3*TMath::Pi()/2) + dummy1.Az(dummy1.Az() -TMath::Pi()*2); + + ZdAz dummy = fBending(dummy1); dest = CorrectTarget(GetSePos(), dummy); // [se] @@ -772,4 +869,5 @@ ZdAz max = fBending(fMax/kRad2Deg)*kRad2Deg; + /* if (d.Zd()SetDeceleration(0.5*fMac1->GetVelRes()); - fMac2->SetDeceleration(0.5*fMac2->GetVelRes()); - + cout << "Stopping movement (dec=20%)..." << endl; + + fMac1->SetDeceleration(0.3*fMac1->GetVelRes()); + fMac2->SetDeceleration(0.3*fMac2->GetVelRes()); fMac1->SetRpmMode(FALSE); fMac2->SetRpmMode(FALSE); +/* + fMac1->SetDeceleration(0.3*fMac1->GetVelRes()); + fMac2->SetDeceleration(0.3*fMac2->GetVelRes()); + + fMac2->SendSDO(0x3000, Macs::string('s','t','o','p')); + fMac1->SendSDO(0x3000, Macs::string('s','t','o','p')); + fMac2->WaitForSdo(0x3000, 0); + fMac1->WaitForSdo(0x3000, 0); + fMac1->SetRpmMode(FALSE); + fMac2->SetRpmMode(FALSE); + */ + // // Wait for the movement to really be finished. // - cout << "Waiting for silence..." << endl; + cout << "Waiting for end of movement..." << endl; WaitForEndMovement(); + + // + // Check whether everything works fine. + // + CheckForError(); + cout << "Movement stopped." << endl; +} + +void MCosy::StopTracking() +{ + // + // Set status to Stopping + // + SetStatus(MCosy::kStopping); + + // + // set deceleration to 50% + // + cout << "Stopping tracking (dec=20%)..." << endl; + fMac1->SetDeceleration(0.2*fMac1->GetVelRes()); + fMac2->SetDeceleration(0.2*fMac2->GetVelRes()); + + fMac2->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min] + fMac1->SendSDO(0x3006, 1, (LWORD_t)0); // SetRpmVelocity [re/min] + fMac2->WaitForSdo(0x3006, 1); + fMac1->WaitForSdo(0x3006, 1); + + cout << "Waiting for end of movement..." << endl; + WaitForEndMovement(); + + // + // Wait for the objects to be OKed. + // + fMac1->SetRpmMode(FALSE); + fMac2->SetRpmMode(FALSE); + + // + // Wait for the movement to really be finished. + // + //cout << "Waiting for end of movement..." << endl; + //WaitForEndMovement(); // @@ -998,5 +1149,5 @@ cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl; - *tpout << za.Az() << " " << za.Alt() << " "; + *tpout << setprecision(7) << za.Az() << " " << za.Alt() << " "; ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;; @@ -1005,5 +1156,6 @@ cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl; - *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << endl; + *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << " "; + *tpout << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl; cout << "WM_TPoint: done. (return 0xaffe)" << endl; @@ -1155,6 +1307,6 @@ cout << "Reading telescope range..." << flush; - const Double_t amin = env.GetValue("Az_Min[deg]", -74.5); - const Double_t zmin = env.GetValue("Zd_Min[deg]", -90.0); + const Double_t amin = env.GetValue("Az_Min[deg]", -90.0); + const Double_t zmin = env.GetValue("Zd_Min[deg]", -74.5); fMin.Set(zmin, amin); @@ -1221,7 +1373,7 @@ const int res = fMac3->GetVelRes(); - fMac3->SetVelocity(res); - fMac3->SetAcceleration(res); - fMac3->SetDeceleration(res); + fMac3->SetVelocity(0.3*res); + fMac3->SetAcceleration(0.2*res); + fMac3->SetDeceleration(0.2*res); fMac3->StartPosSync(); } @@ -1857,5 +2009,5 @@ while (1) { - sprintf(name, "/home/tbretz/TPoint/tpoint%03d.txt", i++); + sprintf(name, "tpoint/tpoint%03d.txt", i++); if (gSystem->AccessPathName(name, kFileExists)) break; @@ -1864,4 +2016,6 @@ Timer time; time.Now(); + + cout << "TPoint File ********* " << name << " ********** " << endl; tpout = new ofstream(name); Index: trunk/MagicSoft/Cosy/main/MCosy.h =================================================================== --- trunk/MagicSoft/Cosy/main/MCosy.h (revision 1816) +++ trunk/MagicSoft/Cosy/main/MCosy.h (revision 1817) @@ -29,5 +29,4 @@ #define WM_GEAR 0x100c #define WM_DISPLAY 0x100d - class ShaftEncoder; @@ -133,5 +132,6 @@ Bool_t RequestRePos(); Bool_t SetVelocity(const ZdAz &v); - void SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc); + + void SetPosVelocity(const Float_t ratio, Float_t vel); void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2); @@ -149,5 +149,5 @@ void DisplayHistGear(Bool_t del=kTRUE); - int SetPosition(const ZdAz &dst); + int SetPosition(const ZdAz &dst, Bool_t track=kFALSE); void TerminateApp(); @@ -159,4 +159,6 @@ void StopMovement(); + void StopTracking(); + void WaitForEndMovement();