Changeset 8875 for trunk/MagicSoft/Cosy


Ignore:
Timestamp:
02/24/08 11:26:09 (17 years ago)
Author:
tbretz
Message:
*** empty log message ***
Location:
trunk/MagicSoft/Cosy
Files:
9 edited

Legend:

Unmodified
Added
Removed

  • trunk/MagicSoft/Cosy/Changelog

    r8874 r8875  
    3030   * videodev/VideodevIncl.h:
    3131     - added TVector2
     32
     33   * aposs/Magic.m:
     34     - added GRB mode
     35     - added a division by four to the setting of the rpm mode velocity
    3236
    3337

  • trunk/MagicSoft/Cosy/aposs/Magic.m

    r8872 r8875  
    216216/* OLD ORIGINAL VALUES: Prop=100, Div=300, Int=1000 */
    217217if (get cannr==1) then
    218 SET KPROP  950
    219 SET KDER  2000
    220 SET KINT     0
     218SET KPROP  100 /*950*/
     219SET KDER   300 /*2000*/
     220SET KINT  1000 /*0*/
    221221elseif (get cannr==2) then
    222222SET KPROP  100
    … …  
    329329kIoModule = 4*256                   
    330330
    331 ON INT -(kIoModule+6) GOSUB DOOR_STATUS_CHANGED
    332 ON INT  (kIoModule+6) GOSUB DOOR_STATUS_CHANGED
    333 
    334331/*-------------------------------------------------------------------------*/
    335332/* Init                                                                    */
    … …  
    350347   
    351348   init  = 0
     349
     350   oldstate = kTRUE
    352351
    353352   gosub reset
    … …  
    361360      else
    362361         fuse  = in (kIoModule+1)
    363          emcy  = in (kIoModule+2)
     362         emcy  = in (kIoModule+2)                             
    364363         vltg  = in (kIoModule+3)
    365364         mode  = in (kIoModule+4)
    366365         door  = in (kIoModule+6)
    367366         ready = in 1
    368 
     367                                     
    369368         if (door==0) then
    370369            preop = kTRUE
    371          endif
    372 
    373          if (preop==kTRUE) then
    374             print "Door open!"
    375             gosub reset
    376          elseif (ready==0) and (RF==1) then
     370         endif                         
     371
     372         if (ready==0) and (RF==1) then
    377373            print "DKC not ready, but RF set... setting RF=AH=0!"
    378374            gosub reset
    … …  
    438434         endif         
    439435      endif
    440 
     436       
     437      if (preop==kTRUE and oldstate!=preop) then
     438          print "Door switch changed state!"
     439          gosub reset
     440      endif
     441      oldstate = preop
     442       
    441443      rc = CANIN sdorx -1 0 canhi canlo
    442444   goto mainloop
    … …  
    455457   SUBPROG reset
    456458      init = 0
    457      
     459               
    458460      out 1 0
    459461      out 2 0   
    … …  
    461463      motor off   
    462464      waitt 1000
    463    
     465
    464466      if (brake==1 and get cannr==3) then         
    465467         waitt 3000          /* wait 3s for DKC to stop the motor */
    … …  
    467469         waitt 1000
    468470      endif   
    469      
     471
    470472      canout pdo3 (ready | (fuse<<1) | (emcy<<2) | (vltg<<3) | (mode<<4)) (rf | (brake<<1))
    471473   return
    … …  
    482484      PRINT "Setting Idx:", idx, "/", subidx, " to ", sdoval
    483485*/
    484       if (idx==0x1000 and subidx==1) then
    485          if (sdoval==1) then
    486             if (door==0) then
    487                canlo = 1
    488             else
    489                preop = kFALSE
    490             endif
    491          elseif
    492             preop = kTRUE
     486      if (idx==0x1000 and subidx==1) then   
     487         preop = kTRUE
     488         if (sdoval==1 and door==1) then
     489             preop = kFALSE
     490         else
     491             canlo = 1
    493492         endif
    494493      elseif (idx==0x1003 and subidx==0 and sdoval==0) then
    … …  
    537536      elseif (idx == 0x2003) then
    538537        if (subidx) then
    539           DEC sdoval
     538          /* If velocity mode active */
     539          /*if (AXEND&4)
     540             DEC sdoval%4
     541          else*/
     542             DEC sdoval
     543          /*endif*/
    540544        else
    541           ACC sdoval
     545          /*if (AXEND&4)
     546             ACC sdoval%4
     547          else*/
     548             ACC sdoval
     549          /*endif*/
    542550        endif
    543551      elseif (idx == 0x3000) then
    … …  
    584592         elseif (subidx == 1) then
    585593            /* It is unclear where this arbitraty factor comes from */
    586             CVEL sdoval/4
     594            CVEL sdoval%4
    587595         endif
    588596      elseif (idx == 0x3007) then
    … …  
    810818/*-------------------------------------------------------------------------*/
    811819   SUBPROG PROC_PDO1
    812       CANOUT pdo1 (AXEND|(preop<<9)) APOS
     820      CANOUT pdo1 (AXEND|(((~preop)&1)<<8)) APOS
    813821   RETURN
    814822

  • trunk/MagicSoft/Cosy/cosy.cc

    r8869 r8875  
    196196        */
    197197
    198     MDriveCom com(ceco, ceco_tx, ceco_rx);
    199 
    200     MCosy *cosy = new MCosy(env, &com, pointing);
    201 
    202     com.SetMsgQueue(cosy);
     198    MDriveCom *com = new MDriveCom(ceco, ceco_tx, ceco_rx);
     199
     200    MCosy *cosy = new MCosy(env, com, pointing);
     201
     202    com->SetMsgQueue(cosy);
    203203
    204204    Interface *interface = new Ethernet(sps, 5357, 5358, cosy);
    … …  
    252252    delete cosy;
    253253
     254    gLog << all << "Deleting DriveCom at " << MTime(-1) << endl;
     255    delete com;
     256
    254257    if (kDebugThreads)
    255258        TThread::Ps();

  • trunk/MagicSoft/Cosy/devdrv/macs.cc

    r8870 r8875  
    1212
    1313using namespace std;
     14
     15//#define EXPERT
    1416
    1517/*
    … …  
    514516    //StartHostGuarding();
    515517
    516     // REMOVE THIS AND LET CC START THE NODE
    517     // StartNode();
     518#ifdef EXPERT
     519    StartNode();
     520#endif
    518521
    519522    gLog << inf2 << "- " << GetNodeName() << ": Checking armed status." << endl;
    … …  
    643646    }
    644647
    645     gLog << dbg << GetNodeName() << ": Starting abolsute positioning to " << (LWORDS_t)pos << " ticks." << endl;
     648    gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl;
    646649    SendSDO(0x6004, 1, (LWORD_t)pos);
    647650    fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO
    … …  
    657660    }
    658661
    659     gLog << dbg << GetNodeName() << ": Starting relative positioning by " << (LWORDS_t)pos << " ticks." << endl;
     662    gLog << dbg << GetNodeName() << ": Starting absolute positioning to " << (LWORDS_t)pos << " ticks." << endl;
    660663    SendSDO(0x6004, 0, (LWORD_t)pos);
    661664    fPosActive = kTRUE; // Make sure that the status is set correctly already before the first PDO

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

    r8865 r8875  
    406406
    407407//#ifdef EXPERT
    408     //point.SetPointAccDec(0.3, 0.3);
    409     //point.SetPointVelocity(0.3);
     408//    point.SetPointAccDec(0.25, 0.25);
     409//    point.SetPointVelocity(0.4);
    410410//#else
    411411
    … …  
    449449    track.SetPointVelocity(0.1);
    450450
    451     //    track.SetPointAccDec(0.4, 0.4);
     451    // track.SetPointAccDec(0.4, 0.4);
    452452    // track.SetPointVelocity(0.4);
    453453
    … …  
    699699                return 0xebb0;
    700700
    701             //const bool arm = *((bool*)mp);
    702             //if (arm)
    703             //{
     701            const bool arm = mp ? *((bool*)mp) : true;
     702            if (arm)
     703            {
    704704                fMac1->Arm();
    705705                fMac2->Arm();
    706                 //cout << "ARMED" << endl;
    707             //}
    708             //else
    709             //{
    710             //    fMac1->Disarm();
    711             //    fMac2->Disarm();
    712                 //cout << "DISARMED" << endl;
    713             //}
     706            }
     707            else
     708            {
     709                fMac1->Disarm();
     710                fMac2->Disarm();
     711            }
    714712        }
    715713        //cout << "WM_Position: done. (return 0x7777)" << endl;
    … …  
    12901288    //cout << (fStatus&MDriveCom::kTracking?"TRA: ":"POS: ") << bendist.Zd() << " " << bendist.Az() << endl;
    12911289
    1292     fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError);
     1290    fCom->SendReport(fStatus, fRaDec, fZdAzSoll, bendist, fTrackingError, armed);
    12931291
    12941292    fWin->UpdateWeather(*fCom);
    … …  
    16141612}
    16151613
    1616 MCosy::MCosy(MEnv &env, const char *addr, const char *pointing)
     1614MCosy::MCosy(MEnv &env, MDriveCom *com, const char *pointing)
    16171615: Network(), fObservatory(MObservatory::kMagic1), fStarguider(NULL),
    16181616fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), /*fMac3(0),*/
    … …  
    16651663    fAz->SetDisplay(fWin->GetLabel1());
    16661664
    1667     fCom = new MDriveCom(this, addr, fOutRep);
    1668 //    fCom->Start();
     1665    fCom = com;//new MDriveCom(this, addr, tx, rx, fOutRep);
     1666    fCom->SetOutRep(fOutRep);
     1667    //    fCom->Start();
    16691668}
    16701669
    … …  
    16911690MCosy::~MCosy()
    16921691{
     1692    if(fCom)
     1693    {
     1694        fCom->SetMsgQueue(NULL);
     1695        fCom->SetOutRep(NULL);
     1696    }
     1697
    16931698    gLog << inf2 << "Deleting GUI timer." << endl;
    16941699    // FIXME: Wait until last Update was finished!!!
    … …  
    17041709        delete fOutTp;
    17051710    }
     1711
    17061712    delete fOutRep;
    17071713
    1708     gLog << inf2 << "Deleting CC communication." << endl;
    1709     delete fCom;
     1714    //gLog << inf2 << "Deleting CC communication." << endl;
     1715    //delete fCom;
    17101716
    17111717    gLog << inf2 << "Deleting Nodes." << endl;

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

    r8865 r8875  
    181181
    182182public:
    183     MCosy(MEnv &env, const char *addr, const char *pointing);
     183    MCosy(MEnv &env, MDriveCom *com, const char *pointing);
    184184    ~MCosy();
    185185
    … …  
    212212    MLog *GetOutRep() { return fOutRep; }
    213213
    214     //rwagner
    215214    MDriveCom *GetDriveCom() { return fCom; }
    216215

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

    r8824 r8875  
     1#include "MSlewing.h"
    12#include "MSlewing.h"
    23
    … …  
    78#include "MDriveCom.h"
    89
     10#include "MString.h"
    911#include "MPointing.h"
    1012
    … …  
    165167        // Round to check whether we are as near as possible
    166168        // to the value we expect
    167         cd.Round();
    168 
     169
     170        cout << cd.Zd() << " " << cd.Az() << endl;
     171
     172        cd /= fCosy->kResSE; // Scale to Revolutions
     173
     174        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
     185        cd.Abs();
     186
     187        // FIXME:: Do NOT check Shaftencoder values!!!
    169188        // Check if there is a control deviation on the axis
    170         const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
    171         const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
     189        const Bool_t cdzd = cd.Zd()>0.5 ? kTRUE : kFALSE;
     190        const Bool_t cdaz = cd.Az()>0.5 ? kTRUE : kFALSE;
     191
     192        cout << (int)cdzd << " " << (int)cdaz << endl;
    172193
    173194        // check if we reached the correct position already
    … …  
    183204        // ==============================================
    184205        //   Estimate the noncircularity of the zd axis
    185         const ZdAz ist = dst-rd*TMath::TwoPi()/fCosy->kResSE;
    186 
    187         const double p1 = ist.Zd()-19.0605/kRad2Deg;
    188         const double p2 = dst.Zd()-19.0605/kRad2Deg;
    189 
    190         const double f1 = (-26.0101*sin(p1)+443.761*ist.Zd())*rad2se;
    191         const double f2 = (-26.0101*sin(p2)+443.761*dst.Zd())*rad2se;
    192         // ==++=========================================
     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
    193217
    194218        // change units from se to re
    195219//        rd *= fCosy->kGearTot/fCosy->kResSE; // [re]
    196220        rd *= Div(fCosy->kGearTot,fCosy->kResSE); // [re]
    197         rd.Zd(f2-f1);
     221//        rd.Zd(f2-f1);
    198222
    199223        // Initialize Velocities so that we reach both positions
    … …  
    201225        if (i)
    202226        {
    203             //lout << "--- LO-SPEED ---" << endl;
    204             SetAccDec(fCosy->fMac1, 0.1, 0.1);
    205             SetAccDec(fCosy->fMac2, 0.1, 0.1);
     227            //cout << "--- LO-SPEED ---" << endl;
     228            SetAccDec(fCosy->fMac1, 0.05, 0.05);
     229            SetAccDec(fCosy->fMac2, 0.05, 0.05);
    206230
    207231            SetPosVelocity(1.0, 0.05);
    … …  
    209233        else
    210234        {
     235            // 15 motor revolutions
    211236            const Double_t y = 15*fCosy->kGearTot.Y()/fCosy->kResSE.Y();
    212237
    … …  
    215240
    216241            if (rd.Az()>-y && rd.Az()<y)
    217               {
    218                 //lout << "--- LO-SPEED Mac1 ---" << endl;
    219242                SetAccDec(fCosy->fMac1, 0.05, 0.05);
    220               }
    221243            else
    222               {
    223 
    224                 //lout << "MSlewing::SetPosition SetAccDec Mac1: " << fAcc << fDec << endl;
    225                
    226                 SetAccDec(fCosy->fMac1, fAcc, fDec);
    227                
    228               }
    229 
    230 
    231             // new 16.05.05  F.G.
    232             // SetAccDec(fCosy->fMac1, fAcc, fDec);
     244                SetAccDec(fCosy->fMac1, fAcc, fDec);
    233245
    234246            SetAccDec(fCosy->fMac2, fAcc, fDec);
    … …  
    267279
    268280    t.Now();
    269     gLog << warn << t << " - Warning: Requested position not reached (i=" << dec << i << ")" << endl;
     281    gLog << warn << t << " - Warning: Requested position not reached (i=" << i << ")" << endl;
    270282
    271283    fCosy->fCom->SendStatus("Target position missed!");

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

    r8869 r8875  
    2323    Float_t fDec;
    2424
     25    Float_t fMaxResidual; // [rev] Maximum residual deviation to stop pointin
     26
    2527    void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
    2628    void SetPosVelocity(const Float_t ratio, Float_t vel);
    2729
    2830public:
    29     MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1) { }
     31    MSlewing(MCosy *cosy) : fCosy(cosy), fVel(0.1), fAcc(0.2), fDec(0.1), fMaxResidual(1./16384) { }
    3032
    3133    void SetPointAccDec(Float_t acc, Float_t dec) { fAcc = acc; fDec = dec; }
    3234    void SetPointVelocity(Float_t vel) { fVel = vel; }
    3335    int  SetPosition(const ZdAz &dst, Bool_t track=kFALSE);
     36    void SetMaxResidual(Float_t ac) { fMaxResidual = ac; }
    3437
    3538    ClassDef(MSlewing, 0)

  • trunk/MagicSoft/Cosy/videodev/VideodevIncl.h

    r920 r8875  
    11#ifndef __CINT__
    22
     3#include <TVector2.h>
     4
    35#endif // __CINT__
Note: See TracChangeset for help on using the changeset viewer.