Ignore:
Timestamp:
01/14/03 12:08:46 (22 years ago)
Author:
tbretz
Message:
*** empty log message ***
File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r1702 r1703  
    4141//const XY kGearRatio2(GEAR_RATIO_ALT*RES_RE/360.0,  GEAR_RATIO_AZ*RES_RE/360.0);    //[re/deg]
    4242
     43/* +===================================+
     44    FIXME: What if fMac3 (Sync) died?
     45   +===================================+
     46*/
     47
    4348double MCosy::Rad2SE(double rad) const
    4449{
    … …  
    150155    // Get the values
    151156    //
    152     const int p0 = !fZd1->IsZombieNode() ? fZd1->GetPos() : 0;
    153     const int p1 = !fZd2->IsZombieNode() ? fZd2->GetPos() : 0;
    154     const int p2 = !fAz->IsZombieNode()  ? fAz->GetPos()  : 0;
    155 
    156     const int a0 = p0; //p0>8192?p0-16384:p0;
    157     const int a1 = p1; //p1>8192?p1-16384:p1;
    158     const int a2 = p2; //p2>8192?p2-16384:p2;
     157    const int p0 = fZd1->GetPos();
     158    const int p1 = fZd2->GetPos();
     159    const int p2 = fAz->GetPos();
    159160
    160161    //
    161162    // interpolate shaft encoder positions
    162163    //
    163     const float a = (float)(a0-a1)/2;
     164    const float p = (float)(p0-p1)/2;
    164165
    165166    //
    166167    // calculate 'regelabweichung'
    167168    //
    168     return ZdAz(a, a2);
     169    return ZdAz(p, p2);
    169170}
    170171
    … …  
    177178Bool_t MCosy::RequestRePos()
    178179{
    179     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    180         return kTRUE;
    181 
    182180    //
    183181    // Send request
    … …  
    189187    // Wait until the objects are received.
    190188    //
     189    // FIXME, what when waiting times out (Zombie)
    191190    WaitForSdos();
    192191
    … …  
    194193    // If waiting was not interrupted everything is ok. return.
    195194    //
    196     if (!StopWaitingForSDO())
     195    if (!StopWaitingForSDO() && !HasZombie())
    197196        return kTRUE;
    198197
    … …  
    216215ZdAz MCosy::GetRePos()
    217216{
    218     return !fMac1->IsZombieNode() && !fMac2->IsZombieNode() ? ZdAz(fMac2->GetPos(), fMac1->GetPos()) : ZdAz(0,0);
     217    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
    219218}
    220219
    … …  
    304303int MCosy::StopWaitingForSDO() const
    305304{
    306     return Break() || HasError() || HasZombie();
     305    return Break() || HasError();
    307306}
    308307
    … …  
    316315void MCosy::WaitForEndMovement()
    317316{
     317    // FIXME, what when waiting times out (Zombie)
    318318    WaitForSdos();
    319319
    320     while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO())
     320    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO() && !HasZombie())
    321321        usleep(1);
    322322}
    … …  
    378378    // FIXME: Correct by fOffset ?
    379379
     380    /*
    380381    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    381382    {
    … …  
    383384        return TRUE;
    384385    }
     386    */
     387
     388    //
     389    // Make sure that the motors are in sync mode (necessary if the
     390    // MACS has been rebooted from a Zombie state.
     391    //
     392    InitSync();
     393    if (fMac3->IsZombieNode())
     394        return false;
    385395
    386396    //
    387397    // Calculate new target position (shortest distance to go)
    388398    //
    389     const ZdAz src = GetSePos();//*TMath::Pi()*2/16384;;
     399    const ZdAz src = GetSePos();
    390400
    391401    //
    … …  
    404414    cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
    405415
    406     for (int i=0; i<10 && !StopWaitingForSDO(); i++)
     416    for (int i=0; i<10 && !StopWaitingForSDO() && !HasZombie(); i++)
    407417    {
    408418        //
    … …  
    450460
    451461        rd.Round();
     462
     463        // FIXME? Check for Error or Zombie?
    452464
    453465        /*
    … …  
    477489// in case of success, kFALSE in case of failure.
    478490//
    479 Bool_t MCosy::SetVelocity(ZdAz v)
     491Bool_t MCosy::SetVelocity(const ZdAz &v)
    480492{
    481493    //
    … …  
    488500    // Wait for the objects to be OKed.
    489501    //
     502    // FIXME, what when waiting times out (Zombie)
    490503    WaitForSdos();
    491504
    … …  
    493506    // If the waiting for the objects wasn't interrupted return kTRUE
    494507    //
    495     if (!StopWaitingForSDO())
     508    if (!StopWaitingForSDO() && !HasZombie())
    496509        return kTRUE;
    497510
    … …  
    513526// revolution mode.
    514527//
    515 void MCosy::InitTracking()
    516 {
     528bool MCosy::InitTracking()
     529{
     530    // FIXME? Handling of Zombie OK?
    517531    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    518         return;
     532        return false;
    519533
    520534    //
    … …  
    523537    fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
    524538    fMac2->SetDeceleration(0.90*fMac2->GetVelRes());
     539    if (fMac2->IsZombieNode())
     540        return false;
    525541
    526542    fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
    527543    fMac1->SetDeceleration(0.90*fMac1->GetVelRes());
     544    if (fMac1->IsZombieNode())
     545        return false;
    528546
    529547    SetStatus(MCosy::kMoving | MCosy::kTracking);
    530548
    531549    fMac2->SetRpmMode(TRUE);
     550    if (fMac2->IsZombieNode())
     551        return false;
     552
    532553    fMac1->SetRpmMode(TRUE);
     554    if (fMac1->IsZombieNode())
     555        return false;
     556
     557    return true;
    533558}
    534559
    … …  
    640665    // Init accelerations and Rpm Mode
    641666    //
    642     InitTracking();
     667    if (!InitTracking())
     668        return;
    643669
    644670    XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
    … …  
    655681    //
    656682    fRaDec = dst;
    657 
    658     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    659         return;
    660 
    661683    fTracking = kTRUE;
    662684
    … …  
    671693    //
    672694    const float dt = 1;  // 1 second
    673     while (!StopWaitingForSDO())
     695    while (!StopWaitingForSDO() && !HasZombie())
    674696    {
    675697        //
    … …  
    774796void MCosy::StopMovement()
    775797{
    776     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    777         return;
    778 
    779798    //
    780799    // Set status to Stopping
    … …  
    785804    // set deceleration to 50%
    786805    //
    787     cout << "Stopping  positioning..." << endl;
     806    cout << "Stopping..." << endl;
    788807    fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
     808    if (!fMac1->IsZombieNode())
     809        fMac1->SetRpmMode(FALSE);
     810
    789811    fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
    790 
    791     //
    792     // Stop revolution mode (movement)
    793     //
    794     cout << "Stopping possibleRPM mode..." << endl;
    795     fMac1->SetRpmMode(FALSE);
    796     fMac2->SetRpmMode(FALSE);
     812    if (!fMac2->IsZombieNode())
     813        fMac2->SetRpmMode(FALSE);
    797814
    798815    //
    … …  
    809826}
    810827
    811 void *MCosy::Proc(int msg, void *mp)
    812 {
    813     lout << "Checking for Zombies" << endl;
     828bool MCosy::CheckNetwork()
     829{
     830    CheckConnections();
    814831    if (HasZombie())
    815832    {
    816         lout << "Found Zombies" << endl;
     833        lout << "- Found Zombies in Network..." << endl;
    817834        if (!RebootZombies())
    818             return (void*)0xebb0;
     835            return false;
    819836    }
    820 
     837    return true;
     838}
     839
     840void *MCosy::Proc(int msg, void *mp)
     841{
    821842    switch (msg)
    822843    {
    … …  
    827848    case WM_STOP:
    828849        cout << "MCosy::Proc: Stop." << endl;
     850        if (!CheckNetwork())
     851            return (void*)0xebb0;
    829852        StopMovement();
    830853        return NULL;
    … …  
    832855    case WM_PRESET:
    833856        cout << "WM_Preset: start." << endl;
    834         if (!fZd1->IsZombieNode()) fZd1->SetPreset();
    835         if (!fZd2->IsZombieNode()) fZd2->SetPreset();
    836         if (!fAz->IsZombieNode())  fAz->SetPreset();
     857        if (!CheckNetwork())
     858            return (void*)0xebb0;
     859        fZd1->SetPreset();
     860        fZd2->SetPreset();
     861        fAz->SetPreset();
    837862        cout << "WM_Preset: done. (return 0xaffe)" << endl;
    838863        return (void*)0xaffe;
    … …  
    841866        {
    842867            cout << "WM_Calib: start." << endl;
     868            if (!CheckNetwork())
     869                return (void*)0xebb0;
     870
    843871            SlaStars sla;
    844872            sla.SetMjd2Now();
    … …  
    858886            cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
    859887
    860             if (!fZd1->IsZombieNode())
    861                 fZd1->SetPreset(za.Zd());
    862             if (!fZd2->IsZombieNode())
    863                 fZd2->SetPreset(-za.Zd());
    864             if (!fAz->IsZombieNode())
    865                 fAz->SetPreset(za.Az());
     888            fZd1->SetPreset(za.Zd());
     889            fZd2->SetPreset(-za.Zd());
     890            fAz->SetPreset(za.Az());
    866891
    867892            cout << "WM_Calib: done. (return 0xaffe)" << endl;
    … …  
    897922        cout << "WM_Position: start." << endl;
    898923        {
     924            if (!CheckNetwork())
     925                return (void*)0xebb0;
     926
    899927            ZdAz dest = *((ZdAz*)mp);
    900 
    901928            SetPosition(dest*kDeg2Rad);
    902929        }
    … …  
    907934        cout << "WM_Track: START" << endl;
    908935        {
     936            if (!CheckNetwork())
     937                return (void*)0xebb0;
     938
    909939            RaDec dest = *((RaDec*)mp);
    910940            TrackPosition(dest*kDeg2Rad);
    … …  
    933963    case WM_HOME:
    934964        cout << "WM_Home: START" << endl;
    935         if (!fMac1->IsZombieNode() && !fMac2->IsZombieNode())
     965        if (!CheckNetwork())
     966            return (void*)0xebb0;
     967        else
    936968        {
    937969            cout << "Going Home..." << endl;
    … …  
    9761008    case WM_QUIT:
    9771009        cout << "WM_Quit: now." << endl;
     1010        if (!CheckNetwork())
     1011        {
     1012            lout << "ERROR: Cannot shutdown CANbus network." << endl;
     1013            return (void*)0xebb0;
     1014        }
    9781015        TerminateApp();
    9791016        cout << "WM_Quit: done." << endl;
    … …  
    10041041        resreaz = fMac1->GetRes();
    10051042    else
    1006         if (!fMac3->IsZombieNode())
     1043        if (fMac3 && !fMac3->IsZombieNode())
    10071044            resreaz = fMac3->GetRes();
    10081045        else
    … …  
    10401077}
    10411078
     1079void MCosy::InitSync()
     1080{
     1081    if (!fMac3)
     1082        return;
     1083
     1084    const int res = fMac3->GetVelRes();
     1085
     1086    fMac3->SetVelocity(res);
     1087    fMac3->SetAcceleration(res);
     1088    fMac3->SetDeceleration(res);
     1089    fMac3->StartPosSync();
     1090}
     1091
    10421092void MCosy::TalkThread()
    10431093{
    1044     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
     1094    /* ========== FIXME? =============
     1095     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
    10451096        return;
    1046 
     1097     */
    10471098    fMac1->ReqPos();
    10481099    fMac2->ReqPos();
    1049 
    1050     if (fMac3)
    1051     {
    1052         const int res = fMac3->GetVelRes();
    1053 
    1054         fMac3->SetVelocity(res);
    1055         fMac3->SetAcceleration(res);
    1056         fMac3->SetDeceleration(res);
    1057         fMac3->StartPosSync();
    1058     }
    10591100
    10601101    if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
    … …  
    12271268    // Update Gui, foremer MTGui.
    12281269    //
    1229     if (!fZd1->IsZombieNode()) fZd1->DisplayVal();
    1230     if (!fZd2->IsZombieNode()) fZd2->DisplayVal();
    1231     if (!fAz->IsZombieNode())  fAz->DisplayVal();
     1270    fZd1->DisplayVal();
     1271    fZd2->DisplayVal();
     1272    fAz->DisplayVal();
    12321273
    12331274    ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
    … …  
    12381279    avail |= !fMac1->IsZombieNode() ? 0x01 : 0;
    12391280    avail |= !fMac2->IsZombieNode() ? 0x02 : 0;
    1240     avail |= !fMac3->IsZombieNode() ? 0x04 : 0;
     1281    avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
    12411282    avail |= !fZd1->IsZombieNode()  ? 0x08 : 0;
    12421283    avail |= !fZd2->IsZombieNode()  ? 0x10 : 0;
    … …  
    12681309    // Don't call this function twice!
    12691310    Network::Start();
    1270 /*
    1271     if (fMac1)
    1272         if (fMac1->IsZombieNode()) { delete fMac1; fMac1=NULL; }
    1273     if (fMac2)
    1274         if (fMac2->IsZombieNode()) { delete fMac2; fMac2=NULL; }
    1275     if (fMac3)
    1276         if (fMac3->IsZombieNode()) { delete fMac3; fMac3=NULL; }
    1277 */
    1278 /*    if (fZd1)
    1279         if (fZd1->IsZombieNode())  { delete fZd1;  fZd1=NULL; }
    1280         else */fZd1->SetDisplay(fWin->GetLabel2());
    1281 
    1282 /*    if (fZd2)
    1283         if (fZd2->IsZombieNode())  { delete fZd2;  fZd2=NULL; }
    1284         else */fZd2->SetDisplay(fWin->GetLabel3());
    1285 
    1286 /*    if (fAz)
    1287         if (fAz->IsZombieNode())   { delete fAz;   fAz=NULL; }
    1288         else */fAz->SetDisplay(fWin->GetLabel1());
    12891311
    12901312    ReadConfig();
    … …  
    12921314    lout << "- Starting TX Thread." << endl;
    12931315    fTTalk = new MTTalk(this);
    1294     //    fTGui = new MTGui(this);
    12951316
    12961317    lout << "- Starting GUI update." << endl;
    … …  
    13611382    lout << "- Starting GUI." << endl;
    13621383    fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
    1363 
    1364     fAz->SetDisplay(fWin->GetLabel1());
    1365     fZd1->SetDisplay(fWin->GetLabel2());
    1366     fZd2->SetDisplay(fWin->GetLabel3());
    1367 
    1368     lout.SetOutputGui(fWin->GetLog(), kTRUE);
    13691384}
    13701385
    … …  
    13971412    lout << "- Starting GUI." << endl;
    13981413    fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
    1399 
    1400     fAz->SetDisplay(fWin->GetLabel1());
    1401     fZd1->SetDisplay(fWin->GetLabel2());
    1402     fZd2->SetDisplay(fWin->GetLabel3());
    1403 
    1404     lout.SetOutputGui(fWin->GetLog(), kTRUE);
    14051414}
    14061415
    … …  
    14201429    lout << "- Starting GUI." << endl;
    14211430    fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
    1422 
    1423     lout.SetOutputGui(fWin->GetLog(), kTRUE);
    14241431}
    14251432
    … …  
    14511458    }
    14521459
     1460    lout.SetOutputGui(fWin->GetLog(), kTRUE);
     1461
     1462    fZd1->SetDisplay(fWin->GetLabel2());
     1463    fZd2->SetDisplay(fWin->GetLabel3());
     1464    fAz->SetDisplay(fWin->GetLabel1());
     1465
    14531466    int i=0;
    14541467    char name[100];
    … …  
    15031516    cout << "Deleting Nodes." << endl;
    15041517
    1505     if (fAz)   delete fAz;
    1506     if (fZd1)  delete fZd1;
    1507     if (fZd2)  delete fZd2;
    1508     if (fMac1) delete fMac1;
    1509     if (fMac2) delete fMac2;
    1510     if (fMac3) delete fMac3;
     1518    delete fAz;
     1519    delete fZd1;
     1520    delete fZd2;
     1521    delete fMac1;
     1522    delete fMac2;
     1523    if (fMac3)
     1524        delete fMac3;
    15111525
    15121526    cout << "Deleting MGCosy." << endl;
Note: See TracChangeset for help on using the changeset viewer.