Ignore:
Timestamp:
03/12/03 14:19:12 (22 years ago)
Author:
tbretz
Message:
*** empty log message ***
Location:
trunk/MagicSoft/Cosy/main
Files:
2 edited

Legend:

Unmodified
Added
Removed

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

    r1813 r1817  
    248248}
    249249
     250
     251
    250252// --------------------------------------------------------------------------
    251253//
    … …  
    258260//  target position at the same time.
    259261//
    260 void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc)
     262void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel)
    261263{
    262264    //
    … …  
    266268
    267269    vel *= vr;
    268     acc *= vr;
    269270
    270271    if (ratio <1)
    … …  
    272273        fMac1->SetVelocity(vel);
    273274        fMac2->SetVelocity(vel*ratio);
    274 
    275         fMac1->SetAcceleration(acc);
    276         fMac2->SetAcceleration(acc*ratio);
    277 
    278         fMac1->SetDeceleration(acc);
    279         fMac2->SetDeceleration(acc*ratio);
    280275    }
    281276    else
    … …  
    283278        fMac1->SetVelocity(vel/ratio);
    284279        fMac2->SetVelocity(vel);
    285 
    286         fMac1->SetAcceleration(acc/ratio);
    287         fMac2->SetAcceleration(acc);
    288 
    289         fMac1->SetDeceleration(acc/ratio);
    290         fMac2->SetDeceleration(acc);
    291280    }
    292281}
    … …  
    305294    if (HasZombie())
    306295        return;
     296
    307297    SetStatus(MCosy::kMoving);
    308298
    … …  
    342332           !(Break() || HasError()) && !HasZombie())
    343333        usleep(1);
     334
     335    if (Break() || HasError() || HasZombie())
     336        lout << "WaitForEndMovement aborted..." << endl;
    344337}
    345338
    … …  
    400393// As a reference the shaftencoder values are used.
    401394//
    402 int MCosy::SetPosition(const ZdAz &dst) // [rad]
     395int MCosy::SetPosition(const ZdAz &dst, Bool_t track) // [rad]
    403396{
    404397    // FIXME: Correct by fOffset ?
     398/*
     399    ZdAz
     400
     401    if (dst.Az()<-TMath::Pi()/2)
     402        dst.Az(dst.Az() + TMath::Pi()*2);
     403
     404    if (dst.Az()>3*TMath::Pi()/2)
     405        dst.Az(dst.Az() -TMath::Pi()*2);
     406*/
     407 //   track=kFALSE;
     408
     409   // const ZdAz d1 = track ? CorrectTarget(src, dst) : dst*16384/TMath::Pi()/2; // [se]
    405410
    406411    const ZdAz d = dst*kRad2Deg;
    … …  
    422427    }
    423428
     429    //
     430    // Calculate new target position (shortest distance to go)
     431    //
     432    const ZdAz src = GetSePos(); // [se]
    424433
    425434    /*
    … …  
    439448    //    return false;
    440449
    441     //
    442     // Calculate new target position (shortest distance to go)
    443     //
    444     const ZdAz src = GetSePos(); // [se]
     450    // ====================================
     451    RequestRePos();
     452    ZdAz before = GetRePos();
     453    // ====================================
    445454
    446455    //
    … …  
    458467    cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
    459468
     469    //
     470    // Set velocities
     471    //
     472    const int vr = fMac1->GetVelRes();
     473
     474    cout << "Mac1: Velres=" << dec << vr << endl;
     475
    460476    int i;
    461477    for (i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++)
    … …  
    472488        //
    473489        ZdAz rd = dest-p; // [se]
     490/*
     491        if (rd.Az()>0 && rd.Az()<0.5)
     492            rd.Az(1);
     493        if (rd.Az()<0 && rd.Az()>-0.5)
     494            rd.Az(-1);
     495*/
     496        cout << "Deviation: " << rd.Zd() << "se  Az:" << rd.Az() << "se" << endl;
     497
     498
     499        // ===========================================
     500        Float_t k1 = dest.Zd()*360/16384 - 14.3979; //[deg]
     501        Float_t k0 = p.Zd()*360/16384    - 14.3979; //[deg]
     502
     503        //
     504        // -28re/se*cos((x/deg-14.4)*pi/180) + 443.2re/se     x
     505        // 28re/se*2*pi/360*sin((x/deg-14.4)*pi/180)*deg + 443.2re/se*x/deg
     506        //
     507
     508        Float_t dk = 443.152*(dest.Zd()-p.Zd());
     509        Float_t dx = 28.0148/2/TMath::Pi()*(sin(k1/kRad2Deg)-sin(k0/kRad2Deg));
     510
     511        cout << "Using: " << rd.Zd()*kGearRatio.X() << endl; // [re]
     512        cout << "Estimated1: " << 16384*dx + dk << endl;
     513        cout << "Estimated2: " << (16384*dx + dk)/1.085949688 << endl;
     514        // ===========================================
     515
    474516
    475517        ZdAz cd = rd;     // [se]
    476518        cd.Round();
    477 
     519/*
     520        if (rd.Az()>0 && rd.Az()<0.5)
     521            rd.Az(0.5);
     522        if (rd.Az()<0 && rd.Az()>-0.5)
     523            rd.Az(-0.5);
     524*/
    478525        //
    479526        // Check if there is a control deviation on the axis
    … …  
    489536            lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl;
    490537            SetStatus(MCosy::kStopped);
     538
     539            // ====================================
     540            RequestRePos();
     541            ZdAz after = GetRePos();
     542            // ====================================
     543
     544            after -= before;
     545
     546            cout << "Drove: " << after.Zd() << " " << after.Az() << endl;
    491547            return TRUE;
    492548        }
    … …  
    501557        // at the same time
    502558        //
    503         lout << "SetVelocity" << endl;
    504559        if (i)
    505             SetPosVelocity(1.0, 0.05, 0.1);
    506         else                              // vel(0.6)  acc(0.5)
    507             SetPosVelocity(fabs(rd.Ratio()), 0.15, 0.4);
    508 //            SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3);
     560        {
     561            fMac1->SetAcceleration(0.1*vr);
     562            fMac2->SetAcceleration(0.1*vr);
     563
     564            fMac1->SetDeceleration(0.1*vr);
     565            fMac2->SetDeceleration(0.1*vr);
     566
     567            SetPosVelocity(1.0, 0.05);
     568        }
     569        else
     570        {
     571            if (rd.Az()>-15*kGearRatio.Y() && rd.Az()<15*kGearRatio.Y())
     572            {
     573                cout << " -------------- LO ---------------- " << endl;
     574                fMac1->SetAcceleration(0.05*vr);
     575                fMac1->SetDeceleration(0.05*vr);
     576            }
     577            else
     578            {
     579                cout << " -------------- HI ---------------- " << endl;
     580                fMac1->SetAcceleration(0.2*vr);
     581                fMac1->SetDeceleration(0.1*vr);
     582            }
     583
     584            fMac2->SetAcceleration(0.2*vr);
     585            fMac2->SetDeceleration(0.1*vr);
     586
     587            // vel(0.6)  acc(0.5)
     588            SetPosVelocity(fabs(rd.Ratio()), 0.175);
     589            //            SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3);
     590        }
    509591
    510592        rd.Round();
    … …  
    529611    }
    530612
    531     StopMovement();
     613    if (i<10)
     614        StopMovement();
     615
    532616    lout << "Warning: Requested position not reached (i=" << i << ")" << endl;
    533617    return FALSE;
    … …  
    591675    // Start revolution mode
    592676    //
    593     fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
    594     fMac2->SetDeceleration(0.90*fMac2->GetVelRes());
     677    fMac2->SetAcceleration(0.2*fMac2->GetVelRes());
     678    fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
    595679    if (fMac2->IsZombieNode())
    596680        return false;
    597681
    598     fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
    599     fMac1->SetDeceleration(0.90*fMac1->GetVelRes());
     682    fMac1->SetAcceleration(0.2*fMac1->GetVelRes());
     683    fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
    600684    if (fMac1->IsZombieNode())
    601685        return false;
    … …  
    695779    ZdAz dest = sla.CalcZdAz(dst);
    696780
    697     if (!SetPosition(dest))
     781    if (dest.Az()<-TMath::Pi()/2)
     782        dest.Az(dest.Az() + TMath::Pi()*2);
     783
     784    if (dest.Az()>3*TMath::Pi()/2)
     785        dest.Az(dest.Az() -TMath::Pi()*2);
     786
     787    if (!SetPosition(dest, kTRUE))
    698788    {
    699789        lout << "Error: Cannot start tracking, positioning failed." << endl;
    … …  
    751841    //
    752842    // *OLD*const float dt = 1;  // 1 second
    753     const float dt = 3;  // 1 second
     843    const float dt = 3;  // 2 second
    754844    while (!(Break() || HasError()) && !HasZombie())
    755845    {
    … …  
    763853        //
    764854        sla.SetMjd(sla.GetMjd()+dt/(60*60*24));
    765         ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
     855        ZdAz dummy1 = sla.CalcZdAz(fRaDec);
     856
     857        if (dummy1.Az()<-TMath::Pi()/2)
     858            dummy1.Az(dummy1.Az() + TMath::Pi()*2);
     859        if (dummy1.Az()>3*TMath::Pi()/2)
     860            dummy1.Az(dummy1.Az() -TMath::Pi()*2);
     861
     862        ZdAz dummy = fBending(dummy1);
    766863        dest = CorrectTarget(GetSePos(), dummy); // [se]
    767864
    … …  
    772869        ZdAz max = fBending(fMax/kRad2Deg)*kRad2Deg;
    773870
     871        /*
    774872        if (d.Zd()<min.Zd() || d.Az()<min.Az())
    775873        {
    … …  
    784882            break;
    785883        }
    786 
     884        */
    787885        ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.;  // [re/min]
    788886
    … …  
    883981    // set deceleration to 50%
    884982    //
    885     cout << "Stopping movement..." << endl;
    886     fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
    887     fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
    888 
     983    cout << "Stopping movement (dec=20%)..." << endl;
     984
     985    fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
     986    fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
    889987    fMac1->SetRpmMode(FALSE);
    890988    fMac2->SetRpmMode(FALSE);
    891989
     990/*
     991    fMac1->SetDeceleration(0.3*fMac1->GetVelRes());
     992    fMac2->SetDeceleration(0.3*fMac2->GetVelRes());
     993
     994    fMac2->SendSDO(0x3000, Macs::string('s','t','o','p'));
     995    fMac1->SendSDO(0x3000, Macs::string('s','t','o','p'));
     996    fMac2->WaitForSdo(0x3000, 0);
     997    fMac1->WaitForSdo(0x3000, 0);
     998    fMac1->SetRpmMode(FALSE);
     999    fMac2->SetRpmMode(FALSE);
     1000    */
     1001
    8921002    //
    8931003    // Wait for the movement to really be finished.
    8941004    //
    895     cout << "Waiting for silence..." << endl;
     1005    cout << "Waiting for end of movement..." << endl;
    8961006    WaitForEndMovement();
     1007
     1008    //
     1009    // Check whether everything works fine.
     1010    //
     1011    CheckForError();
     1012    cout << "Movement stopped." << endl;
     1013}
     1014
     1015void MCosy::StopTracking()
     1016{
     1017    //
     1018    // Set status to Stopping
     1019    //
     1020    SetStatus(MCosy::kStopping);
     1021
     1022    //
     1023    // set deceleration to 50%
     1024    //
     1025    cout << "Stopping tracking (dec=20%)..." << endl;
     1026    fMac1->SetDeceleration(0.2*fMac1->GetVelRes());
     1027    fMac2->SetDeceleration(0.2*fMac2->GetVelRes());
     1028
     1029    fMac2->SendSDO(0x3006, 1, (LWORD_t)0);  // SetRpmVelocity [re/min]
     1030    fMac1->SendSDO(0x3006, 1, (LWORD_t)0);  // SetRpmVelocity [re/min]
     1031    fMac2->WaitForSdo(0x3006, 1);
     1032    fMac1->WaitForSdo(0x3006, 1);
     1033
     1034    cout << "Waiting for end of movement..." << endl;
     1035    WaitForEndMovement();
     1036
     1037    //
     1038    // Wait for the objects to be OKed.
     1039    //
     1040    fMac1->SetRpmMode(FALSE);
     1041    fMac2->SetRpmMode(FALSE);
     1042
     1043    //
     1044    // Wait for the movement to really be finished.
     1045    //
     1046    //cout << "Waiting for end of movement..." << endl;
     1047    //WaitForEndMovement();
    8971048
    8981049    //
    … …  
    9981149
    9991150            cout << "     Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
    1000             *tpout << za.Az() << " " << za.Alt() << " ";
     1151            *tpout << setprecision(7) << za.Az() << " " << za.Alt() << " ";
    10011152
    10021153            ZdAz sepos = GetSePos()*TMath::Pi()*2/16384;;
    … …  
    10051156
    10061157            cout << "     SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
    1007             *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << endl;
     1158            *tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << " ";
     1159            *tpout << rd.Ra()/15 << " " << rd.Dec() << " " << setprecision(11) << sla.GetMjd() << endl;
    10081160
    10091161            cout << "WM_TPoint: done. (return 0xaffe)" << endl;
    … …  
    11551307
    11561308    cout << "Reading telescope range..." << flush;
    1157     const Double_t amin = env.GetValue("Az_Min[deg]", -74.5);
    1158     const Double_t zmin = env.GetValue("Zd_Min[deg]", -90.0);
     1309    const Double_t amin = env.GetValue("Az_Min[deg]", -90.0);
     1310    const Double_t zmin = env.GetValue("Zd_Min[deg]", -74.5);
    11591311    fMin.Set(zmin, amin);
    11601312
    … …  
    12211373    const int res = fMac3->GetVelRes();
    12221374
    1223     fMac3->SetVelocity(res);
    1224     fMac3->SetAcceleration(res);
    1225     fMac3->SetDeceleration(res);
     1375    fMac3->SetVelocity(0.3*res);
     1376    fMac3->SetAcceleration(0.2*res);
     1377    fMac3->SetDeceleration(0.2*res);
    12261378    fMac3->StartPosSync();
    12271379}
    … …  
    18572009    while (1)
    18582010    {
    1859         sprintf(name, "/home/tbretz/TPoint/tpoint%03d.txt", i++);
     2011        sprintf(name, "tpoint/tpoint%03d.txt", i++);
    18602012        if (gSystem->AccessPathName(name, kFileExists))
    18612013            break;
    … …  
    18642016    Timer time;
    18652017    time.Now();
     2018
     2019    cout << "TPoint File ********* " << name << " ********** " << endl;
    18662020
    18672021    tpout = new ofstream(name);

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

    r1813 r1817  
    2929#define WM_GEAR         0x100c
    3030#define WM_DISPLAY      0x100d
    31 
    3231
    3332class ShaftEncoder;
    … …  
    133132    Bool_t RequestRePos();
    134133    Bool_t SetVelocity(const ZdAz &v);
    135     void SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc);
     134
     135    void SetPosVelocity(const Float_t ratio, Float_t vel);
    136136
    137137    void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
    … …  
    149149    void DisplayHistGear(Bool_t del=kTRUE);
    150150
    151     int  SetPosition(const ZdAz &dst);
     151    int  SetPosition(const ZdAz &dst, Bool_t track=kFALSE);
    152152
    153153    void TerminateApp();
    … …  
    159159
    160160    void StopMovement();
     161    void StopTracking();
     162
    161163    void WaitForEndMovement();
    162164
Note: See TracChangeset for help on using the changeset viewer.