Ignore:
Timestamp:
02/06/03 11:28:05 (22 years ago)
Author:
tbretz
Message:
*** empty log message ***
File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r1703 r1742  
    187187    // Wait until the objects are received.
    188188    //
     189    fMac2->WaitForSdo(0x6004);
     190    fMac1->WaitForSdo(0x6004);
     191
    189192    // FIXME, what when waiting times out (Zombie)
    190     WaitForSdos();
     193    // WaitForSdos();
    191194
    192195    //
    193196    // If waiting was not interrupted everything is ok. return.
    194197    //
    195     if (!StopWaitingForSDO() && !HasZombie())
     198    if (!(Break() || HasError()) && !HasZombie())
    196199        return kTRUE;
    197200
    … …  
    255258    {
    256259        fMac1->SetVelocity(vel);
     260        fMac2->SetVelocity(vel*ratio);
     261
    257262        fMac1->SetAcceleration(acc);
     263        fMac2->SetAcceleration(acc*ratio);
     264
    258265        fMac1->SetDeceleration(acc);
    259 
    260         fMac2->SetVelocity(vel*ratio);
    261         fMac2->SetAcceleration(acc*ratio);
    262266        fMac2->SetDeceleration(acc*ratio);
    263267    }
    … …  
    265269    {
    266270        fMac1->SetVelocity(vel/ratio);
     271        fMac2->SetVelocity(vel);
     272
    267273        fMac1->SetAcceleration(acc/ratio);
     274        fMac2->SetAcceleration(acc);
     275
    268276        fMac1->SetDeceleration(acc/ratio);
    269 
    270         fMac2->SetVelocity(vel);
    271         fMac2->SetAcceleration(acc);
    272277        fMac2->SetDeceleration(acc);
    273278    }
    … …  
    285290void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
    286291{
     292    if (HasZombie())
     293        return;
     294
    287295    SetStatus(MCosy::kMoving);
    288296
    … …  
    292300    cout << "Waiting for positioning..." << flush;
    293301
     302    if (axe1) fMac2->WaitForSdo(0x6004, 1);
     303    if (axe2) fMac1->WaitForSdo(0x6004, 1);
     304
    294305    WaitForEndMovement();
    295306
    … …  
    303314int MCosy::StopWaitingForSDO() const
    304315{
    305     return Break() || HasError();
     316    return 0/*Break() || HasError()*/;
    306317}
    307318
    … …  
    316327{
    317328    // FIXME, what when waiting times out (Zombie)
    318     WaitForSdos();
    319 
    320     while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO() && !HasZombie())
     329    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) &&
     330           !(Break() || HasError()) && !HasZombie())
    321331        usleep(1);
    322332}
    … …  
    345355    SetStatus(MCosy::kError);
    346356
    347     //
    348     // Now try to handle the error.
    349     //
    350     fMac1->HandleError();
    351     fMac2->HandleError();
    352 
    353     //
    354     // If the error couldn't get solved return
    355     //
    356     if (HasError())
    357         return;
    358 
    359     //
    360     // Set motor status to stopped
    361     //
    362     SetStatus(MCosy::kStopped);
     357    /*
     358     FIXME: HANDLINGE ERROR
     359
     360     //
     361     // Now try to handle the error.
     362     //
     363     fMac1->HandleError();
     364     fMac2->HandleError();
     365
     366     //
     367     // If the error couldn't get solved return
     368     //
     369     if (HasError())
     370     return;
     371
     372     //
     373     // Set motor status to stopped
     374     //
     375     SetStatus(MCosy::kStopped);
     376     */
    363377}
    364378
    … …  
    390404    // MACS has been rebooted from a Zombie state.
    391405    //
    392     InitSync();
    393     if (fMac3->IsZombieNode())
    394         return false;
     406    //InitSync();
     407    //if (fMac3->IsZombieNode())
     408    //    return false;
    395409
    396410    //
    397411    // Calculate new target position (shortest distance to go)
    398412    //
    399     const ZdAz src = GetSePos();
     413    const ZdAz src = GetSePos(); // [se]
    400414
    401415    //
    … …  
    405419    // const ZdAz dest = CorrectTarget(src, dst);
    406420    //
    407     fZdAzSoll = fBending(dst);
    408     const ZdAz dest = fZdAzSoll*16384/2/TMath::Pi();
     421    const ZdAz dest = fBending(dst)*16384/2/TMath::Pi(); // [se]
     422    fZdAzSoll = dst;
    409423
    410424    lout << "Positioning to Target..." << endl;
    … …  
    414428    cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
    415429
    416     for (int i=0; i<10 && !StopWaitingForSDO() && !HasZombie(); i++)
     430    for (int i=0; i<10 && !(Break() || HasError()) && !HasZombie(); i++)
    417431    {
    418432        //
    … …  
    500514    // Wait for the objects to be OKed.
    501515    //
     516
     517    fMac2->WaitForSdo(0x3006, 1);
     518    fMac1->WaitForSdo(0x3006, 1);
     519
    502520    // FIXME, what when waiting times out (Zombie)
    503     WaitForSdos();
     521    // WaitForSdos();
    504522
    505523    //
    506524    // If the waiting for the objects wasn't interrupted return kTRUE
    507525    //
    508     if (!StopWaitingForSDO() && !HasZombie())
     526    if (!(Break() || HasError()) && !HasZombie())
    509527        return kTRUE;
    510528
    … …  
    693711    //
    694712    const float dt = 1;  // 1 second
    695     while (!StopWaitingForSDO() && !HasZombie())
     713    while (!(Break() || HasError()) && !HasZombie())
    696714    {
    697715        //
    … …  
    706724        ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
    707725        dest = CorrectTarget(GetSePos(), dummy); // [se]
     726        dest *= kGearRatio;  // [re]
     727
     728        ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.;  // [re/min]
    708729
    709730        //
    … …  
    719740        // Shaft- and the rotary encoders
    720741        //
    721         dest *= kGearRatio;  // [re]
    722742        dest -= GetRePos() + fOffset;
    723743
    … …  
    733753        //
    734754        ZdAz vt = v/4;
    735         ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.;  // [re/min]
    736755        LimitSpeed(&vt, vcalc);
    737756        vt.Round();
    … …  
    804823    // set deceleration to 50%
    805824    //
    806     cout << "Stopping..." << endl;
     825    cout << "Stopping movement..." << endl;
    807826    fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
    808     if (!fMac1->IsZombieNode())
    809         fMac1->SetRpmMode(FALSE);
    810 
    811827    fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
    812     if (!fMac2->IsZombieNode())
    813         fMac2->SetRpmMode(FALSE);
     828
     829    fMac1->SetRpmMode(FALSE);
     830    fMac2->SetRpmMode(FALSE);
    814831
    815832    //
    … …  
    828845bool MCosy::CheckNetwork()
    829846{
    830     CheckConnections();
     847    //CheckConnections();
    831848    if (HasZombie())
    832849    {
    … …  
    835852            return false;
    836853    }
     854
     855    /*
     856     FIXME HANDLING ERROR
     857     */
     858    if (HasError())
     859    {
     860        fMac1->HandleError();
     861        fMac2->HandleError();
     862        fMac3->HandleError();
     863        if (HasError() || HasZombie())
     864            return false;
     865    }
     866
    837867    return true;
    838868}
    … …  
    969999            cout << "Going Home..." << endl;
    9701000            TEnv env(".cosyrc");
     1001
     1002            SetStatus(MCosy::kMoving);
     1003
    9711004            fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
    9721005            fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
     1006
     1007            lout << "SETHOME DONE" << endl;
     1008
     1009            SetStatus(HasError() ? MCosy::kError : MCosy::kStopped);
    9731010
    9741011            fAz->SetPreset();
    … …  
    9781015            fMac1->ReqPos();
    9791016            fMac2->ReqPos();
     1017            fMac3->StopMotor();
    9801018        }
    9811019        cout << "WM_Home: done. (return 0x403e)" << endl;
    … …  
    10381076
    10391077    Double_t resreaz = 0;
    1040     if (!fMac1->IsZombieNode())
     1078    if (fMac1 && !fMac1->IsZombieNode())
    10411079        resreaz = fMac1->GetRes();
    10421080    else
    … …  
    10471085
    10481086    Double_t resrezd = 0;
    1049     if (!fMac2->IsZombieNode())
     1087    if (fMac2 && !fMac2->IsZombieNode())
    10501088        resrezd = fMac2->GetRes();
    10511089    else
    10521090        resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500);
    1053  
     1091
    10541092    Double_t ressezd = 0;
    1055     if (!fZd1->IsZombieNode())
     1093    if (fZd1 && !fZd1->IsZombieNode())
    10561094        ressezd = fZd1->GetPhysRes();
    10571095    else
    1058         if (!fZd2->IsZombieNode())
     1096        if (fZd2 && !fZd2->IsZombieNode())
    10591097            ressezd = fZd2->GetPhysRes();
    10601098        else
    … …  
    10621100
    10631101    Double_t resseaz = 0;
    1064     if (!fAz->IsZombieNode())
     1102    if (fAz && !fAz->IsZombieNode())
    10651103        resseaz = fAz->GetPhysRes();
    10661104    else
    … …  
    10801118{
    10811119    if (!fMac3)
     1120    {
     1121        lout << "Unable to Init Sync! Mac3 not available." << endl;
    10821122        return;
     1123    }
    10831124
    10841125    const int res = fMac3->GetVelRes();
    … …  
    10951136     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    10961137        return;
    1097      */
    1098     fMac1->ReqPos();
    1099     fMac2->ReqPos();
     1138        */
     1139
     1140    if (fMac1 && fMac2)
     1141    {
     1142        fMac1->ReqPos();
     1143        fMac2->ReqPos();
     1144    }
     1145
     1146    InitSync();
     1147
     1148    /*** FOR DEMO MODE ***/
     1149    if (!fZd1 || !fZd2 || !fAz)
     1150        return;
     1151    /*** FOR DEMO MODE ***/
    11001152
    11011153    if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
    11021154        return;
    11031155
     1156    if (!fMac1 || !fMac2)
     1157        return;
    11041158
    11051159    //
    11061160    // Start the Network
    11071161    //
    1108     //cout << "Reading configuration file..." << flush;
    1109     //TEnv env(".cosyrc");
    1110     //cout << "done." << endl;
    1111 /*
    1112     const int res = fMac3->GetVelRes();
    1113 
    1114     fMac3->SetVelocity(res);
    1115     fMac3->SetAcceleration(res);
    1116     fMac3->SetDeceleration(res);
    1117 
    1118     fMac1->ReqPos();
    1119     fMac2->ReqPos();
    1120 
    1121     //const ZdAz repos=GetRePos();
    1122     //cout << "APOS: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
    1123 
    1124     //cout << Deg2AzRE(env.GetValue("MinAz[Deg]", -1.0)) << " < Az < "
    1125     //</< Deg2AzRE(env.GetValue("MaxAz[Deg]", +1.0)) << "RE" << endl;
    1126     //cout << env.GetValue("MinAz[Deg]", -1.0) << " < Az < "
    1127     //<< env.GetValue("MaxAz[Deg]", +1.0) << kDEG << endl;
    1128     //cout << Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)) << "RE < Zd < "
    1129     //<< Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)) << "RE" << endl;
    1130 
    1131     cout << "Setting up software endswitch..." << flush;
    1132     fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0)));
    1133     fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0)));
    1134 
    1135     fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0)));
    1136     fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0)));
    1137     cout << "done." << endl;
    1138 
    1139     fMac1->EnableTimeout(kTRUE, 500);
    1140     fMac2->EnableTimeout(kTRUE, 500);
    1141 
    1142     //fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("MinZd[Deg]", -1.0)));
    1143     //fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("MaxZd[Deg]", +1.0)));
    1144     //    fMac3->StartVelSync();
     1162    /*
     1163     TEnv env(".cosyrc");
     1164
     1165     cout << "Setting up software endswitch..." << flush;
     1166     fMac1->SetNegEndswitch(Deg2AzRE(env.GetValue("Az_Min[Deg]", -1.0)));
     1167     fMac1->SetPosEndswitch(Deg2AzRE(env.GetValue("Az_Max[Deg]", +1.0)));
     1168
     1169     fMac2->SetNegEndswitch(Deg2ZdRE(env.GetValue("Zd_Min[Deg]", -1.0)));
     1170     fMac2->SetPosEndswitch(Deg2ZdRE(env.GetValue("Zd_Max[Deg]", +1.0)));
     1171     cout << "done." << endl;
    11451172    */
    11461173    SlaStars sla;
    … …  
    12201247            // FIXME: I cannot take the avarage
    12211248            //
    1222             time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
     1249            if (fZd1->GetMjd()>fZd2->GetMjd())
     1250                time.Zd(fZd1->GetMjd());
     1251            else
     1252                time.Zd(fZd2->GetMjd());
     1253            //time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
    12231254            time.Az(fAz->GetMjd());
    12241255
    … …  
    12471278            }
    12481279
    1249             fZdAzSoll.Set(sollzd.Zd(), sollaz.Az());
    1250             fZdAzSoll *= 2*TMath::Pi()/16384;
     1280            ZdAz soll(sollzd.Zd(), sollaz.Az());
     1281            fZdAzSoll = fBending.CorrectBack(soll*2*TMath::Pi()/16384);
    12511282
    12521283            fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
    … …  
    12681299    // Update Gui, foremer MTGui.
    12691300    //
    1270     fZd1->DisplayVal();
    1271     fZd2->DisplayVal();
    1272     fAz->DisplayVal();
     1301    if (fZd1)
     1302        fZd1->DisplayVal();
     1303    if (fZd2)
     1304        fZd2->DisplayVal();
     1305    if (fAz)
     1306        fAz->DisplayVal();
    12731307
    12741308    ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
    … …  
    12771311    Byte_t avail = 0;
    12781312
    1279     avail |= !fMac1->IsZombieNode() ? 0x01 : 0;
    1280     avail |= !fMac2->IsZombieNode() ? 0x02 : 0;
     1313    avail |= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
     1314    avail |= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
    12811315    avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
    1282     avail |= !fZd1->IsZombieNode()  ? 0x08 : 0;
    1283     avail |= !fZd2->IsZombieNode()  ? 0x10 : 0;
    1284     avail |= !fAz->IsZombieNode()   ? 0x20 : 0;
     1316    avail |= (fZd1  && !fZd1->IsZombieNode())  ? 0x08 : 0;
     1317    avail |= (fZd2  && !fZd2->IsZombieNode())  ? 0x10 : 0;
     1318    avail |= (fAz   && !fAz->IsZombieNode())   ? 0x20 : 0;
     1319
     1320    if (HasError())
     1321        SetStatus(MCosy::kError);
     1322
     1323    lout.UpdateGui();
    12851324
    12861325    fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError/kGearRatio2,
    12871326                 fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail);
     1327
    12881328
    12891329    /*
    … …  
    14471487    case 0:
    14481488        lout << "<<Stanard mode>>" << endl;
     1489        fBending.Load("bending.txt");
    14491490        Constructor(id1, id2, id3, id4, id5, id6);
    14501491        break;
    14511492    case 1:
    14521493        lout << "<<SE mode>>" << endl;
     1494        fBending.Load("bending.txt");
    14531495        ConstructorSE(id4, id5, id6);
    14541496        break;
Note: See TracChangeset for help on using the changeset viewer.