Kategori arşivi: Gator

Gator pozisyon ve hız kontrol sistemi

drive.itemstatus durumları

Bir Cevap Yazın

drive thread’inin bir task’i sürücüyle belli bir iş yaptıktan sonra busy bayrağını 0 yapmadan hemen önce sürücünün (kapının) o anki durumunu drive.itemstatus bayrağında gösterir. Ana thread bu bayrağı sorgulayarak yoluna devam eder.

  • 0 : DRIVE_UNINITIALIZED Sürücü başlatılmamış ya da hata durumu.
  • 1 : DRIVE_UNDEFINED Sürücü yeni başlamış ya da durum bilgisi kaybedildi
  • 2 : DRIVE_OPENING Açılma yönünde hareket ediliyor.
  • 3 : DRIVE_CLOSING Kapanma yönünde hareket ediliyor.
  • 4 : DRIVE_OPEN_OBS Açılma esnasında hareket durdu (engel ya da komutla)
  • 5 : DRIVE_CLOSE_OBS Kapanma esnasında hareket durdu (engel ya da komutla)
  • 6 : DRIVE_OPENED Kapı Açık
  • 7 : DRIVE_CLOSED Kapı Kapalı

[10] : Sabit Hızla KAPATMA

Bir Cevap Yazın

Bu komut, Vemg hızı ile kapının kapatılmasını sağlar. Kapı boyu + aralık toleransı kadar bir mesafe sonra engel algılanmasa bile motor duracaktır.
Bu işlemde hata üretilmez. İşlem bittikten sonra sürücü durumu DRIVE_CLOSED olacak ve busy = 0 yapılacaktır.

      // SABIT HIZLA KAPATMA:  Vemg hizi ile kapi kapatma komutu
      //////////////////////////////////////////////////////////////////////////
        case 10:
            drive.command = 0;

            rampset = Vemg;             // set degeri Vemg olacak!
            current_threshold = Tmc;    // durma zorlanma degeri: kapanma limit algılama yükü
                                        // burada engel algılama yapmıyorum..
            PID.overloaded = 0;         // zorlanma algilama sayacını da
            OCcounter = 0;              // sifirla

            /* Position Breakpoint Ayari:
             *  Kapi boyu + olcum toleransi kadar bir mesafe hareket hakki var.
             *  Tetiklenen encodertick flag hareketi sonlandıracak
             *  Bu kadar mesafe boyunca donus olmussa kapı sonuna gelinmiş olmalı
             *  motor duracak, hata da verilebilirdi..
             */
            ClearEncoderCounter();
            bppos.ul = dt.ul + APERTURE_TOLERANCE;
            bppos.l = (signed long *) ( -1 * bppos.l);
            SetREVBreakpoint();

            // sabit rampali kalkis gorevini baslat:
            dir = 1;                    // yon: REV
            drive.subindex = 11;
            drive.status = 1100;        // sabit rampa adimi alt gorevi
        break;


        // Hareket sonu mesafesine erişim ya da zorlanma limiti bekleme adımı
        case 11:

            // ZORLANMA ALGILANDI:
            if (PID.overloaded)
            {
                PID.nocontrol = 1;      // PID kontrol kapalı olmalı!

                // Demek zorlanma oldu.. Bayrağı sıfırla...
                PID.overloaded = 0;
                OCcounter = 0;
                drive.status = 12;
            }

            // kapı boyu kadar hareket edildiyse motor durmalı:
            if (encodertick)
            {
                // TODO: burada bir hata kodu uretebilirim ama
                // onu handle edecek mekanizma da olmali...
                // bunun yerine "bu kadar hareket ettiysem kapı kapanmıştır artık"
                // deyip bitiriyorum..
                drive.status = 1000;
                drive.subindex = 12;
            }


            // bu esnada kullanıcı durdurma komutu verdiyse:
            if (drive.command == 1000)
            {
                drive.command = 0;
                drive.status = 1000;
                drive.subindex = 12;
            }
        break;


        // kapı sonu, hareket bitti:
        case 12:
            // 90ms. daha kapiyi kapama yonunde zorla ve bitir:
            if (PID.controlcounter > 8)
            {
                PID.controlcounter = 0;
                speedref = 0;
                MDC = 0;
                MOTOR_BRK_L;
                drive.itemstatus = DRIVE_CLOSED;           // kapi KAPALI
                drive.status = 0;
                drive.busy = 0;
                // position sayaçlarini sifirla:
                ClearEncoderCounter();
                position.ul = 0;
            }
        break;

