Ignore:
Timestamp:
08/26/08 12:11:32 (16 years ago)
Author:
tbretz
Message:
*** empty log message ***
Location:
trunk/MagicSoft/Cosy/main
Files:
7 edited

Legend:

Unmodified
Added
Removed

  • trunk/MagicSoft/Cosy/main/MCosy.cc

    r8875 r9132  
    77#include <TTimer.h>
    88#include <TApplication.h>
    9 
    10 #include <TH2.h>
    11 #include <TH3.h>
    12 #include <TProfile.h>
    13 #include <TCanvas.h>
    149
    1510//#include "MLog.h"
    … …  
    2621#include "MTracking.h"
    2722
    28 #include "macs.h"
    29 #include "shaftencoder.h"
     23#include "dkc.h"
    3024
    3125ClassImp(MCosy);
    … …  
    3428
    3529typedef struct tm tm_t;
    36 
    37 /* +===================================+
    38     FIXME: What if fMac3 (Sync) died?
    39    +===================================+
    40 */
    4130
    4231//#define EXPERT
    … …  
    114103}
    115104*/
    116 
    117 MTTalk::~MTTalk()
    118 {
    119     gLog << inf2 << "~MTTalk::CancelThread" << std::endl;
    120     CancelThread();
    121     gLog << inf2 << "~MTTalk::MTTalk" << std::endl;
    122 }
    123 
    124105// --------------------------------------------------------------------------
    125106//
    … …  
    136117ZdAz MCosy::GetSePos() const
    137118{
    138     const int pa = fAz->GetPos();
    139     if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
    140         return ZdAz(0, pa);
    141 
    142     //
    143     // Get the values          (FIXME!)
    144     //
    145     //int p1 =  (fZd1->GetPos()+fZd1->GetPhysRes()/2)%fZd1->GetPhysRes();
    146     //int p2 = -(fZd2->GetPos()+fZd2->GetPhysRes()/2)%fZd2->GetPhysRes();
    147 
    148     int p1 =  fZd1->GetPos();//+fZd1->GetPhysRes()/2)%fZd1->GetPhysRes();
    149     int p2 = -fZd2->GetPos();//+fZd2->GetPhysRes()/2)%fZd2->GetPhysRes();
    150 
    151     if (fZd1->IsZombieNode())
    152         return ZdAz(p2, pa);
    153     if (fZd2->IsZombieNode())
    154         return ZdAz(p1, pa);
    155 
    156     //
    157     // interpolate shaft encoder positions
    158     //
    159     float p = (float)(p1+p2)/2;
    160 
    161     return ZdAz(p, pa);
    162 }
    163 
    164 // --------------------------------------------------------------------------
    165 //
    166 //  reads the Rotary encoder positions from the last request of the Macs.
    167 //
    168 //  The positions are returned as a ZdAz object. Use RequestRePos to request
    169 //  the current positions first.
    170 //
    171 ZdAz MCosy::GetRePos()
    172 {
    173     return ZdAz(fMac2->GetPos(), fMac1->GetPos());
    174 }
    175 
    176 // --------------------------------------------------------------------------
    177 //
    178 //  reads the Rotary encoder positions from the Macs.
    179 //
    180 //  The positions are returned as a ZdAz object. The positions are the ones
    181 //  which are send as PDOs to the computer. This is done at a given
    182 //  frequency. Which means, that this positions are not ought to be
    183 //  up-to-date.
    184 //
    185 ZdAz MCosy::GetRePosPdo()
    186 {
    187     return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
     119    const Double_t pa = fMac1 ? (Double_t)fMac1->GetPdoPos2()/fMac1->GetPosRes()  : 0;
     120    const Double_t p1 = fMac2 ? (Double_t)fMac2->GetPdoPos2()/fMac2->GetPosRes() : 0;
     121
     122    return ZdAz(p1, pa);
    188123}
    189124
    … …  
    323258    }
    324259
    325     const ZdAz se = GetSePos()*TMath::TwoPi()/kResSE;   // [rad]
     260    const ZdAz se = GetSePos()*TMath::TwoPi();   // [rad]
    326261    const ZdAz unbendedse = fBending.CorrectBack(se)*TMath::RadToDeg(); // ist pointing
    327262
    … …  
    350285}
    351286
     287/*
    352288Double_t MCosy::Starguider(Double_t mjd, ZdAz &dest) const
    353289{
    … …  
    365301
    366302    ZdAz point=AlignTrackingPos(ZdAz(zd, az)/TMath::RadToDeg());
    367     /*
    368     if (!AlignTrackingPos(ZdAz(zd, az), point))
    369     {
    370         cout << "Starguider position couldn't be aligned..." << endl;
    371         return -1;
    372     }*/
    373 
    374     // FIXME: Check Range missing!
    375303
    376304    const ZdAz diff = (dest-point)*TMath::RadToDeg();
    … …  
    389317    return (mjd-mjd0) * (24*60*60); // [s]
    390318}
     319*/
    391320
    392321// --------------------------------------------------------------------------
    … …  
    405334    MSlewing point(this);
    406335
    407 //#ifdef EXPERT
    408 //    point.SetPointAccDec(0.25, 0.25);
    409 //    point.SetPointVelocity(0.4);
    410 //#else
    411 
    412 // original settings
    413 //
    414     point.SetPointAccDec(0.2, 0.1);
    415     point.SetPointVelocity(0.1);
    416 
    417 
    418 //#endif
    419 
    420 // original
     336    // Default: point.SetPointAcc(0.03, 0.01);
     337    point.SetPointAcc(0.03, 0.01);
     338    point.SetPointVelocity(0.3);
     339
    421340    return point.SetPosition(dst, track);
    422 
    423 // test
    424 //    return point.SetPosition(dst, kTRUE);
    425 }
    426 
    427 void MCosy::SetTrackingPosRE(ZdAz za)
    428 {
    429     za /= kGearTot;                        // [U_tel]
    430     za *= TMath::TwoPi();                  // [rad]
    431     //cout << "RE1:  " << za.Zd()*180/3.1415 << " " << za.Az()*180/3.1415 << endl;
    432     fTrackingPosRaw = za*TMath::RadToDeg();
    433     fTrackingPos    = fBending.CorrectBack(za)*TMath::RadToDeg();
    434     //cout << "RE2:  " << fTrackingPos.Zd() << " " << fTrackingPos.Az() << endl;
    435341}
    436342
    … …  
    439345    MTracking track(this);
    440346    track.SetOut(fOutRep);
    441 //#ifdef EXPERT
    442 //    track.SetPointAccDec(0.4, 0.4);
    443 //    track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
    444 //#else
    445 
    446 
    447 
    448     track.SetPointAccDec(0.2, 0.1);
    449     track.SetPointVelocity(0.1);
    450 
    451     // track.SetPointAccDec(0.4, 0.4);
    452     // track.SetPointVelocity(0.4);
    453 
    454 
    455 
    456 //#endif
    457     track.SetTrackAccDec(0.1, 0.1);
     347
     348    track.SetPointAcc(0.03, 0.01);
     349    track.SetPointVelocity(0.3);
     350    track.SetTrackAcc(0.01, 0.01);
    458351
    459352    track.TrackPosition(dst);
    … …  
    462355void MCosy::TrackPositionGRB(const RaDec &dst) // ra, dec [rad]
    463356{
     357    TrackPosition(dst);
     358    return;
     359
    464360    MTracking track(this);
    465361    track.SetOut(fOutRep);
    466 //#ifdef EXPERT
    467 //    track.SetPointAccDec(0.4, 0.4);
    468 //    track.SetPointVelocity(0.2); // fast: 0.6, slow: 0.2
    469 //#else
    470     track.SetPointAccDec(0.4, 0.4);
    471     track.SetPointVelocity(0.3);
    472 //#endif
    473     track.SetTrackAccDec(0.1, 0.1);
     362    track.SetPointAcc(0.09, 0.03);
     363    track.SetPointVelocity(1.0);
     364    track.SetTrackAcc(0.01, 0.01);
    474365
    475366    track.TrackPosition(dst);
    … …  
    493384    // set deceleration to 50%
    494385    //
    495     gLog << inf2 << "Stopping movement (dec=30%)..." << endl;
     386    gLog << inf2 << "Stopping movement..." << endl;
    496387    if (fMac1 && fMac2)
    497388    {
    498 #ifdef EXPERT
    499         fMac1->SetDeceleration(TMath::Nint(0.5*fMac1->GetVelRes()));
    500         fMac2->SetDeceleration(TMath::Nint(0.5*fMac2->GetVelRes()));
    501 #else
    502         fMac1->SetDeceleration(TMath::Nint(0.3*fMac1->GetVelRes()));
    503         fMac2->SetDeceleration(TMath::Nint(0.3*fMac2->GetVelRes()));
    504 #endif
     389        // FIXME: Makes sense?
     390        fMac1->SetDeceleration(TMath::Nint(0.03*1000000000));
     391        fMac2->SetDeceleration(TMath::Nint(0.09*1000000000));
     392
    505393        fMac1->SetRpmMode(FALSE);
    506394        fMac2->SetRpmMode(FALSE);
    507395    }
    508 
    509 /*
    510     fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
    511     fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
    512 
    513     fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
    514     fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
    515     fMac2->WaitForSdo(0x3000, 0);
    516     fMac1->WaitForSdo(0x3000, 0);
    517     fMac1->SetRpmMode(FALSE);
    518     fMac2->SetRpmMode(FALSE);
    519     */
    520396
    521397    //
    … …  
    538414bool MCosy::CheckNetwork()
    539415{
    540     //return kTRUE;
    541     //CheckConnections();
     416    if (!HasConnection())
     417    {
     418        gLog << warn << "- No connection to network." << endl;
     419        return false;
     420    }
    542421
    543422    CheckForError();
    … …  
    557436        fMac1->HandleError();
    558437        fMac2->HandleError();
    559         //if (fMac3)
    560         //    fMac3->HandleError();
    561438        if (HasError() || HasZombie())
    562439            return false;
    … …  
    564441
    565442    CheckForError();
    566     return true;
     443
     444    return fMac1->IsOperative() && fMac2->IsOperative();
    567445}
    568446
    … …  
    581459        StopMovement();
    582460        return 0;
    583 /*
    584     case WM_PRESET:
    585         cout << "WM_Preset: start." << endl;
    586         if (!CheckNetwork())
    587             return (void*)0xebb0;
    588         fZd1->SetPreset();
    589         fZd2->SetPreset();
    590         fAz->SetPreset();
    591         cout << "WM_Preset: done. (return 0xaffe)" << endl;
    592         return (void*)0xaffe;
    593 */
    594         /*
    595     case WM_CALIB:
    596         {
    597             cout << "WM_Calib: start." << endl;
    598             if (!CheckNetwork())
    599                 return (void*)0xebb0;
    600 
    601             SlaStars sla(fObservatory);
    602             sla.Now();
    603 
    604             RaDec rd = *((RaDec*)mp);
    605 
    606             //RaDec rd(37.94, 89.2644);      // POLARIS
    607             //RaDec rd(213.915417, 19.1825); // ARCTURUS
    608 
    609             cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
    610 
    611             ZdAz za=sla.CalcZdAz(rd*TMath::DegToRad())*16384.0/k2Pi;
    612 
    613             cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
    614 
    615             ZdAz sepos = GetSePos();
    616             cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
    617 
    618             fZd1->SetPreset(za.Zd());
    619             fZd2->SetPreset(-za.Zd());
    620             fAz->SetPreset(za.Az());
    621 
    622             cout << "WM_Calib: done. (return 0xaffe)" << endl;
    623         }
    624         return (void*)0xaffe;
    625         */
     461
    626462    case WM_TPOINT:
    627463        {
    … …  
    655491            *fOutTp << setprecision(7) << za.Az() << " " << za.Alt() << " ";
    656492
    657             ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE;
     493            ZdAz sepos = GetSePos()*TMath::TwoPi();
    658494            za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
    659495            za *= TMath::RadToDeg();
    … …  
    681517
    682518            ZdAz dest = *((ZdAz*)mp) * TMath::DegToRad();
    683             if (!SetPosition(dest, kTRUE))
    684                 return 0x1234;
     519            //if (!SetPosition(dest, kTRUE))
     520            //    return 0x1234;
    685521
    686522            SlaStars sla(fObservatory);
    … …  
    777613        //cout << "WM_Track: done. (return 0x8888)" << endl;
    778614        return 0x7878;
    779 
     615/*
    780616    case WM_TESTSE:
    781617        //cout << "WM_TestSe: start." << endl;
    … …  
    795631        //cout << "WM_Disply: done. (return 0xd1e1)" << endl;
    796632        return 0xd1e1;
    797 
     633 */
    798634    case WM_TRACK:
    799635    case WM_GRB:
    … …  
    832668        return 0xbe0e;
    833669
    834     case WM_HOME:
    835         //cout << "WM_Home: START" << endl;
    836         if (!CheckNetwork())
    837             return 0xebb0;
    838         else
    839         {
    840             cout << "HOME NOT ALLOWED... for Magic." << endl;
    841             /*
    842             cout << "Going Home..." << endl;
    843             TEnv env(".cosyrc");
    844 
    845             SetStatus(MDriveCom::kMoving);
    846 
    847             fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
    848             fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
    849 
    850             gLog << "SETHOME DONE" << endl;
    851 
    852             SetStatus(HasError() ? MDriveCom::kError : MDriveCom::kStopped);
    853 
    854             fAz->SetPreset();
    855             fZd1->SetPreset();
    856             fZd2->SetPreset();
    857 
    858             fMac1->ReqPos();
    859             fMac2->ReqPos();
    860             fMac3->StopMotor();
    861             */
    862         }
    863         //cout << "WM_Home: done. (return 0x403e)" << endl;
    864         return 0x403e;
    865 
    866670    case WM_CALCALTAZ:
    867671        {
    … …  
    879683            cout << "Zd/Az target:  " << a1.Zd()*TMath::RadToDeg() << "° " << a1.Az()*TMath::RadToDeg() << "°" << endl;
    880684
    881             if (fZd1 && fZd2 && fAz)
     685            if (fMac1 && fMac2)
    882686                a1 = AlignTrackingPos(a1);
    883687
    … …  
    886690            a1 *= TMath::RadToDeg();
    887691
    888             const ZdAz a2 = a1*kResSE/360;
     692            const ZdAz a2 = a1/360;
    889693
    890694            cout << "Zd/Az bended:  " << a1.Zd() << "° " << a1.Az() << "°" << endl;
    … …  
    895699    case WM_ENDSWITCH:
    896700        {
    897             ZdAz pos = GetSePos()*TMath::TwoPi()/kResSE;
     701            ZdAz pos = GetSePos()*TMath::TwoPi();
    898702            pos = fBending.SubtractOffsets(pos)*TMath::RadToDeg();
    899703
    … …  
    919723}
    920724
    921 Int_t MTTalk::Thread()
    922 {
    923     fCosy->TalkThread();
    924     return 0;
    925 }
    926 
    927725void MCosy::ReadConfig(MEnv &env)
    928726{
    … …  
    947745    gLog << " * Min': " << fMin.Zd()*TMath::RadToDeg() << "deg  " << fMin.Az()*TMath::RadToDeg() << "deg" << endl;
    948746    gLog << " * Max': " << fMax.Zd()*TMath::RadToDeg() << "deg  " << fMax.Az()*TMath::RadToDeg() << "deg" << endl;
    949 
    950     kGear.Set(env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0),
    951               env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0));
    952 
    953     Bool_t mac1 = fMac1 && !fMac1->IsZombieNode();
    954     Bool_t mac2 = fMac2 && !fMac2->IsZombieNode();
    955     //Bool_t mac3 = fMac3 && !fMac3->IsZombieNode();
    956 
    957     Bool_t zd1  = fZd1  && !fZd1->IsZombieNode();
    958     Bool_t zd2  = fZd2  && !fZd2->IsZombieNode();
    959     Bool_t az   = fAz   && !fAz->IsZombieNode();
    960 
    961     Double_t x = 0;
    962     Double_t y = 0;
    963 //    y = mac1 ? fMac1->GetRes() : (mac3 ? fMac3->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500));
    964     y = mac1 ? fMac1->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500);
    965     x = mac2 ? fMac2->GetRes() : env.GetValue("Zd_ResRE[re/U_mot]", 1500);
    966     kResRE.Set(x,y);
    967 
    968     gLog << " * Az RE resolution: " << x << " re/U_mot" << endl;
    969     gLog << " * Zd RE resolution: " << y << " re/U_mot" << endl;
    970 
    971     x = zd1 ? fZd1->GetPhysRes() : (zd2 ? fZd2->GetPhysRes() : env.GetValue("Zd_ResSE[se/U_tel]", 16384));
    972     y = az  ? fAz->GetPhysRes() : env.GetValue("Az_ResSE[se/U_tel]", 16384);
    973     kResSE.Set(x,y);
    974 
    975     gLog << " * Az SE resolution: " << x << " se/U_tel" << endl;
    976     gLog << " * Zd SE resolution: " << y << " se/U_tel" << endl;
    977 
    978     /*
    979     kResRE.Y(0);
    980     if (fMac1 && !fMac1->IsZombieNode())
    981         kResRE.Y(fMac1->GetRes());
    982     else
    983         y = fMac3 && !fMac3->IsZombieNode() ? fMac3->GetRes() : env.GetValue("Az_ResRE[re/U_mot]", 1500);
    984         if (fMac3 && !fMac3->IsZombieNode())
    985             kResRE.Y(fMac3->GetRes());
    986         else
    987             kResRE.Y(env.GetValue("Az_ResRE[re/U_mot]", 1500));
    988 
    989     kResRE.X(0);
    990     if (fMac2 && !fMac2->IsZombieNode())
    991         kResRE.X(fMac2->GetRes());
    992     else
    993         kResRE.X(env.GetValue("Zd_ResRE[re/U_mot]", 1500));
    994 
    995     kResSE.X(0);
    996     if (fZd1 && !fZd1->IsZombieNode())
    997         kResSE.X(fZd1->GetPhysRes());
    998     else
    999         if (fZd2 && !fZd2->IsZombieNode())
    1000             kResSE.X(fZd2->GetPhysRes());
    1001         else
    1002             kResSE.X(env.GetValue("Zd_ResSE[se/U_mot]", 16384));
    1003 
    1004     kResSE.Y(0);
    1005     if (fAz && !fAz->IsZombieNode())
    1006         kResSE.Y(fAz->GetPhysRes());
    1007     else
    1008         kResSE.Y(env.GetValue("Az_ResSE[se/U_mot]", 16384));
    1009         */
    1010 
    1011     // believing the Macs manual '*4' shouldn't be necessary, but it is.
    1012     // Because the a RE is 4 quad counts.
    1013     // Calculating speeds we have to convert back to qc
    1014     kResRE  *= 4;
    1015     kGearTot = Mul(kResRE, kGear);
    1016 //    kGearTot = kResRE*kGear;
    1017 
    1018 //    gLog << all;
    1019 //    gLog << " * Setting Gear Ratios:" << endl;
    1020  //   gLog << "   --------------------" << endl;
    1021     gLog << " *  X: " << kGear.X() << "*" << kResRE.X()/4 << "/" << kResSE.X() << "=4*" << kGearTot.X() << "/" << kResSE.X() << endl;
    1022     gLog << " *  Y: " << kGear.Y() << "*" << kResRE.Y()/4 << "/" << kResSE.Y() << "=4*" << kGearTot.Y() << "/" << kResSE.Y() << endl;
    1023 }
    1024 /*
    1025 void MCosy::InitSync()
    1026 {
    1027     if (!fMac3)
    1028     {
    1029         gLog << "Unable to Init Sync! Mac3 not available." << endl;
    1030         return;
    1031     }
    1032 
    1033     const int res = fMac3->GetVelRes();
    1034 
    1035     fMac3->SetVelocity(0.3*res);
    1036     fMac3->SetAcceleration(0.2*res);
    1037     fMac3->SetDeceleration(0.2*res);
    1038     fMac3->StartPosSync();
    1039 }
    1040 */
    1041 void MCosy::TalkThreadSeTest()
    1042 {
    1043 //    if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
    1044     //        return;
    1045 
    1046     if (fHist)
    1047     {
    1048         gLog << err << "You are much too fast... try again." << endl;
    1049         return;
    1050     }
    1051 
    1052     fHist = new TH2F("Diff", "Difference of SE values",
    1053                            201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
    1054     fHist->SetXTitle("ZA [\\circ]");
    1055     fHist->SetYTitle("\\Delta SE");
    1056 
    1057     Double_t offset = 0;
    1058 
    1059     int cnt = 0;
    1060 
    1061     gLog << inf2 << "Starting Shaftencoder Test..." << endl;
    1062 
    1063     gLog << err << "Build in values!!!" << endl;
    1064 
    1065     while (fBackground==kBgdSeTest)
    1066     {
    1067         fZd1->ResetPosHasChanged();
    1068         fZd2->ResetPosHasChanged();
    1069 
    1070         while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
    1071                fBackground==kBgdSeTest)
    1072         {
    1073             usleep(1);
    1074             TThread::CancelPoint();
    1075         }
    1076 
    1077         const Double_t pos[3] = {
    1078             (fZd1->GetPos()+8192)%16384,
    1079             (fZd2->GetPos()+8192)%16384,
    1080             fAz->GetPos() };
    1081 
    1082         //
    1083         //  Estimate Offset from the first ten positions
    1084         //
    1085         if (cnt++<10)
    1086         {
    1087             offset += pos[0]+pos[1];
    1088             continue;
    1089         }
    1090         if (cnt==11)
    1091         {
    1092             offset /= 10;
    1093             cnt++;
    1094         }
    1095 
    1096         Double_t apos = (pos[0]-pos[1])/2 * TMath::TwoPi() / kResSE.X();
    1097 
    1098         ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*TMath::RadToDeg();
    1099         fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
    1100     }
    1101 
    1102     gLog << inf2 << "Shaftencoder Test Stopped... displaying Histogram." << endl;
    1103 
    1104     fBackground=kBgdSeTestDispl;
    1105 }
    1106 
    1107 void MCosy::TalkThreadGear()
    1108 {
    1109 //    if (fZd1->IsZombieNode() || fZd2->IsZombieNode())
    1110     //        return;
    1111 
    1112     if (fHist)
    1113     {
    1114         gLog << err << "You are much too fast... try again." << endl;
    1115         return;
    1116     }
    1117 
    1118     fHist = new TH3F("Gear", "Gear Ratio Re/Se",
    1119                      (int)((fMax.Zd()-fMin.Zd())/2.5+1), fMin.Zd(), fMax.Zd(),
    1120                      (int)((fMax.Az()-fMin.Az())/2.5+1), fMin.Az(), fMax.Az(),
    1121                      61, 349.5, 500.5);
    1122 
    1123     fHist->SetXTitle("Zd [\\circ]");
    1124     fHist->SetYTitle("Az [\\circ]");
    1125     fHist->SetZTitle("Re/Se");
    1126 
    1127     gLog << inf2 << "Starting Gear determination..." << endl;
    1128 
    1129     ZdAz se0 = GetSePos();
    1130     ZdAz re0 = GetRePosPdo();
    1131 
    1132     while (fBackground==kBgdGear)
    1133     {
    1134         fZd1->ResetPosHasChanged();
    1135         fZd2->ResetPosHasChanged();
    1136         fAz->ResetPosHasChanged();
    1137 
    1138         while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
    1139                !fAz->PosHasChanged() && fBackground==kBgdGear)
    1140         {
    1141             usleep(1);
    1142             TThread::CancelPoint();
    1143         }
    1144 
    1145         ZdAz se = GetSePos();
    1146         ZdAz re = GetRePosPdo();
    1147 
    1148         ZdAz dse = se-se0;
    1149         ZdAz dre = re-re0;
    1150 
    1151         if (fabs(dse.Zd())*144>kResSE.X()) // Each 2.5deg (144)
    1152         {
    1153             se0.Zd(se.Zd());
    1154             re0.Zd(re.Zd());
    1155 
    1156             se -= dse/2;
    1157 
    1158             ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*TMath::RadToDeg();
    1159             ((TH3*)fHist)->Fill(bend.Zd(), bend.Az(), dre.Zd()/dse.Zd());
    1160         }
    1161 
    1162         if (fabs(dse.Az())*144>kResSE.Y()) // Each 2.5deg (144)
    1163         {
    1164             se0.Az(se.Az());
    1165             re0.Az(re.Az());
    1166 
    1167             se -= dse/2;
    1168 
    1169             ZdAz bend = fBending.CorrectBack(se*TMath::TwoPi()/kResSE)*TMath::RadToDeg();
    1170             ((TH3*)fHist)->Fill(bend.Az(), bend.Az(), dre.Az()/dse.Az());
    1171         }
    1172     }
    1173     gLog << inf2 << "Gear Test Stopped... displaying Histogram." << endl;
    1174 
    1175     fBackground=kBgdGearDispl;
    1176 }
    1177 
    1178 void MCosy::TalkThread()
    1179 {
    1180     /* ========== FIXME? =============
    1181      if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    1182         return;
    1183         */
    1184 
    1185     if (fMac1 && fMac2)
    1186     {
    1187         fMac1->ReqPos();
    1188         fMac2->ReqPos();
    1189     }
    1190 
    1191     //InitSync();
    1192 
    1193     /*** FOR DEMO MODE ***/
    1194     if (!fZd1 || !fZd2 || !fAz)
    1195         return;
    1196     /*** FOR DEMO MODE ***/
    1197 
    1198     //
    1199     // Start the Network
    1200     //
    1201     while (1)
    1202     {
    1203         //
    1204         // wait until a tracking session is started
    1205         //
    1206         while (fBackground==kBgdNone)
    1207         {
    1208             usleep(1);
    1209             TThread::CancelPoint();
    1210         }
    1211 
    1212         switch (fBackground)
    1213         {
    1214         case kBgdNone:
    1215             continue;
    1216 /*#ifndef NEWALGO
    1217         case kBgdTracking:
    1218             TalkThreadTracking();
    1219             continue;
    1220 #endif*/
    1221         case kBgdSeTest:
    1222             TalkThreadSeTest();
    1223             continue;
    1224 
    1225         case kBgdGear:
    1226             TalkThreadGear();
    1227             continue;
    1228 
    1229         default:
    1230             continue;
    1231         }
    1232     }
    1233747}
    1234748
    1235749ZdAz MCosy::GetPointingPos() const
    1236750{
    1237     if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
     751    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    1238752        return ZdAz(0, 0);
    1239753
    1240754    // GetPointingPos [deg]
    1241     const ZdAz seist = GetSePos()*TMath::TwoPi()/kResSE; // [rad]
     755    const ZdAz seist = GetSePos()*TMath::TwoPi(); // [rad]
    1242756    return fBending.CorrectBack(seist)*TMath::RadToDeg();
    1243757}
    … …  
    1258772    // Update Gui, foremer MTGui.
    1259773    //
    1260     if (fZd1)
    1261         fZd1->DisplayVal();
    1262     if (fZd2)
    1263         fZd2->DisplayVal();
    1264     if (fAz)
    1265         fAz->DisplayVal();
    1266 
     774    if (fMac1)
     775        fMac1->DisplayVal();
     776    if (fMac2)
     777        fMac2->DisplayVal();
     778
     779    /*
    1267780    Byte_t avail = 0;
    1268781
    1269782    avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
    1270783    avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
    1271     //avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
    1272     avail |= (fZd1  && !fZd1->IsZombieNode())  ? 0x08 : 0;
    1273     avail |= (fZd2  && !fZd2->IsZombieNode())  ? 0x10 : 0;
    1274     avail |= (fAz   && !fAz->IsZombieNode())   ? 0x20 : 0;
    1275784//    avail |= (!(fStatus&MDriveCom::kError) && 1 ? 0x40 : 0;
    1276 
     785  */
    1277786    Bool_t armed = kTRUE;
    1278787
    … …  
    1283792        SetStatus(MDriveCom::kError);
    1284793
    1285 
    1286     ZdAz bendist = fStatus&MDriveCom::kTracking ? fTrackingPos : GetPointingPos();
     794    const TString stataz = fMac1 ? fMac1->GetStatusDKC() : "";
     795    const TString statzd = fMac2 ? fMac2->GetStatusDKC() : "";
     796
     797    const UInt_t stat1 = fMac1 ? fMac1->GetStatusPdo3() : 0;
     798    const UInt_t stat2 = fMac2 ? fMac2->GetStatusPdo3() : 0;
     799
     800    ZdAz bendist = GetPointingPos();
    1287801
    1288802    //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;
    1289803
    1290     fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed);
     804    static MTimeout tout(0);
     805    if (tout.HasTimedOut())
     806    {
     807        tout.Start(999);
     808        fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed);
     809    }
    1291810
    1292811    fWin->UpdateWeather(*fCom);
    1293     fWin->Update(bendist, fTrackingError, fVelocity, /*fOffset,*/
    1294                  fRaDec, fZdAzSoll, fStatus, avail, armed);
     812    fWin->Update(bendist, fTrackingError, /*fVelocity, fOffset,*/
     813                 fRaDec, fZdAzSoll, fStatus, (stat1<<8)|stat2, HasConnection(), armed, statzd, stataz);
    1295814
    1296815    gLog.UpdateGui();
    1297 
    1298     const Bool_t trigger = fTriggerDisplay;
    1299     fTriggerDisplay = kFALSE;
    1300 
    1301     if (fBackground==kBgdSeTestDispl || (trigger&&fBackground==kBgdSeTest))
    1302         DisplayHistTestSe(!trigger);
    1303 
    1304     if (fBackground==kBgdGearDispl || (trigger&&fBackground==kBgdGear))
    1305         DisplayHistGear(!trigger);
    1306816
    1307817    if (fMutexGui.UnLock()==13)
    … …  
    1309819
    1310820    return kTRUE;
    1311 }
    1312 
    1313 void MCosy::DisplayHistTestSe(Bool_t del)
    1314 {
    1315     gLog << inf2 << "Displaying histogram..." << endl;
    1316 
    1317     TH2F &hist = *(TH2F*)fHist;
    1318 
    1319     if (del)
    1320     {
    1321         fHist = NULL;
    1322         fBackground = kBgdNone;
    1323     }
    1324 
    1325     TCanvas *c=new TCanvas("c1", "", 1000, 1000);
    1326     c->Divide(1,2);
    1327 
    1328     c->cd(1);
    1329     TH2 *h=(TH2*)hist.DrawCopy();
    1330 
    1331     TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
    1332     p->SetLineColor(kBlue);
    1333     p->Draw("same");
    1334     p->SetBit(kCanDelete);
    1335 
    1336     c->cd(2);
    1337 
    1338     TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
    1339             hist.GetBinLowEdge(hist.GetNbinsX()+1));
    1340     p2.SetXTitle("Zd [\\circ]");
    1341     for (int i=0; i<hist.GetNbinsX(); i++)
    1342         p2.SetBinError(i, p->GetBinError(i));
    1343     p2.SetLineColor(kRed);
    1344     p2.SetStats(0);
    1345     p2.DrawCopy();
    1346 
    1347     if (del)
    1348         delete &hist;
    1349 }
    1350 
    1351 void MCosy::DisplayHistGear(Bool_t del)
    1352 {
    1353     gLog << inf2 << "Displaying histogram..." << endl;
    1354 
    1355     TH3F &hist = *(TH3F*)fHist;
    1356 
    1357     if (del)
    1358     {
    1359         fHist = NULL;
    1360         fBackground = kBgdNone;
    1361     }
    1362 
    1363     TCanvas *c=new TCanvas("c1", "", 1000, 1000);
    1364     c->Divide(2,2);
    1365 
    1366     // ----------
    1367 
    1368     c->cd(1);
    1369     TH2D &h1=*(TH2D*)hist.Project3D("zx"); // Zd
    1370     h1.SetTitle(" Gear Ratio Zenith Distance [re/se]  ");
    1371     h1.SetXTitle("Zd [\\circ]");
    1372     h1.Draw();
    1373     h1.SetBit(kCanDelete);
    1374 
    1375     TProfile *p1 = h1.ProfileX("_pfx", -1, 9999, "s");
    1376     p1->SetLineColor(kBlue);
    1377     p1->Draw("same");
    1378     p1->SetBit(kCanDelete);
    1379 
    1380     // ----------
    1381 
    1382     c->cd(2);
    1383     TH2D &h2=*(TH2D*)hist.Project3D("zy"); // Az
    1384     h2.SetTitle(" Gear Ratio Azimuth [re/se]  ");
    1385     h2.SetXTitle("Zd [\\circ]");
    1386     h2.Draw();
    1387     h2.SetBit(kCanDelete);
    1388 
    1389     TProfile *p2 = h2.ProfileX("_pfx", -1, 9999, "s");
    1390     p2->SetLineColor(kBlue);
    1391     p2->Draw("same");
    1392     p2->SetBit(kCanDelete);
    1393 
    1394     // ----------
    1395 
    1396     c->cd(3);
    1397 
    1398     TAxis &axe1 = *h1.GetXaxis();
    1399 
    1400     TH1F f1("spreadzd", " Spread Zenith Distance ",
    1401             axe1.GetNbins(), axe1.GetXmin(), axe1.GetXmax());
    1402     f1.SetXTitle("Zd [\\circ]");
    1403     for (int i=0; i<axe1.GetNbins(); i++)
    1404         f1.SetBinError(i, p1->GetBinError(i));
    1405     f1.SetLineColor(kRed);
    1406     f1.SetStats(0);
    1407     f1.DrawCopy();
    1408 
    1409     c->cd(4);
    1410 
    1411     // ----------
    1412 
    1413     TAxis &axe2 = *h2.GetXaxis();
    1414 
    1415     TH1F f2("spreadaz", " Spread Azimuth ",
    1416             axe2.GetNbins(), axe2.GetXmin(), axe2.GetXmax());
    1417     f2.SetXTitle("Az [\\circ]");
    1418     for (int i=0; i<axe2.GetNbins(); i++)
    1419         f2.SetBinError(i, p2->GetBinError(i));
    1420     f2.SetLineColor(kRed);
    1421     f2.SetStats(0);
    1422     f2.DrawCopy();
    1423 
    1424     // ----------
    1425 
    1426     if (del)
    1427         delete &hist;
    1428821}
    1429822
    … …  
    1445838    ReadConfig(env);
    1446839
    1447     gLog << inf << "- Starting TX Thread." << endl;
    1448     fTTalk = new MTTalk(this);
    1449 
    1450840    gLog << inf << "- Starting GUI update." << endl;
    1451841    fUpdateGui->TurnOn();
    … …  
    1466856    gLog << inf << "- GUI Update stopped." << endl;
    1467857
    1468     gLog << inf << "- Stopping TX Thread." << endl;
    1469     delete fTTalk;
    1470     gLog << inf << "- TX Thread stopped." << endl;
    1471 
    1472858    gLog << inf << "- Stopping CAN network." << endl;
    1473859    Network::Stop();
    … …  
    1483869// Disable the synchronization by using a negative CAN Id for id2.
    1484870//
    1485 void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
    1486                         Int_t id4, Int_t id5, Int_t id6)
     871void MCosy::Constructor(Int_t id1, Int_t id2)
    1487872{
    1488873    //
    … …  
    1491876    gLog << inf << "- Setting up network." << endl;
    1492877
    1493     fMac1=new Macs(id1, "Mac/Az");
    1494     fMac2=new Macs(id3, "Mac/Zd");
    1495     //if (id2>=0)
    1496     //    fMac3=new Macs(id2, "Mac/Az-Sync");
    1497 
    1498     fZd1=new ShaftEncoder(id4, "SE/Zd1");
    1499     fZd2=new ShaftEncoder(id5, "SE/Zd2");
    1500     fAz =new ShaftEncoder(id6, "SE/Az");
    1501 
    1502     fZd1->SetReport(fOutRep);
    1503     fZd2->SetReport(fOutRep);
    1504     fAz->SetReport(fOutRep);
    1505 
    1506     fAz->SetMotor(fMac1);
    1507     fZd1->SetMotor(fMac2);
    1508     fZd2->SetMotor(fMac2);
    1509  
     878    fMac1=new Dkc(id1, "DKC/Az");
     879    fMac2=new Dkc(id2, "DKC/Zd");
     880
     881    fMac1->SetReport(fOutRep);
     882    fMac2->SetReport(fOutRep);
     883
    1510884    gLog << inf << "- Connecting devices to network." << endl;
    1511885
    … …  
    1515889    SetNode(fMac1);
    1516890    SetNode(fMac2);
    1517     //if (id2>=0)
    1518     //    SetNode(fMac3);
    1519     SetNode(fZd1);
    1520     SetNode(fZd2);
    1521     SetNode(fAz);
    1522891
    1523892    //
    … …  
    1528897
    1529898    gLog << all << "- Starting GUI." << endl;
    1530     fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
    1531 }
     899    fWin=new MGCosy(fObservatory, this);
     900}
     901
    1532902/*
    1533 void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
     903void MCosy::ConstructorDemo()
    1534904{
    1535905    //
    … …  
    1537907    //
    1538908    gLog << "- Setting up network." << endl;
    1539 
    1540     fZd1=new ShaftEncoder(id4, "SE/Zd1", gLog);
    1541     fZd2=new ShaftEncoder(id5, "SE/Zd2", gLog);
    1542     fAz =new ShaftEncoder(id6, "SE/Az",  gLog);
    1543 
    1544     gLog << "- Connecting devices to network." << endl;
    1545 
    1546     //
    1547     // Connect the devices to the network
    1548     //
    1549     SetNode(fZd1);
    1550     SetNode(fZd2);
    1551     SetNode(fAz);
    1552909
    1553910    //
    … …  
    1560917    fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
    1561918}
    1562 
    1563 void MCosy::ConstructorDemo()
    1564 {
    1565     //
    1566     // Create Nodes
    1567     //
    1568     gLog << "- Setting up network." << endl;
    1569 
    1570     //
    1571     // Create Gui Event timer and Gui
    1572     //
    1573     gLog << "- Initializing GUI Timer." << endl;
    1574     fUpdateGui = new TTimer(this, 100); // 100ms
    1575 
    1576     gLog << "- Starting GUI." << endl;
    1577     fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
    1578 }
    1579919*/
    1580920
    … …  
    1614954MCosy::MCosy(MEnv &env, MDriveCom *com, const char *pointing)
    1615955: Network(), fObservatory(MObservatory::kMagic1), fStarguider(NULL),
    1616 fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), /*fMac3(0),*/
    1617 fBackground(kBgdNone), fTriggerDisplay(kFALSE), fStatus(MDriveCom::kStopped),
    1618 fOutTp(0), fOutRep(0)
    1619 {
    1620     const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
    1621     const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
    1622     const Int_t id3 = env.GetValue("Zd_Id-MAC",  3); //3
    1623     const Int_t id4 = env.GetValue("Zd_Id-SE1",  4); //4
    1624     const Int_t id5 = env.GetValue("Zd_Id-SE2",  5); //5
    1625     const Int_t id6 = env.GetValue("Az_Id-SE",   6); //6
     956fMac1(0), fMac2(0), fStatus(MDriveCom::kStopped), fOutTp(0), fOutRep(0)
     957{
     958    const Int_t id1 = env.GetValue("Az_Id", 1);
     959    const Int_t id2 = env.GetValue("Zd_Id", 3);
    1626960
    1627961    TString name = GetFileName("rep", "cosy", "rep");
    … …  
    1644978        else
    1645979            gLog << err << "ERROR - Reading pointing model from " << pointing << endl;
    1646         Constructor(id1, id2, id3, id4, id5, id6);/*
     980        Constructor(id1, id2);/*
    1647981        break;
    1648982    case 1:
    … …  
    1659993    gLog.SetOutputGui(fWin->GetLog(), kTRUE);
    1660994
    1661     fZd1->SetDisplay(fWin->GetLabel2());
    1662     fZd2->SetDisplay(fWin->GetLabel3());
    1663     fAz->SetDisplay(fWin->GetLabel1());
     995    fMac2->SetDisplay(fWin->GetLabel2());
     996    fMac1->SetDisplay(fWin->GetLabel1());
    1664997
    1665998    fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep);
    … …  
    17161049
    17171050    gLog << inf2 << "Deleting Nodes." << endl;
    1718     fZd1->SetReport(0);
    1719     fZd2->SetReport(0);
    1720     fAz->SetReport(0);
    1721 
    1722     delete fAz;
    1723     delete fZd1;
    1724     delete fZd2;
     1051    fMac1->SetReport(0);
     1052    fMac2->SetReport(0);
     1053
    17251054    delete fMac1;
    17261055    delete fMac2;
    1727     //if (fMac3)
    1728     //    delete fMac3;
    17291056
    17301057    gLog << inf2 << "Deleting MGCosy." << endl;

  • trunk/MagicSoft/Cosy/main/MCosy.h

    r8875 r9132  
    4545#define WM_ARM          0x1013
    4646
    47 class ShaftEncoder;
    48 class Macs;
     47class Dkc;
     48
    4949class MGCosy;
    5050class MCosy;
    51 class TH1;
    5251class MStarguider;
    5352class MDriveCom;
    5453class MEnv;
    5554
    56 class MTTalk : public MThread
    57 {
    58 private:
    59     MCosy *fCosy;
     55class TTimer;
    6056
    61     Int_t Thread();
    62 
    63 public:
    64     MTTalk(MCosy *cosy) : MThread("MTTalk"), fCosy(cosy)
    65     {
    66         RunThread();
    67     }
    68     ~MTTalk();
    69 };
    70 
    71 class TTimer;
    7257class MCosy : public Network, public MsgQueue, public TObject
    7358{
    74     friend class MTTalk;
    7559    friend class MSlewing;
    7660    friend class MTracking;
    … …  
    8165    MStarguider *fStarguider;
    8266
    83     ShaftEncoder *fZd1;
    84     ShaftEncoder *fZd2;
    85     ShaftEncoder *fAz;
    86 
    87     Macs *fMac1;
    88     Macs *fMac2;
    89     //Macs *fMac3;
     67    Dkc *fMac1;
     68    Dkc *fMac2;
    9069
    9170    MGCosy    *fWin;
    … …  
    9372
    9473    TTimer    *fUpdateGui;
    95     MTTalk    *fTTalk;    // should be outsourced, like the starguider.
    96                           // with a generic interface to both...
    97     ZdAz fTrackingPos;    // [deg] Current tracking position
    98     ZdAz fTrackingPosRaw; // [deg] Raw tracking position
    9974
    10075    TMutex fMutexGui;
    101 
    102     enum BackgroundAction_t
    103     {
    104         kBgdNone,
    105         //kBgdTracking,
    106         kBgdSeTest,
    107         kBgdSeTestDispl,
    108         kBgdGear,
    109         kBgdGearDispl
    110     };
    111 
    112     BackgroundAction_t fBackground;    // Flag for present backgroundthread
    11376
    11477    ZdAz  fTrackingError; // [rad] Tracking Offset between SE and calc-pos
    … …  
    11679    RaDec fRaDec;         // Position to track
    11780    ZdAz  fAccuracy;      // Actual accuracy of Tracking
    118     ZdAz  fVelocity;      // Actual velocity of Tracking
    11981    ZdAz  fMin;
    12082    ZdAz  fMax;
    121 
    122     TH1  *fHist;
    123     Bool_t fTriggerDisplay;
    124 
    125     XY kResSE;   // describing the resolution of the system [se/U_tel]
    126     XY kResRE;   // describing the resolution of the system [re/U_mot]
    127     XY kGear;    // describing the resolution of the system [U_mot/U_tel]
    128     XY kGearTot; // describing the resolution of the system [re/U_tel]
    12983
    13084    MPointing fBending;
    … …  
    14296    UInt_t GetStatus() const { return fStatus; }
    14397
    144     ZdAz GetRePos();
    145     ZdAz GetRePosPdo();
    146     ZdAz GetSePos() const; // [se]
    147     // FIXME: Must depend on the Shaftencoder mounted
    148     ZdAz GetSePosRad() const { return GetSePos()*TMath::TwoPi()/16384; } // [rad]
    149 
    150     void InitSync();
    151 
    152     void TalkThread();
    153     void TalkThreadSeTest();
    154     void TalkThreadGear();
    155 
    156     void DisplayHistTestSe(Bool_t del=kTRUE);
    157     void DisplayHistGear(Bool_t del=kTRUE);
    158 
    15998    int  SetPosition(const ZdAz &dst, Bool_t track=kFALSE);
    160     void TrackPosition(const RaDec &dst); // ra, dec [rad]
     99    void TrackPosition(const RaDec &dst);    // ra, dec [rad]
    161100    void TrackPositionGRB(const RaDec &dst); // ra, dec [rad]
    162101
    … …  
    170109    void WaitForEndMovement();
    171110
    172     void Constructor(Int_t id1, Int_t id2, Int_t id3, Int_t id4, Int_t id5, Int_t id6);
     111    void Constructor(Int_t id1, Int_t id2);
    173112    //void ConstructorSE(Int_t id1, Int_t id2, Int_t id3);
    174113    //void ConstructorDemo();
    … …  
    198137    MGCosy *GetWin() { return fWin; }
    199138
    200     ZdAz GetTrackingPosDeg() const { return fTrackingPos; };
    201     ZdAz GetTrackingPosRaw() const { return fTrackingPosRaw; };
    202     void SetTrackingPosRE(ZdAz za);
    203 
    204     AltAz GetAltAzDeg() const
    205     {
    206         ZdAz sepos = GetSePos()*TMath::TwoPi()/kResSE;
    207         AltAz za1(TMath::Pi()/2-sepos.Zd(), sepos.Az());
    208         za1 *= kRad2Deg;
    209         return za1;
    210     }
     139    ZdAz GetSePos() const; // [revolutions]
    211140
    212141    MLog *GetOutRep() { return fOutRep; }
    … …  
    214143    MDriveCom *GetDriveCom() { return fCom; }
    215144
    216     // static ZdAz CorrectTarget(const ZdAz &src, const ZdAz &dst);
    217     //    static ZdAz RaDec2ZdAz(const double mjd, const RaDec &pos, const RaDec &pm=RaDec(0,0));
    218 
    219145    ClassDef(MCosy, 0)
    220146};

  • trunk/MagicSoft/Cosy/main/MSlewing.cc

    r8875 r9132  
    55
    66#include "MCosy.h"
    7 #include "macs.h"
     7#include "dkc.h"
    88#include "MDriveCom.h"
    99
    … …  
    1717//#define EXPERT
    1818#undef EXPERT
     19
     20bool MSlewing::SetAcc(Dkc *mac, Float_t acc)
     21{
     22    // FIXME: Get acceleration scale from DKC!
     23    mac->SetAcceleration(TMath::Nint(acc*1000000000));
     24    return !mac->IsZombieNode();
     25}
    1926
    2027// --------------------------------------------------------------------------
    … …  
    2835//  target position at the same time.
    2936//
    30 void MSlewing::SetPosVelocity(const Float_t ratio, Float_t vel)
    31 {
    32     //
    33     // Set velocities
    34     //
    35     const int vr = fCosy->fMac1->GetVelRes();
    36     vel *= vr;
    37 
    38     if (ratio<1)
    39     {
    40         fCosy->fMac1->SetVelocity(TMath::Nint(vel));
    41         fCosy->fMac2->SetVelocity(TMath::Nint(vel*ratio));
     37void MSlewing::SetPosVelocity(const ZdAz &res, Float_t vel)
     38{
     39    const Double_t taz = TMath::Abs(res.Az())/fCosy->fMac1->GetVelMax();
     40    const Double_t tzd = TMath::Abs(res.Zd())/fCosy->fMac2->GetVelMax();
     41
     42    if (tzd > taz)
     43    {
     44        fCosy->fMac1->SetVelocityRel(vel*TMath::Abs(res.Az()/res.Zd()));
     45        fCosy->fMac2->SetVelocityRel(vel);
    4246    }
    4347    else
    4448    {
    45         fCosy->fMac1->SetVelocity(TMath::Nint(vel/ratio));
    46         fCosy->fMac2->SetVelocity(TMath::Nint(vel));
    47     }
    48 }
    49 
    50 // --------------------------------------------------------------------------
    51 //
    52 // Does a relative positioning.
     49        fCosy->fMac1->SetVelocityRel(vel);
     50        fCosy->fMac2->SetVelocityRel(vel*TMath::Abs(res.Zd()/res.Az()));
     51    }
     52}
     53
     54// --------------------------------------------------------------------------
     55//
     56// Does an absolute positioning.
    5357//
    5458// The steps to move are given in a ZdAz object relative to the current
    … …  
    5761// if Axis 2==kTRUE. The function waits for the movement to be finished.
    5862//
    59 void MSlewing::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
     63void MSlewing::DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
    6064{
    6165    if (fCosy->HasZombie())
    … …  
    6468    fCosy->SetStatus(MDriveCom::kMoving);
    6569
    66     if (axe1) fCosy->fMac2->StartRelPos(TMath::Nint(rd.Zd()));
    67     if (axe2) fCosy->fMac1->StartRelPos(TMath::Nint(rd.Az()));
     70    if (axe1) fCosy->fMac2->StartAbsPosRev(rd.Zd());
     71    if (axe2) fCosy->fMac1->StartAbsPosRev(rd.Az());
     72
     73    if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 0);
     74    if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 0);
     75
     76    // FIXME: We need a delay here to account for the delay of the
     77    // toggle bit in the SPS. We need a more precise return value from
     78    // the SPS:
     79    usleep(150000);
     80
    6881#ifdef EXPERT
    6982    cout << "Waiting for positioning..." << flush;
    7083#endif
    71     if (axe1) fCosy->fMac2->WaitForSdo(0x6004, 1);
    72     if (axe2) fCosy->fMac1->WaitForSdo(0x6004, 1);
    73 
    7484    fCosy->WaitForEndMovement();
    7585#ifdef EXPERT
    … …  
    7888}
    7989
    80 bool MSlewing::SetAccDec(Macs *mac, Float_t acc, Float_t dec)
    81 {
    82     const int vr = mac->GetVelRes();
    83     mac->SetAcceleration(TMath::Nint(acc*vr));
    84     mac->SetDeceleration(TMath::Nint(dec*vr));
    85     return !mac->IsZombieNode();
    86 }
    87 
    8890bool MSlewing::Break()
    8991{
    9092    return fCosy->Break() || fCosy->HasError() || fCosy->HasZombie();
     93}
     94
     95
     96// --------------------------------------------------------------------------
     97//
     98// Caluclate the difference between feedback 1 and feedback 2 at
     99// the given zenith angle (feedback 2)
     100//
     101Double_t MSlewing::GetDiff(const ZdAz &za) const
     102{
     103    const Double_t zd = za.Zd(); //[revolutions]
     104
     105    const Double_t sh = -1.21  *(TMath::SinH(0.916*zd*TMath::TwoPi())-1);
     106    const Double_t cs =  0.667 *TMath::Cos(1.735*(zd-0.236)*TMath::TwoPi());
     107    const Double_t of =  0.6497;
     108
     109    return (sh+cs+of)/360;      //[revolutions]
    91110}
    92111
    … …  
    104123int MSlewing::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
    105124{
    106     const ZdAz d = dst*kRad2Deg;
    107 
    108     MTime t(-1);
    109     gLog << all << t << " - Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
    110 
    111     //
    112     // Calculate new target position (shortest distance to go)
    113     //
    114     //const ZdAz src = fCosy->GetSePos(); // [se]
    115 
    116     //
    117     // Make sure that the motors are in sync mode (necessary if the
    118     // MACS has been rebooted from a Zombie state.
    119     //
    120     //InitSync();
    121     //if (fMac3->IsZombieNode())
    122     //    return false;
     125    gLog << all << MTime(-1) << " - Target Position: " << dst.Zd()*TMath::RadToDeg() << "deg, " << dst.Az()*TMath::RadToDeg() << "deg (Zd/Az)" << endl;
    123126
    124127    //
    … …  
    128131    // const ZdAz dest = CorrectTarget(src, dst);
    129132    //
    130     ZdAz bend = fCosy->fBending(dst); // [rad]
    131 
    132     const ZdAz dest = bend*fCosy->kResSE/TMath::TwoPi(); // [se]
     133    const ZdAz bend = fCosy->fBending(dst);  // [rad]
     134    const ZdAz dest = bend/TMath::TwoPi();   // [revolutions]
    133135
    134136    if (!fCosy->CheckRange(bend))
    135137        return kFALSE;
    136138
    137     bend *= kRad2Deg;
    138139    fCosy->fZdAzSoll = dst;
    139140
    140     //cout << "Source        Zd: " << src.Zd()  << "se  Az:" << src.Az()  << "se" << endl;
    141     //cout << "Destination   Zd: " << Rad2SE(dst.Zd()) << "se  Az:" << Rad2SE(dst.Az())  << "se" << endl;
    142     //cout << "Bend'd Dest   Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
    143     //cout << "Bend'd Dest   Zd: " << bend.Zd() << "deg  Az:" << bend.Az() << "deg" << endl;
    144 
    145     //
    146     // Set velocities
    147     //
    148     //const int vr = fCosy->fMac1->GetVelRes();
    149 
    150     const Float_t rad2se = fCosy->kResSE.X()/TMath::TwoPi();
    151 
    152141    int i;
    153     for (i=0; i<(track?1:10) && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/; i++)
    154     {
    155 
     142    for (i=0; i<(track?1:10) && !Break(); i++)
     143    {
    156144        gLog << inf2 << "- Step #" << i << endl;
    157145
    158         // Get Shaft Encoder Positions
    159         const ZdAz p=fCosy->GetSePos();
    160 
    161         // calculate control deviation
    162         ZdAz rd = dest-p; // [se]
    163         ZdAz cd = rd;     // [se]
    164         // Correct for having two SE available
    165 // FIMXE....
    166 //       cd.Zd(cd.Zd()*2);
    167         // Round to check whether we are as near as possible
    168         // to the value we expect
    169 
    170         cout << cd.Zd() << " " << cd.Az() << endl;
    171 
    172         cd /= fCosy->kResSE; // Scale to Revolutions
     146        // Get feedback 2
     147        const ZdAz sepos = fCosy->GetSePos();
     148
     149        // Calculate residual to move deviation
     150        const ZdAz res = dest-sepos; // [revolutions]
    173151
    174152        gLog << inf2 << "- Shaftencoders show a residual deviation of dZd=";
    175         gLog << MString::Format("%.2f", cd.Zd()*360*60) << "' and dAz=";
    176         gLog << MString::Format("%.2f", cd.Az()*360*60) << "'" << endl;
    177 
    178         cout << cd.Zd() << " " << cd.Az() << endl;
    179  
    180         cd *= 1./fMaxResidual; // Scale to units of the maximum residual
    181 
    182         cout << fCosy->kResSE.X() << " " << 1./fMaxResidual << endl;
    183         cout << cd.Zd() << " " << cd.Az() << endl;
    184 
     153        gLog << MString::Format("%.2f", res.Zd()*360*60) << "' and dAz=";
     154        gLog << MString::Format("%.2f", res.Az()*360*60) << "'" << endl;
     155
     156        // Check which axis should still be moved
     157        ZdAz cd  = res;             // [revolutions]
     158        cd *= 1./fMaxResidual;      // Scale to units of the maximum residual
    185159        cd.Abs();
    186160
    187         // FIXME:: Do NOT check Shaftencoder values!!!
    188161        // Check if there is a control deviation on the axis
    189162        const Bool_t cdzd = cd.Zd()>0.5 ? kTRUE : kFALSE;
    190163        const Bool_t cdaz = cd.Az()>0.5 ? kTRUE : kFALSE;
    191164
    192         cout << (int)cdzd << " " << (int)cdaz << endl;
    193 
    194165        // check if we reached the correct position already
    195166        if (!cdzd && !cdaz)
    196167        {
    197             t.Now();
    198             gLog << all << t << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
     168            gLog << all << MTime(-1) << " - Positioning done in " << i << (i==1?" step.":" steps.") << endl;
    199169            fCosy->SetStatus(MDriveCom::kStopped);
    200170            fCosy->fCom->SendStatus("Target position reached.");
    … …  
    204174        // ==============================================
    205175        //   Estimate the noncircularity of the zd axis
    206 //        const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE;
    207 
    208 //        const double p1 = ist.Zd()-19.0605/kRad2Deg;
    209 //        const double p2 = dst.Zd()-19.0605/kRad2Deg;
    210 
    211  //       const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*rad2se;
    212  //       const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se;
    213         // ===========================================
    214 
    215         gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl;
    216 
    217 
    218         // change units from se to re
    219 //        rd *= fCosy->kGearTot/fCosy->kResSE; // [re]
    220         rd *= Div(fCosy->kGearTot,fCosy->kResSE); // [re]
    221 //        rd.Zd(f2-f1);
    222 
    223         // Initialize Velocities so that we reach both positions
    224         // at the same time
    225         if (i)
    226         {
    227             //cout << "--- LO-SPEED ---" << endl;
    228             SetAccDec(fCosy->fMac1, 0.05, 0.05);
    229             SetAccDec(fCosy->fMac2, 0.05, 0.05);
    230 
    231             SetPosVelocity(1.0, 0.05);
    232         }
    233         else
    234         {
    235             // 15 motor revolutions
    236             const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y();
    237 
    238 //          lout << "MSlewing::SetPosition y: " << y << " rd.Az(): " << rd.Az() << endl;
    239 //          lout << "MSlewing::SetPosition fCosy->kGearTot.Y(): " << fCosy->kGearTot.Y() << " fCosy->kResSE.Y(): " << fCosy->kResSE.Y() << endl;
    240 
    241             if (rd.Az()>-y && rd.Az()<y)
    242                 SetAccDec(fCosy->fMac1, 0.05, 0.05);
    243             else
    244                 SetAccDec(fCosy->fMac1, fAcc, fDec);
    245 
    246             SetAccDec(fCosy->fMac2, fAcc, fDec);
    247 
    248             SetPosVelocity(fabs(rd.Ratio()), fVel);
    249         }
    250 
    251         rd.Round();
    252 
    253         // FIXME? Check for Error or Zombie?
    254 
    255         // cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
    256         // cout << " + APOS:  Zd=" << setw(6) << p.Zd()  << "se   Az=" << setw(6) << p.Az()  << "se" << endl;
    257         // cout << " +       dZd=" << setw(6) << cd.Zd() << "se  dAz=" << setw(6) << cd.Az() << "se" << endl;
    258         // cout << " +       dZd=" << setw(6) << rd.Zd() << "re  dAz=" << setw(6) << rd.Az() << "re" << endl;
    259         // cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X()  << "se   Az=" << setw(6) << kGearRatio.Y()  << "se" << endl;
    260 
    261         // repositioning (relative)
    262         gLog << inf2 << "- Do Relative Positioning..." << endl;
    263         DoRelPos(rd, cdzd, cdaz);
    264         gLog << inf2 << "- Relative Positioning Done" << endl;
    265     }
    266     if (i==1 && track && !Break()/*(fCosy->Break() || fCosy->HasError() || fCosy->HasZombie())*/)
    267     {
    268         t.Now();
    269         gLog << all << t << " - Positioning done." << endl;
     176        const Double_t add = GetDiff(sepos)-GetDiff(dest);
     177
     178        const ZdAz dest2(dest.Zd()+add, dest.Az());
     179        const ZdAz res2 = dest-sepos;
     180        // =================================================
     181
     182        //gLog << warn << "WARNING - The center of the elevation axis is taken as center of the drive bow" << endl;
     183
     184        SetAcc(fCosy->fMac1, fAcc.Az());
     185        SetAcc(fCosy->fMac2, fAcc.Zd());
     186
     187        SetPosVelocity(res2, fVel);
     188
     189        gLog << inf2 << "- Do absolute positioning..." << endl;
     190        DoAbsPos(dest2, cdzd, cdaz);
     191        gLog << inf2 << "- Absolute Positioning Done" << endl;
     192    }
     193    if (i==1 && track && !Break())
     194    {
     195        gLog << all << MTime(-1) << " - Positioning done." << endl;
    270196        fCosy->SetStatus(MDriveCom::kStopped);
    271197        fCosy->fCom->SendStatus("Tracking preposition reached.");
    … …  
    278204        fCosy->SetStatus(MDriveCom::kStopped);
    279205
    280     t.Now();
    281     gLog << warn << t << " - Warning: Requested position not reached (i=" << i << ")" << endl;
     206    gLog << warn << MTime(-1) << " - Warning: Requested position not reached (i=" << i << ")" << endl;
    282207
    283208    fCosy->fCom->SendStatus("Target position missed!");

  • trunk/MagicSoft/Cosy/main/MSlewing.h

    r8875 r9132  
    66#endif
    77
    8 class Macs;
     8class Dkc;
    99class MCosy;
    10 class ZdAz;
     10
     11#ifndef MARS_MPointing
     12#include "MPointing.h"
     13#endif
    1114
    1215class MSlewing
    … …  
    1518    MCosy *fCosy;
    1619
    17     bool SetAccDec(Macs *mac, Float_t acc, Float_t dec);
     20    bool SetAcc(Dkc *mac, Float_t acc);
    1821    bool Break();
    1922
    2023private:
    2124    Float_t fVel;
    22     Float_t fAcc;
    23     Float_t fDec;
     25    ZdAz    fAcc;
    2426
    2527    Float_t fMaxResidual; // [rev] Maximum residual deviation to stop pointin
    2628
    2729    void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
    28     void SetPosVelocity(const Float_t ratio, Float_t vel);
     30    void DoAbsPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
     31    void SetPosVelocity(const ZdAz &res, Float_t vel);
     32
     33    Double_t GetDiff(const ZdAz &za) const;
    2934
    3035public:
    31     MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1), fMaxResidual(1./16384) { }
     36    MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0,0), fMaxResidual(1./16384) { }
     37    virtual ~MSlewing() { }
    3238
    33     void SetPointAccDec(Float_t acc, Float_t dec) { fAcc = acc; fDec = dec; }
     39    void SetPointAcc(Float_t zd, Float_t az) { fAcc.Zd(zd); fAcc.Az(az); }
    3440    void SetPointVelocity(Float_t vel) { fVel = vel; }
    3541    int  SetPosition(const ZdAz &dst, Bool_t track=kFALSE);

  • trunk/MagicSoft/Cosy/main/MStarguider.cc

    r8870 r9132  
    523523    fCaos = new MCaos;
    524524    fCaos->ReadResources();
     525    fCaos->SetRadii(237.9, 239.9);
    525526
    526527    fStargCaos = new MCaos;
    … …  
    16661667    // From the Shaftencoders we get the current 'pointing' position
    16671668    // as it is seen by the drive system (system pointing position)
    1668     const ZdAz za1 = fCosy->GetTrackingPosRaw();
     1669    // FIXME????
     1670    const ZdAz za1 = fCosy->GetSePos()*TMath::TwoPi();
    16691671
    16701672    // Write real pointing position
    … …  
    17401742    if (d>1)
    17411743    {
    1742         MString txt;
    1743         txt.Print("%dfps", (int)((n-n0)/d+.5));
     1744        const TString txt = MString::Format("%dfps", (int)((n-n0)/d+.5));
    17441745        fFps->SetText(txt);
    17451746        fTime = t;
    … …  
    17871788            pos = fCosy->GetPointingPos();
    17881789        center = fCaos->Run(img, printl, printr, pos, t, 19, 3.0);
    1789         cout << "Caos Filter Camera center position: " << center.GetX() << " " << center.GetY() << endl;
     1790        cout << "Caos Filter Camera center position: " << center.GetX() << " " << center.GetY() << " (R=" << center.GetR() << ")" << endl;
    17901791
    17911792    }
    … …  
    19951996            fSkyBright->SetBackgroundColor(color);
    19961997
    1997             MString txt;
    1998             txt.Print("Sky Brightness: %.1f", bright);
     1998            const MString txt = MString::Format("Sky Brightness: %.1f", bright);
    19991999            fSkyBright->SetText(txt);
    20002000
    … …  
    20852085        if (aa.Az()<0)
    20862086            aa.Az(aa.Az()+360);
    2087         txt.Print("(%d, %d) %.1fd/%.1fd", fDx, fDy, -aa.Alt(), aa.Az()-180);
     2087        txt.Form("(%d, %d) %.1fd/%.1fd", fDx, fDy, -aa.Alt(), aa.Az()-180);
    20882088    }
    20892089    else
    2090         txt.Print("(%d, %d)", fDx, fDy);
     2090        txt.Form("(%d, %d)", fDx, fDy);
    20912091    fPosZoom->SetText(txt);
    20922092}
    … …  
    21242124    MAstro::Deg2Dms(deg, sgn, d, m, s);
    21252125
    2126     MString txt;
    2127     str += txt.Print("%c %03d %02d %03d ", sgn, d, m, s);
    2128 }
     2126    str += MString::Format("%c %03d %02d %03d ", sgn, d, m, s);
     2127}

  • trunk/MagicSoft/Cosy/main/MTracking.cc

    r8871 r9132  
    33#include "MLogManip.h"
    44
    5 #include "macs.h"
     5#include "dkc.h"
    66#include "shaftencoder.h"
    77
    … …  
    2020// --------------------------------------------------------------------------
    2121//
    22 // request the current positions from the rotary encoders.
    23 // use GetRePos to get the psotions. If the request fails the function
    24 // returns kFALSE, otherwise kTRUE
    25 //
    26 bool MTracking::RequestRePos()
    27 {
    28     for (int i=0; i<2; i++)
    29     {
    30         //
    31         // Send request
    32         //
    33         fCosy->fMac2->RequestSDO(0x6004);
    34         fCosy->fMac1->RequestSDO(0x6004);
    35 
    36         //
    37         // Wait until the objects are received.
    38         //
    39         fCosy->fMac2->WaitForSdo(0x6004, 0, 500, i>0);
    40         fCosy->fMac1->WaitForSdo(0x6004, 0, 500, i>0);
    41 
    42         //
    43         // If waiting was not interrupted everything is ok. return.
    44         //
    45         if (!Break())
    46             return true;
    47 
    48         fCosy->PrintError();
    49     }
    50 
    51     //
    52     // If the waiting was interrupted due to a network error,
    53     // print some logging message.
    54     //
    55     if (fCosy->HasError())
    56         gLog << err << "ERROR - while requesting re pos from Macs (SDO #6004)" << endl;
    57 
    58     return false;
    59 }
    60 
    61 // --------------------------------------------------------------------------
    62 //
    6322// Sets the tracking velocity
    6423//
    … …  
    6827Bool_t MTracking::SetVelocity(const ZdAz &v)
    6928{
    70     for (int i=0; i<2; i++)
    71     {
    72         //
    73         // Send the new velocities for both axes.
    74         //
    75         fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
    76         fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]
    77 
    78         //
    79         // Wait for the objects to be acknoledged.
    80         //
    81         fCosy->fMac2->WaitForSdo(0x3006, 1, 500, i>0);
    82         fCosy->fMac1->WaitForSdo(0x3006, 1, 500, i>0);
    83 
    84         //
    85         // If the waiting for the objects wasn't interrupted return kTRUE
    86         //
    87         if (!Break())
    88             return kTRUE;
    89 
    90         fCosy->PrintError();
    91     }
     29    const Double_t vrzd = fCosy->fMac2->GetVelRes();
     30    const Double_t vraz = fCosy->fMac1->GetVelRes();
     31
     32    //
     33    // Send the new velocities for both axes.
     34    //
     35    fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)(v.Zd()*vrzd));  // SetRpmVelocity [re/min]
     36    fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)(v.Az()*vraz));  // SetRpmVelocity [re/min]
     37
     38    //
     39    // Wait for the objects to be acknoledged.
     40    //
     41    fCosy->fMac2->WaitForSdo(0x3006, 1, 100);
     42    fCosy->fMac1->WaitForSdo(0x3006, 1, 100);
     43
     44    //
     45    // If the waiting for the objects wasn't interrupted return kTRUE
     46    //
     47    if (!Break())
     48        return kTRUE;
     49
     50    fCosy->PrintError();
    9251
    9352    //
    … …  
    11776    // Start revolution mode
    11877    //
    119     if (!SetAccDec(fCosy->fMac2, fTrackAcc, fTrackDec))
     78    if (!SetAcc(fCosy->fMac2, fTrackAcc.Zd()))
    12079        return false;
    12180
    122     if (!SetAccDec(fCosy->fMac1, fTrackAcc, fTrackDec))
     81    if (!SetAcc(fCosy->fMac1, fTrackAcc.Az()))
    12382        return false;
    12483
    … …  
    13594    return true;
    13695}
     96
    13797/*
    13898void MTracking::StopTracking()
    … …  
    177137}
    178138*/
     139
    179140// --------------------------------------------------------------------------
    180141//
    … …  
    185146// velocities are limited to the maximum velocity.
    186147//
    187 Bool_t MTracking::LimitSpeed(ZdAz *vt, const SlaStars &sla) const
    188 {
     148Bool_t MTracking::LimitSpeed(const ZdAz &vt, const SlaStars &sla) const
     149{
     150    // vt [deg/min]
     151
     152    // We can set a maximum speed here
     153    // And we can limit the change of the speed (which is done
     154    //  by acceleration in the drive anyway)
     155
     156    return kTRUE;
     157/*
    189158    // vt[re/min]
    190159
    … …  
    237206    }
    238207    return rc;
    239 }
    240 
    241 // --------------------------------------------------------------------
    242 //
    243 // Return pointing position of the telescope based on the
    244 // Shaftencoders with interpolation with motor encoders.
    245 //
    246 // GetPointingPos [re]
    247 //
    248 ZdAz MTracking::GetPointingPosRE(Bool_t pdo) const
    249 {
    250      // Conversion factor from se to re
    251      //const XY re = fCosy->kGearTot/fCosy->kResSE; //[re/se]
    252      const XY re = Div(fCosy->kGearTot, fCosy->kResSE); //[re/se]
    253 
    254      // Check wether moving direction has changed
    255      const bool bool1 = fCosy->fZd1->DirHasChanged();
    256      const bool bool2 = fCosy->fZd2->DirHasChanged();
    257 
    258      // If both directions have changed reset the flags
    259      if (bool1 && bool2)
    260      {
    261          fCosy->fZd1->ResetDirHasChanged();
    262          fCosy->fZd2->ResetDirHasChanged();
    263      }
    264 
    265      // Get shaftencoder positions
    266      // Ignore the shaftencoder which has not yet changed its value
    267      const Int_t pzd1 = fCosy->fZd1->GetPosDirCorrected();
    268      const Int_t pzd2 = fCosy->fZd2->GetPosDirCorrected();
    269      const Int_t paz  = fCosy->fAz->GetPos();
    270 
    271      const Int_t res1 = fCosy->fZd1->GetPhysRes();
    272      const Int_t res2 = fCosy->fZd2->GetPhysRes();
    273 
    274      // Get current shaftencoder position of the telescope
    275      Double_t seposzd1 = ((pzd1+res1/2)%res1)*re.X();
    276      Double_t seposzd2 = ((pzd2+res2/2)%res2)*re.X();
    277      //Double_t seposzd1 = pzd1*re.X();
    278      //Double_t seposzd2 = pzd2*re.X();
    279      Double_t seposaz  = paz *re.Y();
    280 
    281      // distance between (To+dt) and To [re]
    282      // position time difference < 5usec
    283      // fRePos does the synchronization between the
    284      // Shaft- and the rotary encoders
    285      const ZdAz repos = pdo ? fCosy->GetRePosPdo() : fCosy->GetRePos();
    286 
    287      // Get rotary encoder positions
    288      // Get stored offset if one SE has not changed its direction yet
    289      const Int_t offset1 = fCosy->fZd1->GetOffsetDirCorrected();
    290      const Int_t offset2 = fCosy->fZd2->GetOffsetDirCorrected();
    291 
    292      // Calculate the part of one SE which the motors moved
    293      // since the last SE has changed its value
    294      const Double_t offzd1 = repos.Zd() - offset1;
    295      const Double_t offzd2 = repos.Zd() - offset2;
    296      const Double_t offaz  = repos.Az() - fCosy->fAz->GetOffset();
    297 
    298      // Correct for the direction in which the motor is moving
    299      // (in which the shaftencoders should change its values)
    300      if (offaz<0)
    301          seposaz += re.Y();
    302      if (offzd1<0)
    303          seposzd1 += re.X();
    304      if (offzd2<0)
    305          seposzd2 += re.X();
    306 
    307      // If the correction exceeds one shaftencoder step stop interpolation
    308      // of shaftencoder positions using rotary encoder values.
    309      //ofstream fout("offsets.log", ios::app);
    310      //fout << MTime(-1) << " " << offaz << " " << offzd1 << " " << offzd2 << endl;
    311      /*
    312       if (TMath::Abs(offaz)>re.Y())
    313          offaz = TMath::Sign(re.Y(), offaz);
    314       if (TMath::Abs(offzd1)>re.X())
    315          offzd1 = TMath::Sign(re.X(), offzd1);
    316       if (TMath::Abs(offzd2)>re.X())
    317          offzd2 = TMath::Sign(re.X(), offzd2);
    318          */
    319 
    320      // and interpolate the shaftencoder steps using the motor
    321      // encoder positon (Be carefull the minus-sign is important)
    322      seposzd1 += offzd1;
    323      seposzd2 -= offzd2;
    324      seposaz  += offaz;
    325 
    326      return ZdAz((seposzd1-seposzd2)/2, seposaz);
     208    */
    327209}
    328210
    … …  
    359241    }
    360242
    361     //
    362     // calculate offset from present se position
    363     //
    364     //const ZdAz sepos = fCosy->GetSePos()*fCosy->kGearTot/fCosy->kResSE; //[re]
    365     if (!RequestRePos())
    366         return;
    367 
    368     // Estimate Offset before starting to track
    369     ZdAz repos = fCosy->GetRePos();
    370     fCosy->fZd1->SetOffset(TMath::Nint(repos.Zd()));
    371     fCosy->fZd2->SetOffset(TMath::Nint(repos.Zd()));
    372     fCosy->fAz->SetOffset(TMath::Nint(repos.Az()));
    373 
    374     fCosy->SetTrackingPosRE(GetPointingPosRE());
    375 
    376243    // Initialize Tracker (slalib or starguider)
    377244    fCosy->fRaDec = dst;
    … …  
    402269    // *OLD*const float dt = 1;  // 1 second
    403270    const float dt = 5;//3;  // 2 second
    404     while (!Break())
    405     {
    406         //
    407         // Request Target position for Now+dt
    408         //
    409         sla.Now(dt);
    410 
    411         //
    412         // Request nominal position for this time in the future (To+dt)
    413         //
     271    while (!Break()/* && !fCosy->HasError() && !fCosy->HasZombie()*/)
     272    {
     273        /*
     274        sla.Now(1);
    414275        const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
    415276        ZdAz dest = fCosy->AlignTrackingPos(pointing);     // fix ambiguity
    416 
    417         //ZdAz vcalc = sla.GetApproxVel(fCosy->fRaDec);
    418         //vcalc *= fCosy->kGearRatio2*4./60.; // [re/min]
    419 
    420         float dtime = -1;
    421         //if (kFALSE /*fUseStarguider*/)
    422         //    dtime = Starguider(sla.GetMjd(), dest);
    423 
    424         ZdAz repos;
    425         if (dtime<0)
    426         {
    427             dest = fCosy->fBending(dest);            // [rad]
    428             if (!fCosy->CheckRange(dest))
    429                 break;
    430 
    431             // Destination position at t+dt in re-units
    432             dest *= fCosy->kGearTot/TMath::TwoPi();  // [re]
    433 
    434             // Request absolute position of rotary encoder from Macs
    435             // Such that the RE position used in GetPointingPos is
    436             // as up-to-date as possible.
    437 // DO I NEED THIS OR IS THE PDOPOS ENOUGH?
    438             if (!RequestRePos())
    439                 break;
    440 
    441             // *NEW* offset handling
    442             // Get current position of the telescope and
    443             // forward this position to MCosy
    444             ZdAz sepos = GetPointingPosRE(); //[re]
    445             fCosy->SetTrackingPosRE(sepos);
    446 
    447             // distance between (To+dt) and To [re]
    448             // position time difference < 5usec
    449             // fRePos does the synchronization between the
    450             // Shaft- and the rotary encoders
    451             repos = fCosy->GetRePos();
    452 
    453             // Now calculate the distance to move from now
    454             // to a time in t+dt.
    455             dest -= sepos;
    456 
    457             dtime = dt;
    458         }
     277        dest = fCosy->fBending(dest);            // [rad]
     278        if (!fCosy->CheckRange(dest))
     279            break;
     280        dest *= 1./TMath::TwoPi(); //[rev]
     281        fCosy->fMac2->SendSDO(0x6004, 0, (LWORD_t)(dest.Zd()*fCosy->fMac2->GetPosRes()+.5), false);
     282        fCosy->fMac1->SendSDO(0x6004, 0, (LWORD_t)(dest.Az()*fCosy->fMac1->GetPosRes()+.5), false);
     283        usleep(100000); // 1s
     284        continue;
     285        */
     286
     287        //
     288        // Request Target position for Now+dt
     289        //
     290        sla.Now(dt);
     291
     292        //
     293        // Request nominal position for this time in the future (To+dt)
     294        //
     295        const ZdAz pointing = sla.CalcZdAz(fCosy->fRaDec); // [rad]
     296        ZdAz dest = fCosy->AlignTrackingPos(pointing);     // fix ambiguity
     297
     298        //ZdAz repos;
     299        dest = fCosy->fBending(dest);            // [rad]
     300        if (!fCosy->CheckRange(dest))
     301            break;
     302
     303        // Destination position at t+dt in re-units
     304        dest *= TMath::RadToDeg();  // [re]
     305
     306        const ZdAz sepos = fCosy->GetSePos()*360;  // [deg]
     307
     308        // Now calculate the distance to move from now
     309        // to a time in t+dt.
     310        dest -= sepos;
    459311
    460312        //
    … …  
    462314        // correct for the duration of RaDec2AltAz
    463315        //
    464         /* --- OLD --- */
    465         ZdAz v = dest*60.0/dtime; //[re/min]
    466         /* --- NEW --- seems to work worse! */
    467         //const Double_t dtaz = sla.GetTime() - fCosy->fMac1->GetPosTime();
    468         //const Double_t dtzd = sla.GetTime() - fCosy->fMac2->GetPosTime();
    469         //
    470         //ZdAz v = dest*60.0;
    471         //v.Zd(v.Zd()/dtzd);
    472         //v.Az(v.Az()/dtaz);
    473         /* --- END --- */
    474 
    475         //*fCosy->fOutRep << "> Dt:  " << dtaz << "  " << dtzd << endl;
    476 
    477         if (LimitSpeed(&v, sla))
     316        const ZdAz v  = dest * 60/dt;  // [deg/min]
     317        const ZdAz vt = v/360;         // [rpm]
     318
     319        //Double_t kpZd = TMath::Abs(fCosy->fTrackingError.Zd()*TMath::RadToDeg()*60*4);
     320        //Double_t kpAz = TMath::Abs(fCosy->fTrackingError.Az()*TMath::RadToDeg()*60*12);
     321        //v.Zd(v.Zd()*(1+TMath::Min(0.3, kpZd)));
     322        //v.Az(v.Az()*(1+TMath::Min(0.3, kpAz)));
     323
     324        if (LimitSpeed(v, sla))
    478325        {
    479326            gLog << dbg << "vt: " << v.Zd() << " " << v.Az() << "re/min" << endl;
    … …  
    482329
    483330        //
    484         // calculate real velocity of future [re/min]
    485         // believing the Macs manual '/4' shouldn't be necessary, but it is.
    486         //
    487         ZdAz vt = v; //[re'/min]
    488         //lout << " " << vt.Zd() << " " << vt.Az() << " ";
    489         vt.Round();
    490         //lout << " " << vt.Zd() << " " << vt.Az() << endl;
    491 
    492         //
    493331        // check if the drive is fast enough to follow the star
    494332        //
    495         if (vt.Zd()>.9*fCosy->fMac1->GetVelRes() || vt.Az()>.9*fCosy->fMac2->GetVelRes())
     333        if (TMath::Abs(vt.Zd())>0.5*fCosy->fMac2->GetVelMaxRev() ||
     334            TMath::Abs(vt.Az())>0.5*fCosy->fMac1->GetVelMaxRev())
    496335        {
    497             gLog << err << "ERROR - Tracking speed faster than 90% of possible maximum velocity." << endl;
     336            gLog << err << "ERROR - Tracking speed faster than 50% of possible maximum velocity." << endl;
     337            gLog << "Zd: " << vt.Zd() << "   Az: " << vt.Az() << endl;
    498338            break;
    499339        }
    … …  
    503343        // Maybe we should attenuate the changes
    504344        //
    505         //*fCosy->fOutRep << "> SetVelocity1:  " << vt.Zd() << "  " << vt.Az() << endl;
    506345        if (!SetVelocity(vt))
    507346            break;
    508         //*fCosy->fOutRep << "> SetVelocity2 " << endl;
    509 
    510         //
    511         // Now do 'unnecessary' things (timing)
    512         //
    513         fCosy->fVelocity = vt/fCosy->kGear; // [U_mot/min]
    514         // *OLD* fVelocity = vt/kGearRatio2*4;
    515 
    516         if (fOut)
    517         {
    518             fOut->Lock("MTracking::TrackPosition");
    519             *fOut << "RE-REPORT 00 " << MTime(-1) << " " << repos.Zd() << " " << repos.Az() <<" " << vt.Zd() << " " << vt.Az() << endl;
    520             fOut->UnLock("MTracking::TrackPosition");
    521         }
    522347
    523348        //
    … …  
    547372Int_t MTracking::Thread()
    548373{
    549     if (fCosy->fZd1->IsZombieNode() && fCosy->fZd2->IsZombieNode())
    550         return 1;
    551 
    552     if (fCosy->fAz->IsZombieNode())
    553         return 2;
    554 
    555374    if (!fCosy->fMac1 || !fCosy->fMac2)
    556375        return 3;
    … …  
    558377    gLog << inf2 << "- Tracking Thread started (" << MTime(-1) << ")" << endl;
    559378
    560     //const XY re2se = fCosy->kGearTot/fCosy->kResSE; //[re/se]
    561 
    562379    SlaStars sla(fCosy->fObservatory);
    563380    sla.Now();
    564381
    565     //ZdAz time;
    566 
    567382    ZdAz soll = sla.CalcZdAz(fCosy->fRaDec); // [rad]
    568383
    … …  
    571386    //
    572387    bool phca1=false;
    573     bool phca2=false;
    574388    bool phcaz=false;
    575 
    576     //ZdAz wasse = fCosy->GetSePos();
    577     //ZdAz oldse = fCosy->GetSePos();
    578389
    579390    while (1)
    … …  
    585396
    586397        // Check for changes of the shaftencoder values
    587         //*fCosy->fOutRep << "> ResetPosHasChanged" << endl;
    588         fCosy->fZd1->ResetPosHasChanged();
    589         fCosy->fZd2->ResetPosHasChanged();
    590         fCosy->fAz->ResetPosHasChanged();
    591         //*fCosy->fOutRep << "> Check for PosHasChanged" << endl;
     398        fCosy->fMac1->ResetHasChangedPos2();
     399        fCosy->fMac2->ResetHasChangedPos2();
    592400        do
    593401        {
    594             phca1 = fCosy->fZd1->PosHasChanged();
    595             phca2 = fCosy->fZd2->PosHasChanged();
    596             phcaz = fCosy->fAz->PosHasChanged();
     402            phcaz = fCosy->fMac1->HasChangedPos2();
     403            phca1 = fCosy->fMac2->HasChangedPos2();
    597404
    598405            usleep(1);
     406
    599407            TThread::CancelPoint();
    600408
    601         } while (!phca1 && !phca2 && !phcaz);
    602 
    603         // Get time from last shaftencoder position change (position: ist)
    604         // FIXME: Is this correct?
    605         //        time.Az(fCosy->fMac1->GetMjd());
    606         //        time.Zd(fCosy->fMac2->GetMjd());
    607 
    608         //Double_t mjd1 = fCosy->fZd1->GetMjd();
    609         //Double_t mjd2 = fCosy->fZd2->GetMjd();
    610         //Double_t mjd0 = fCosy->fAz->GetMjd();
    611 
    612         Double_t mjdaz = fCosy->fMac1->GetPdoMjd();//mjd0;
    613         Double_t mjdzd = fCosy->fMac2->GetPdoMjd();//TMath::Max(mjd1, mjd2);
    614 
    615         // get current position of shaftencoders (interpolated
    616         // using motor encoders)
    617         const ZdAz istse = GetPointingPosRE(kTRUE)/fCosy->kGearTot*TMath::TwoPi();
    618         //const ZdAz istse = fCosy->GetSePosPdo();
     409        } while (!phca1 && !phcaz);
     410
     411        // get current position and corresponding time of shaftencoders
     412        const ZdAz istse = fCosy->GetSePos()*TMath::TwoPi();  // [deg]
     413
     414        const Double_t mjdaz = fCosy->fMac1->GetMjdPos2();
     415        const Double_t mjdzd = fCosy->fMac2->GetMjdPos2();
     416
    619417
    620418        // calculate offset for both axis (only one is needed)
    621         // *NEW* offset handling
    622         //.const ZdAz offset = istre; //(istse*re2se - istre)*weight + fRePos*(weight-1);
    623419        // if Shaftencoder changed position, calculate nominal position
    624         if (phca1 || phca2)
     420        if (phca1)
    625421        {
    626422            ZdAz dummy = sla.CalcZdAz(fCosy->fRaDec, mjdzd);
    … …  
    644440            TThread::CancelPoint();
    645441        }
    646 
    647 
    648         //fCosy->fZdAzSoll = soll;
    649 /*
    650         // Calculate the aligned tracking position from 'soll'-position
    651         if (phca1 || phca2)
    652             fCosy->fTrackingError.Zd(soll.Zd()-istse.Zd());
    653         if (phcaz)
    654             fCosy->fTrackingError.Az(soll.Az()-istse.Az());
    655             */
    656442    }
    657443

  • trunk/MagicSoft/Cosy/main/MTracking.h

    r8869 r9132  
    1717{
    1818private:
    19     Float_t fTrackAcc;
    20     Float_t fTrackDec;
    21 
    22     //Int_t fSePosZd1;
    23     //Int_t fSePosZd2;
    24     //Int_t fSePosAz;
    25 
    26     //Int_t fRePosZd1;
    27     //Int_t fRePosZd2;
    28     //Int_t fRePosAz;
    29 
    30     //ZdAz    fRePos;  // Offset between se and re coordinate system [re]
    31     //ZdAz    fSePos;  // Shaftencoder Position
     19    ZdAz fTrackAcc;
    3220
    3321    MLog   *fOut;
    3422
    35     bool RequestRePos();
    3623    bool SetVelocity(const ZdAz &v);
    37     bool LimitSpeed(ZdAz *vt, const SlaStars &sla) const;
     24    bool LimitSpeed(const ZdAz &vt, const SlaStars &sla) const;
    3825    bool InitTracking();
    39     ZdAz GetPointingPosRE(Bool_t pdo=kFALSE) const;
    4026
    4127    Int_t Thread();
    4228
    4329public:
    44     MTracking(MCosy *cosy) : MSlewing(cosy), MThread("MTracking"), fTrackAcc(0.1), fTrackDec(0.1), fOut(0) { }
     30    MTracking(MCosy *cosy) : MSlewing(cosy), MThread("MTracking"), fTrackAcc(0, 0), fOut(0) { }
    4531
    4632    void TrackPosition(const RaDec &dst); // ra, dec [rad]
    47     void SetTrackAccDec(Float_t acc, Float_t dec) { fTrackAcc=0.1; fTrackDec=0.1; }
     33    void SetTrackAcc(Float_t zd, Float_t az) { fTrackAcc.Zd(zd); fTrackAcc.Az(az); }
    4834
    4935    void SetOut(MLog *fout) { fOut = fout; }
Note: See TracChangeset for help on using the changeset viewer.