Ignore:
Timestamp:
08/26/08 12:11:32 (16 years ago)
Author:
tbretz
Message:
*** empty log message ***
File:
1 edited

Legend:

Unmodified
Added
Removed

  • 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
Note: See TracChangeset for help on using the changeset viewer.