[1100]: Sabit Rampa ile Hızlanma

Bir Cevap Yazın

Bu alt-göreve atlandığında motor hızı 0’dan rampset değerine sabit bir ivme ile artırılır.
Alt-görev çağrıldığında dir bayrağının durumuna göre hareketin yönü belirlenir. dir = 1 ise REV dir = 0 ise FWD yönünde hareket başlatılacak.
Zorlanma algılanırsa rutinden hemen çıkılır. Zorlanma sonucu alt görevden dönülürse overloaded bayrağı 1 olarak dönülür.
busy ve itemstatus bayrakları bu görevlerden etkilenmez.

// SABIT RAMPALI HIZLANMA: (FWD/REV)
 ////////////////////////////////////////////////////////////////////////////
        case 1100:
          speedref = 50;
          MDC = 50;
          PID.controlcounter = 0;
          PID.restart = 1;
          PID.nocontrol = 0;     // kalkış PID kontrol ile, bu önemli!
          SetDir();                     // sürücü yönünü belirle..
          _PTEN = 1;                    // PWM'i başlat!
          drive.status++;
        break;


        // her 40ms'de bir hizi biraz artirmak:
        case 1101:
            if (PID.controlcounter > 3)
            {
                speedref += 10;
                drive.status++;
                PID.controlcounter = 0;
            }
            // bu esnada aynı zamanda  motor zorlanıyor mu diye de bakıyoruz:
            if (PID.overloaded)
            {
                PID.nocontrol = 1;      // zorlanma durumunda PID devre dışı
                speedref = 0;
                drive.status = drive.subindex;       // ve hemen dönüyoruz..
            }
        break;


        // referans degere ulastik mi sorgulamak:
        case 1102:
            if (speedref < rampset) drive.status = 1101;
            else
            {
                speedref = rampset;
                drive.status = drive.subindex;
            }
        break;
    ////////////////////////////////////////////////////////////////////////////////

[1000] : Sabit Frenleme İle Durma

Bir Cevap Yazın

Bu alt-göreve atlandığında motor frenleme ile durdurulur.
busy flag değiştirilmiyor.
itemstatus flag değiştirilmiyor.

   /// SABIT FRENLEME ILE DURMA :  900ms sürer..
   /////////////////////////////////////////////////////////////////////////////////
        case 1000:
            // kullanici durmak istiyorsa, position breakpoint kesmesi gerekmiyor
            // diye dusunurum:
            _PCLEQIEN = 0;
            _PCHEQIEN = 0;

            // motoru bosa at:
            speedref = 0;
            MDC = 0;
            PID.nocontrol = 1;
            MOTOR_BRK_L;
            PID.controlcounter = 0;
            drive.status++;
        break;

        case 1001:
            if (PID.controlcounter > 19)     // 200ms bosta bekle
            {
                // bosta suresi gectikten sonra biraz fren yap:
                speedref = 1000;
                drive.status++;
                PID.controlcounter = 0;
            }
        break;

        case 1002:
            if (PID.controlcounter > 9)  // 100ms bekle
            {
                speedref = 2000;        // freni artir
                drive.status++;
                PID.controlcounter = 0;
            }
        break;

        case 1003:
            if (PID.controlcounter > 9)    // 100ms bekle
            {
                speedref = 3000;            // freni artir
                drive.status++;
                PID.controlcounter = 0;
            }
         break;

       case 1004:
           if (PID.controlcounter > 9)      // 100ms bekle
           {
               speedref = 0;
               drive.status++;
               PID.controlcounter = 0;
           }
       break;

        case 1005:
            if (PID.controlcounter > 19)     // 200ms bekle
            {
                speedref = 4000;            // freni artir
                drive.status++;
                PID.controlcounter = 0;
            }
        break;

       case 1006:
           if (PID.controlcounter > 19)      // 200ms bekle
           {
               speedref = 0;                // ve islemi sonlandir...
               PID.controlcounter = 0;
               drive.status = drive.subindex;
           }
       break;
//////////////////////////////////////////////////////////////////////////////////