Index: unk/MagicSoft/Cosy/MCosy.cc =================================================================== --- /trunk/MagicSoft/Cosy/MCosy.cc (revision 1529) +++ (revision ) @@ -1,930 +1,0 @@ -#include "MCosy.h" - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "MGCosy.h" -#include "SlaStars.h" - -#include "slalib/slalib.h" // FIXME: REMOVE - -#include "macs.h" -#include "base/timer.h" -#include "shaftencoder.h" - -//#include // PRIO_PROCESS - -typedef struct tm tm_t; - -#define GEAR_RATIO_ALT 75.55 // 75.25 VERY IMPORTANT! unit=RE/SE -#define GEAR_RATIO_AZ 179.8 // VERY IMPORTANT! unit=RE/SE - -const XY kGearRatio(GEAR_RATIO_ALT, GEAR_RATIO_AZ); -const XY kGearRatio2(GEAR_RATIO_ALT*16384.0/360.0, GEAR_RATIO_AZ*16384.0/360.0); - -double Rad2SE(double rad) -{ - return 16384.0/D2PI*rad; -} - -double Rad2ZdRE(double rad) -{ - return 16384.0/D2PI*rad*kGearRatio.X(); -} - -double Rad2AzRE(double rad) -{ - return 16384.0/D2PI*rad*kGearRatio.Y(); -} - -double Deg2ZdRE(double rad) -{ - return rad*kGearRatio2.X(); -} - -double Deg2AzRE(double rad) -{ - return rad*kGearRatio2.Y(); -} - -ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst) -{ - // CorrectTarget [se] - - // src [se] - // dst [rad] - - // fAltMax = 70 - // fAltMin = -105/110 - // fAzMin = -355 - // fAzMax = 355 - - ZdAz source = src * 360.0/16384.0; - ZdAz dest = dst * 360.0/D2PI; - - if (dest.Zd()>-1e-6 && dest.Zd()<1e-6) - return dst*(16384.0/D2PI); - - const float fZdMin = -67; - const float fZdMax = 67; - const float fAzMin = -29; - const float fAzMax = 423; - - // - // This corrects to target for the shortest distance, not for the fastest move! - // - ZdAz s = source-dest; - - float min = s.Sqr(); - - // - // Is it enought to search inside one revolution? - // - ZdAz ret = dest; - - for (int i=-5; i<5+1; i++) - { - const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180); - - // - // Range Check - // - if (p.Zd()fZdMax) - continue; - - if (p.Az()fAzMax) - continue; - - // - // Calculate distance - // - s = source-p; - - const float dist = s.Sqr(); - - if (dist > min) - continue; - - // - // New shortest distance - // - ret = p; - min = dist; - } - return ret*(16384.0/360.0); -} - - -ZdAz MCosy::GetSePos() -{ - const int p0 = fAlt1->GetPos(); - const int p1 = fAlt2->GetPos(); - const int p2 = fAz->GetPos(); - - const int a0 = p0; //p0>8192?p0-16384:p0; - const int a1 = p1; //p1>8192?p1-16384:p1; - const int a2 = p2; //p2>8192?p2-16384:p2; - - // - // interpolate shaft encoder positions - // - const float a = (float)(a0-a1)/2; - - // - // calculate 'regelabweichung' - // - return ZdAz(a, a2); -} - -ZdAz MCosy::GetRePos() -{ - return ZdAz(fMac2->GetPos(), fMac1->GetPos()); -} - -ZdAz MCosy::GetRePosPdo() -{ - return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos()); -} - -void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc) -{ - // - // Set velocities - // - const int vr = fMac1->GetVelRes(); - - vel *= vr; - acc *= vr; - - if (ratio <1) - { - fMac1->SetVelocity(vel); - fMac1->SetAcceleration(acc); - fMac1->SetDeceleration(acc); - - fMac2->SetVelocity(vel*ratio); - fMac2->SetAcceleration(acc*ratio); - fMac2->SetDeceleration(acc*ratio); - } - else - { - fMac1->SetVelocity(vel/ratio); - fMac1->SetAcceleration(acc/ratio); - fMac1->SetDeceleration(acc/ratio); - - fMac2->SetVelocity(vel); - fMac2->SetAcceleration(acc); - fMac2->SetDeceleration(acc); - } -} - -void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2) -{ - SetStatus(kMoving); - - if (axe1) fMac2->StartRelPos(rd.Zd()); - if (axe2) fMac1->StartRelPos(rd.Az()); - - cout << "Waiting for positioning..." << flush; - - WaitForEndMovement(); - - cout << "done." << endl; -} - -void MCosy::CheckForError() -{ - if (!HasError()) - { - SetStatus(kStopped); - return; - } - - SetStatus(kError); - fMac1->HandleError(); - fMac2->HandleError(); - if (HasError()) - return; - - SetStatus(kStopped); -} - -int MCosy::SetPosition(const ZdAz &dst) // [rad] -{ - // FIXME: Correct by fOffset ? - - // - // Calculate new target position (shortest distance to go) - // - const ZdAz src = GetSePos(); - const ZdAz dest = CorrectTarget(src, dst); - - 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 << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl; - - for (int i=0; i<10 && !StopWaitingForSDO(); i++) - { - // - // Get Shaft Encoder Positions - // - const ZdAz p=GetSePos(); - - // - // calculate control deviation and rounded cd - // - ZdAz rd = dest-p; // [se] - - ZdAz cd = rd; // [se] - cd.Round(); - - // - // Check if there is a control deviation on the axis - // - const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE; - const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE; - - // - // check if we reached the correct position already - // - if (!cdzd && !cdaz) - { - lout << "Positioning done with " << i << "manuvers." << endl; - return TRUE; - } - - // - // change units from se to re - // - rd *= kGearRatio; // [re] - - // - // Initialize Velocities so that we reach both positions - // at the same time - // - if (i) - SetPosVelocity(1.0, 0.1, 0.1); - else - SetPosVelocity(fabs(rd.Ratio()), 0.9, 0.5); - - rd.Round(); - - /* - cout << " + " << cdzd << " " << 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; - */ - - // - // repositioning (relative) - // - DoRelPos(rd, cdzd, cdaz); - } - - StopMovement(); - lout << "Warning: Requested position not reached." << endl; - return FALSE; -} - -Bool_t MCosy::RequestRePos() -{ - - fMac2->RequestSDO(0x6004); - fMac1->RequestSDO(0x6004); - WaitForSdos(); - if (!StopWaitingForSDO()) - return kTRUE; - - if (HasError()) - lout << "Error #6004 (requesting re pos from Macs) happened." << endl; - - return kFALSE; -} - -Bool_t MCosy::SetVelocity(ZdAz v) -{ - fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min] - fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min] - WaitForSdos(); - if (!StopWaitingForSDO()) - return kTRUE; - - if (HasError()) - lout << "Error #3006 (setting velocity of Macs) happened." << endl; - - return kFALSE; -} - -void MCosy::InitTracking() -{ - // - // Start revolution mode - // - fMac2->SetAcceleration(0.90*fMac2->GetVelRes()); - fMac2->SetDeceleration(0.90*fMac2->GetVelRes()); - - fMac1->SetAcceleration(0.90*fMac1->GetVelRes()); - fMac1->SetDeceleration(0.90*fMac1->GetVelRes()); - - SetStatus(kMoving | kTracking); - - fMac2->SetRpmMode(TRUE); - fMac1->SetRpmMode(TRUE); -} - -void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const -{ - // - // How to limit the speed. If the wind comes and blowes - // we cannot forbid changing of the sign. But on the other hand - // we don't want fast changes! - // - - ULong_t vrzd = fMac1->GetVelRes(); - ULong_t vraz = fMac2->GetVelRes(); - -#define sgn(x) (x<0?-1:1) - - const Float_t limit = 0.25; - - if (sgn(vt->Az()) != sgn(vcalc.Az()) && - fabs(vt->Az()) < limit*fabs(vcalc.Az())) - vt->Az(0); - else - if (fabs(vt->Az()) > 0.9*vraz) - vt->Az(0.9*vraz*sgn(vt->Az())); - - if (sgn(vt->Zd()) != sgn(vcalc.Zd()) && - fabs(vt->Zd()) < limit*fabs(vcalc.Zd())) - vt->Zd(0); - else - if (fabs(vt->Zd()) > 0.9*vrzd) - vt->Zd(0.9*vrzd*sgn(vt->Zd())); -} - -void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad] -{ - SlaStars sla; - - // - // Position to actual position - // - sla.SetMjd2Now(); - ZdAz dest = sla.CalcZdAz(dst); - - if (!SetPosition(dest)) - { - lout << "Error: Cannot start tracking, positioning failed." << endl; - return; - } - - // - // calculate offset from present se position - // - const ZdAz sepos = GetSePos()*kGearRatio; - - if (!RequestRePos()) - return; - - // - // Estimate Offset before starting to track - // - fOffset = sepos-GetRePos(); - - /* - cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl; - cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl; - cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl; - */ - - // - // Init accelerations and Rpm Mode - // - InitTracking(); - - lout << "Start tracking:"; - lout << " Ra: " << Rad2Deg(dst.Ra()) << "\xb0 "; - lout << "Dec: " << Rad2Deg(dst.Dec()) << "\xb0" << endl; - - // - // Initialize Tracker (slalib or starguider) - // - fRaDec = dst; - fTracking = kTRUE; - - ofstream fout("cosy.pos"); - fout << "Tracking:"; - fout << " Ra: " << Rad2Deg(dst.Ra()) << "\x9c "; - fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl; - fout << " Mjd/10ms V/re/min/4" << endl; - - // - // We want to reach the theoretical position exactly in about 0.5s - // - const float dt = 1; // 1 second - while (!StopWaitingForSDO()) - { - // - // Request Target position for this moment - // - sla.Now(); - - // - // Request theoretical Position for a time in the future (To+dt) from CPU - // - sla.SetMjd(sla.CalcMjd()+dt/(60*60*24)); - dest = CorrectTarget(GetSePos(), sla.CalcZdAz(dst)); // [se] - - ZdAz vcalc = sla.GetApproxVel(dst) * kGearRatio2*4./60.; // [re/min] - - // - // Request absolute position of rotary encoder from Macs - // - if (!RequestRePos()) - break; - - // - // distance between (To+dt) and To [re] - // position time difference < 5usec - // fOffset does the synchronization between the - // Shaft- and the rotary encoders - // - dest *= kGearRatio; // [re] - dest -= GetRePos() + fOffset; - - // - // Velocity to go [re/min] to reach the right position at time t+dt - // correct for the duration of RaDec2AltAz - // - const ZdAz v = dest*60.0/(dt-(fMac2->GetTime()-sla)); - - // - // calculate real velocity of future [re/min] - // believing the Macs manual '/4' shouldn't be necessary, but it is. - // - ZdAz vt = v/4; - LimitSpeed(&vt, vcalc); - vt.Round(); - - // - // check if the drive is fast enough to follow the star - // - if (vt.Zd()>.9*fMac1->GetVelRes() || vt.Az()>.9*fMac2->GetVelRes()) - { - lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl; - break; - } - - // - // Set theoretical velocity (as early after calculation as possible) - // Maybe we should attenuate the changes - // - if (!SetVelocity(vt)) - break; - - // - // Now do 'unnecessary' things - // - fVelocity = vt/kGearRatio2*4; - - const double mjd = fMac2->GetMjd(); - fout << setprecision(15) << setw(15) << mjd*60.*60.*24. << " "; - fout << setw(4) << vt.Zd() << " "; - fout << setw(4) << vt.Az() << endl; - // - // FIXME? Calculate an accuracy for the tracking system? - // How good do we reach the calculated position in 'real' - // re valus? - // - - - // - // Update speed as often as possible. - // make sure, that dt is around 10 times larger than the - // update time - // - // usleep(50000); // 0.05s - } - - fTracking = kFALSE; - StopMovement(); - lout << "Tracking stopped." << endl; -} - -int MCosy::IsPositioning() const -{ - return (fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO(); -} - -void MCosy::WaitForEndMovement() -{ - WaitForSdos(); - - while (IsPositioning()) - usleep(1); -} - -void MCosy::StopMovement() -{ - SetStatus(kStopping); - - cout << "Stopping positioning..." << endl; - fMac1->SetDeceleration(0.5*fMac1->GetVelRes()); - fMac2->SetDeceleration(0.5*fMac2->GetVelRes()); - - cout << "Stoping possible RPM mode..." << endl; - fMac1->SetRpmMode(FALSE); - fMac2->SetRpmMode(FALSE); - - cout << "Waiting for silence..." << endl; - WaitForEndMovement(); - - CheckForError(); - - cout << "Movement stopped." << endl; -} - -void *MCosy::Proc(int msg, void *mp) -{ - switch (msg) - { - case WM_WAIT: - cout << "Wait for execution of Proc(WM_*, ): done." << endl; - return NULL; - - case WM_STOP: - StopMovement(); - return NULL; - - case WM_PRESET: - cout << "WM_Preset: start." << endl; - fAlt1->SetPreset(); - fAlt2->SetPreset(); - fAz->SetPreset(); - cout << "WM_Preset: done. (return 0xaffe)" << endl; - return (void*)0xaffe; - - case WM_POLARIS: - { - cout << "WM_Polaris: start." << endl; - SlaStars sla; - sla.SetMjd2Now(); - - RaDec rd(37.94, 89.2644); - ZdAz za=sla.CalcZdAz(rd*D2PI/360.0)*16384.0/D2PI; - - cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl; - - ZdAz sepos = GetSePos(); - cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl; - - fAlt1->SetPreset(za.Zd()); - fAlt2->SetPreset(-za.Zd()); - fAz->SetPreset(za.Az()); - - cout << "WM_Polaris: done. (return 0xaffe)" << endl; - } - return (void*)0xaffe; - - case WM_POSITION: - cout << "WM_Position: start." << endl; - { - ZdAz dest = *((ZdAz*)mp); - - SetPosition(dest*D2PI/360.0); - } - cout << "WM_Position: done. (return 0x7777)" << endl; - return (void*)0x7777; - - case WM_TRACK: - cout << "WM_Track: START" << endl; - { - RaDec dest = *((RaDec*)mp); - TrackPosition(dest*D2PI/360.0); - } - cout << "WM_Track: done. (return 0x8888)" << endl; - return (void*)0x8888; - - case WM_QUIT: - cout << "WM_Quit: now." << endl; - TerminateApp(); - cout << "WM_Quit: done." << endl; - return (void*)0x9999; - } - cout << "Unknown Msg" << endl; - return (void*)0xffffffff; -} - -void *MTTalk::Thread() -{ - fCosy->TalkThread(); - return NULL; -} - -void MCosy::TalkThread() -{ - // - // 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; - fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100)); - fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100)); - PostMsg(WM_PRESET, 0, 0); - PostMsg(WM_WAIT, 0, 0); - - 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 < " - << Deg2AzRE(env.GetValue("MaxAz[Deg]", +1.0)) << "RE" << endl; - cout << env.GetValue("MinAz[Deg]", -1.0) << " < Az < " - << env.GetValue("MaxAz[Deg]", +1.0) << kDEG << endl; - cout << Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)) << "RE < Zd < " - << Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)) << "RE" << endl; - */ - - 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; - -/* - fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0))); - fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0))); -*/ -// fMac3->StartVelSync(); -/* - cout << "PostMsg(WM_PRESET)" << endl; - void *rc = - cout << hex << "WM_PRESET: ret=" << rc << endl; - - RaDec dest = RaDec(45.0, 30.0)*D2PI/360.0; - - cout << "PostMsg(WM_TRACK)" << endl; - cout << sizeof(RaDec) << "==" << sizeof(dest) << endl; - rc=PostMsg(WM_TRACK, &dest, sizeof(dest)); - cout << "DEST killed." << endl; -*/ - // AltAz dest = AltAz(45.0, 30.0); - // double ra, dec; - // slaDaf2r( 71, 0, 0, &ra, &status); // 0 WARNING: RANGE - // slaDaf2r( 89, 0, 0, &dec, &status); // 49 - // cout << "Start tracking: Ra: " << Rad2Deg(ra) << kDEG << " Dec: " << Rad2Deg(dec) << kDEG << endl; - - // dest = AltAz(-46.0, 210); - // SetPosition(dest); - - SlaStars sla; - while (1) - { - // - // wait until a tracking session is started - // - while (!fTracking && !fTTalk->HasStopFlag()) - usleep(1); - - if (fTTalk->HasStopFlag()) - break; - - ofstream fout("cosy.err"); - fout << "Tracking:"; - fout << " Ra: " << Rad2Deg(fRaDec.Ra()) << "\x9c "; - fout << "Dec: " << Rad2Deg(fRaDec.Dec()) << "\x9c" << endl << endl; - fout << " MjdZd/10ms ErrZd/re"; - fout << " MjdAz/10ms ErrAd/re" << endl; - - ZdAz old; - ZdAz ist; - - ZdAz sollzd; - ZdAz sollaz; - - ZdAz istre = -fOffset; // [re] - ZdAz time; - - // - // only update fTrackingError while tracking - // - while (fTracking && !fTTalk->HasStopFlag()) - { - // - // Make changes (eg wind) smoother - attenuation of control function - // - const float weight = 0.3; - - ZdAz offset(fOffset.Zd()*(1.-weight)+(ist.Zd()*kGearRatio.X()-istre.Zd())*weight, - fOffset.Az()*(1.-weight)+(ist.Az()*kGearRatio.Y()-istre.Az())*weight); - - fOffset = offset; - - // - // This is the time constant which defines how fast - // you correct for external influences (like wind) - // - usleep(100000); // 0.1s - - // - // get position, where we are - // - old = ist; - ist = GetSePos(); // [se] - - // - // if the position didn't change continue - // - if ((int)ist.Zd() == (int)old.Zd() && - (int)ist.Az() == (int)old.Az()) - continue; - - istre = GetRePosPdo(); - - // - // Get time from last shaftencoder position change (position: ist) - // FIXME: I cannot take the avarage - // - time.Zd((fAlt1->GetMjd()+fAlt2->GetMjd())/2.0); - time.Az(fAz->GetMjd()); - - // - // if Shaftencoder changed position - // calculate were we should be - // - if ((int)ist.Zd() != (int)old.Zd()) - { - sla.SetMjd(time.Zd()); - sollzd = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se] - } - - if ((int)ist.Az() != (int)old.Az()) - { - sla.SetMjd(time.Az()); - sollaz = CorrectTarget(ist, sla.CalcZdAz(fRaDec)); // [se] - } - - fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(), - (ist.Az()-sollaz.Az())*kGearRatio.Y()); - - fout << setprecision(15) << setw(15) << time.Zd()*60.*60.*24. << " "; - fout << setprecision(5) << setw(5) << fTrackingError.Zd() << " "; - fout << setprecision(15) << setw(15) << time.Az()*60.*60.*24. << " "; - fout << setprecision(5) << setw(5) << fTrackingError.Az() << endl; - } - - fout << endl << endl; - } -} - -Bool_t MCosy::HandleTimer(TTimer *t) -{ - // - // Update Gui, foremer MTGui. - // - fAlt1->DisplayVal(); - fAlt2->DisplayVal(); - fAz->DisplayVal(); - - ZdAz ist = GetSePos()*(360.0/16384.0); // [se] - - fWin->Update(ist, fTrackingError/kGearRatio2, - fVelocity, fOffset/*/kGearRatio2*/, - fStatus); - - return kTRUE; -} - - -int MCosy::StopWaitingForSDO() const -{ - return Break() || HasError(); -} - -void MCosy::Start() -{ - // Don't call this function twice! - Network::Start(); - - lout << "- Starting TX Thread." << endl; - fTTalk = new MTTalk(this); - - lout << "- Starting GUI update." << endl; - fUpdateGui->TurnOn(); - // fTGui = new MTGui(this); -} - -void MCosy::Stop() -{ - - lout << "- Stopping GUI update." << endl; - fUpdateGui->TurnOff(); - lout << "- GUI Update stopped." << endl; - - delete fTTalk; - lout << "- TX Thread stopped." << endl; - - Network::Stop(); -} - -MCosy::MCosy(const char *dev, const int baud, MLog &out) -: Network(dev, baud, out), fTracking(kFALSE) -{ - // - // Create Nodes - // - fMac1=new Macs(1, lout); - fMac2=new Macs(2, lout); - fMac3=new Macs(3, lout); - fAlt1=new ShaftEncoder(4, lout); - fAlt2=new ShaftEncoder(5, lout); - fAz =new ShaftEncoder(6, lout); - - // - // Connect the devices to the network - // - SetNode(fMac1); - SetNode(fMac2); - SetNode(fMac3); - SetNode(fAlt1); - SetNode(fAlt2); - SetNode(fAz); - - // - // Create Gui Event timer and Gui - // - fUpdateGui = new TTimer(this, 100); // 100ms - - fWin=new MGCosy(this, gClient->GetRoot(), 1, 1); - - fAz->SetDisplay(fWin->GetLabel1()); - fAlt1->SetDisplay(fWin->GetLabel2()); - fAlt2->SetDisplay(fWin->GetLabel3()); - - lout.SetOutputGui(fWin->GetLog(), kTRUE); -} - -void MCosy::TerminateApp() -{ - cout << "MCosy::TerminateApp()" << endl; -/* - Int_t rc; - TGMessageBox msg(this, gClient->GetRoot(), - "Information", - "Cosy is shutting down the system - this may take wa while!", - kMBIconExclamation, - kMBOK, //kMBClose - &rc, 0); -*/ - - gApplication->Terminate(0); -} - -MCosy::~MCosy() -{ - cout << "Deleting GUI timer." << endl; - - delete fUpdateGui; - - cout << "Deleting Nodes." << endl; - - delete fAz; - delete fAlt2; - delete fAlt1; - delete fMac1; - delete fMac2; - delete fMac3; - - cout << "Deleting MGCosy." << endl; - - lout.DisableOutputDevice(MLog::eGui); - - delete fWin; - - cout << "MGCosy destructed." << endl; -} Index: unk/MagicSoft/Cosy/MCosy.h =================================================================== --- /trunk/MagicSoft/Cosy/MCosy.h (revision 1529) +++ (revision ) @@ -1,123 +1,0 @@ -#ifndef MCOSY_H -#define MCOSY_H - -#include "coord.h" -#include "msgqueue.h" -#include "network.h" -#include "MThread.h" - -#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_POLARIS 0x1004 -#define WM_QUIT 0x1005 - -class ShaftEncoder; -class Macs; -class MGCosy; -class MCosy; - -class MTTalk : public MThread -{ -private: - MCosy *fCosy; - - void *Thread(); - -public: - MTTalk(MCosy *cosy) : MThread(false), fCosy(cosy) - { - SetPriority(10); - Start(); - } -}; - -#define kError 0x01 -#define kMoving 0x02 -#define kTracking 0x04 -#define kStopping 0x08 -#define kStopped 0x10 - -class TTimer; -class MCosy : public Network, public MsgQueue, public TObject -{ - friend class MTGui; - friend class MTTalk; - -private: - ShaftEncoder *fAlt1; - ShaftEncoder *fAlt2; - ShaftEncoder *fAz; - - Macs *fMac1; - Macs *fMac2; - Macs *fMac3; - - MGCosy *fWin; - - TTimer *fUpdateGui; - MTTalk *fTTalk; // should be outsourced, like the starguider. - // with a generic interface to both... - - ZdAz fTrackingError; // Tracking Offset between SE and calc-pos [re] - ZdAz fOffset; // Offset between se and re coordinate system [re] - RaDec fRaDec; // Position to track - int fTracking; // Flag for present tracking action - ZdAz fAccuracy; // Actual accuracy of Tracking - ZdAz fVelocity; // Actual velocity of Tracking - - UInt_t fStatus; - - void SetStatus(UInt_t stat) { fStatus = stat; } - UInt_t GetStatus() const { return fStatus; } - - ZdAz GetRePos(); - ZdAz GetRePosPdo(); - ZdAz GetSePos(); // [se] - - Bool_t RequestRePos(); - Bool_t SetVelocity(ZdAz v); - void SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc); - - void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2); - - void InitTracking(); - void LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const; - - void TalkThread(); - void GuiThread(MTGui *t); - - int SetPosition(const ZdAz &dst); - - void TerminateApp(); - - void TrackPosition(const RaDec &dst); // ra, dec [rad] - - int StopWaitingForSDO() const; - int IsPositioning() const; - void CheckForError(); - - void StopMovement(); - void WaitForEndMovement(); - -public: - MCosy(const char *dev, const int baud, MLog &out=gLog); - ~MCosy(); - - void Start(); - void Stop(); - - void *Proc(int msg, void *mp); - - Bool_t HandleTimer(TTimer *t); - - static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst); -// static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0)); - -}; - -#endif