Elektroda.pl
Elektroda.pl
X
Computer ControlsComputer Controls
Proszę, dodaj wyjątek dla www.elektroda.pl do Adblock.
Dzięki temu, że oglądasz reklamy, wspierasz portal i użytkowników.

ramie robota male poprawki

konrad1115 14 Lis 2011 19:30 1174 0
  • #1
    konrad1115
    Poziom 11  
    witam kiedyś zacząłem projekt ramienia robota ale nie ukończyłem go postanowiłem więc dokończyć co zacząłem ramie miało być sterowane 5 serwami przez klawiaturę matrycową mam prośbę czy może ktoś sprawdzić schemat i kod w bascomie ????
    nie znam sie na eagle dlatego prosiłbym o wykonanie płytki w tym programie
    https://www.elektroda.pl/rtvforum/viewtopic.php?p=8951415#8951415 -link do schematu


    Code:
    '----------------------------------------------------------------------------------------
    
    ' Program prostego sterownika analogowych serwomechanizmów modelarskich (Ramię robota)
    ' Autor: c4v2 Cyprian Kamil Kowalski
    '----------------------------------------------------------------------------------------
    '----------------------------------------------------------------------------------------
    'dyrektywy kompilatora
    '----------------------------------------------------------------------------------------
    '$sim                                   'Symulacja programowa
    $regfile = "m8def.dat"                                      'Dyrektywa pliku z definicjami rejestrów mikroprocesora.
    $crystal = 8000000                                          'Częstotliwość zegara mikroprocesora.
    '$baud = 9600                           'Prędkość transmisji szeregowej

    '----------------------------------------------------------------------------------------
    'konfiguracja portów, licznika 16-bitowego
    '----------------------------------------------------------------------------------------
    Config Timer1 = Timer , Prescale = 8 , Compare A = Toggle , Clear Timer = 1       'Impuls co 1/8000000*8=1us

    Config Portc.0 = Output                                     'Wyjście pierwszej multipleksowanej kolumny klawiatury
    Config Portc.1 = Output                                     'Wyjście drugiej multipleksowanej kolumny klawiatury
    Config Portb.0 = Output                                     'Wyjście trzeciej multipleksowanej kolumny klawiatury

    Config Portb.1 = Output                                     'Wyjście impulsów dla (OR) wszystkich serw (do podglądu oscyloskopem...)

    Config Portc.2 = Input                                      'Wejście pierwszego wiersza klawiatury
    Config Portc.3 = Input                                      'Wejście drugiego wiersza klawiatury
    Config Portc.4 = Input                                      'Wejście trzeciego wiersza klawiatury
    Config Portc.5 = Input                                      'Wejście czwartego wiersza klawiatury

    'Config Portd.0 = Output                'Wyjście pierwszego impulsu w okresie 20ms dla serwa (do wykorzystania jeżeli RS232 nie będzie wykorzystywany)
    'Config Portd.1 = Output                'Wyjście drugiego impulsu w okresie 20ms dla serwa (do wykorzystania jeżeli RS232 nie będzie wykorzystywany)
    'Config Portd.2 = Output                'Wyjście trzeciego impulsu w okresie 20ms dla serwa (do wykorzystania chwytak?)
    Config Portd.3 = Output                                     'Wyjście czwartego impulsu w okresie 20ms dla (pierwszego) serwa
    Config Portd.4 = Output                                     'Wyjście piątego impulsu w okresie 20ms dla (drugiego) serwa
    Config Portd.5 = Output                                     'Wyjście szóstego impulsu w okresie 20ms dla (trzeciego) serwa
    Config Portd.6 = Output                                     'Wyjście siódmego impulsu w okresie 20ms dla (czwartego) serwa
    Config Portd.7 = Output                                     'Wyjście ósmego impulsu w okresie 20ms dla (piątego) serwa

    '----------------------------------------------------------------------------------------
    'deklaracje procedur
    '----------------------------------------------------------------------------------------
    On Compare1a Set_servos                                     'Deklaracja obsługi przerwania od porównania zawartości licznika z rejestrem.

    '----------------------------------------------------------------------------------------
    'deklaracje stałychych
    '----------------------------------------------------------------------------------------
    Const V_prog = "Servo_x8_0v4"                               'Stała wersji programu w tym przypadku nie potrzebna (takie przyzwyczajenie)
    Const T_servo = 2500                                        'Szczelina czasowa na obsługę pojedynczego serwa, 2.5ms
    Const T_serwo_init = 1000                                   'Wartość "inicjalizacyjna" czasu impulsu wysokiego dla serw, 1ms
    Const T_serwo_min = 1000                                    'Minimalny impuls wysoki, 1ms
    Const T_serwo_max = 2000                                    'Maksymalny impuls wysoki, 2ms
    Const T_inc_decr = 10                                       'Wartość o którą będzie inkrementowany/dekrementowany (wydłużany/skracany) impuls wysoki, 10us
    Const S1_incr = &H0001                                      'Stała przycisku wydłużającego czwarty impuls w okresie 20ms wysoki (dla serwa pierwszego)
    Const S1_decr = &H0002                                      'Stała przycisku skracającego czwarty impuls w okresie 20ms wysoki (dla serwa pierwszego)
    Const S2_incr = &H0004                                      'Stała przycisku wydłużającego piąty wysoki impuls w okresie 20ms wysoki (dla serwa drugiego)
    Const S2_decr = &H0008                                      'Stała przycisku skracającego piąty wysoki impuls w okresie 20ms wysoki (dla serwa drugiego)
    Const S3_incr = &H0010                                      'Stała przycisku wydłużającego szósty wysoki impuls w okresie 20ms (dla serwa trzeciego)
    Const S3_decr = &H0020                                      'Stała przycisku skracającego szósty wysoki impuls w okresie 20ms (dla serwa trzeciego)
    Const S4_incr = &H0040                                      'Stała przycisku wydłużającego siódmy wysoki impuls w okresie 20ms (dla serwa czwartego)
    Const S4_decr = &H0080                                      'Stała przycisku skracającego siódmy wysoki impuls w okresie 20ms (dla serwa czwartego)
    Const S5_incr = &H0100                                      'Stała przycisku wydłużającego ósmy wysoki impuls w okresie 20ms (dla serwa piątego)
    Const S5_decr = &H0200                                      'Stała przycisku skracającego ósmy wysoki impuls w okresie 20ms (dla serwa piątego)
    Const S6_incr = &H0400                                      'Stała przycisku chwytaka "otwórz" (wyjście nie przypisane)
    Const S6_decr = &H0800                                      'Stała przycisku chwytaka "zaciśnij" (wyjście nie przypisane, może na serwie)

    '----------------------------------------------------------------------------------------
    'deklaracje zmiennych
    '----------------------------------------------------------------------------------------
    Dim Kbd_presset As Bit                                      'flaga zezwolenia na interpretację stanu klawiatury
    Dim Clk As Byte                                             'Zmienna zegarowa licząca szczeliny czasowe dla kolejnych serw
    Dim Tmp0 As Byte                                            'Zmienna pomocnicza (obliczania indeksów tablic w przerwaniu)
    Dim Tmp1 As Byte                                            'Zmienna pomocnicza do "przechowywania" stanu wierszy klawiatury dla pierwszej (1/2 bajtu) i drugiej kolumny (1/2 bajtu)
    Dim Tmp2 As Byte                                            'Zmienna pomocnicza do "przechowywania" stanu wierszy klawiatury dla drugiej kolumny (1/2 bajtu)
    Dim Tmp3 As Byte                                            'Zmienna licznikowa w pętlach inkrementacji/dekrementacji wysokich impulsów (wyliczanie indeksu serwa)
    Dim Tmp4 As Word                                            'Zmienna przechowująca "wyłuskany" stan przycisku (bitu) inkrementacji/dekrementacji czasu wysokiego impulsu dla wybranego serwa
    Dim Servo(8)as Word                                         'tablica przechowująca czasy wysokich impulsów dla serw
    Dim T_pulse_low As Word                                     'Wyliczany czas pozostały do końca szczeliny czasowej dla kolejnych serw
    Dim Tmp_kbd As Word                                         '"tymczasowa zmienna klawiaturowa"
    Dim Key As Word                                             'Zmienna przechowująca aktualnie wyliczoną stałą przycisku klawiatury
    Dim Kbd As Word                                             'Zmienna "zapamiętująca" stan klawiatury do interpretacji w programie głównym i eliminacji drgań zestyków przełączników klawiatury w przerwaniach

    '----------------------------------------------------------------------------------------
    'aliasy
    '----------------------------------------------------------------------------------------

    '----------------------------------------------------------------------------------------
    'program główny
    '----------------------------------------------------------------------------------------
    Set Portc.2                                                 'Podciągnięcie wejścia pierwszego wiersza klawiatury
    Set Portc.3                                                 'Podciągnięcie wejścia drugiego wiersza klawiatury
    Set Portc.4                                                 'Podciągnięcie wejścia trzeciego wiersza klawiatury
    Set Portc.5                                                 'Podciągnięcie wejścia czwartego wiersza klawiatury
    Set Portc.0                                                 'Ustawienie pierwszej kolumny klawiatury w stan wysoki
    Set Portc.1                                                 'Ustawienie drugiej kolumny klawiatury w stan wysoki
    Set Portb.0                                                 'Ustawienie trzeciej kolumny klawiatury w stan wysoki
    Do                                                          'Pętla inicjująca tablicę przechowującą czasy wysokich impulsów dla serw
      Incr Tmp0                                                 'Wyliczenie indeksu następnego (obsługiwanego) serwa
      Servo(tmp0) = T_serwo_init                                'Prypisanie wartość "inicjalizacyjnej" czasu impulsu wysokiego dla kolejnych serw
    Loop Until Tmp0 > 7
    Enable Interrupts                                           'Włączenie globalnego systemu przerwań.
    Enable Compare1a                                            'Włączenie przerwania obsługi przerwania od porównania zawartości licznika z rejestrem
    Timer1 = 0                                                  'Licznik liczy od zera
    Portb.1 = 1                                                 'Ustawienie wyjścia impulsów dla (OR) wszystkich serw (do podglądu oscyloskopem...)
    Do                                                          'Początek pętli głównej
      If Kbd_presset = 1 Then                                   'Jeżeli flaga zezwolenia na interpretację stanu klawiatury została ustawiona to:
        Tmp3 = 4                                                'Ponieważ jest wykorzystywane tyko pięć serw (od czwartego do ósmego) przypisanie wartości cztery
        Do                                                      'Poniższe instrukcje umożliwiają (dla inkrementacji czasu wysokiego impulsu)
          Select Case Tmp3                                      '"jednoczesny" ruch serw, jednoczesne naciskanie przycisków (klawiatura z diodami)
            Case 4 : Key = S1_incr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla pierwszego obsługiwanego serwa
                     Tmp4 = Kbd And S1_incr                     '"wyłuskanie" stanu przycisku (bitu) inkrementacji czasu wysokiego impulsu dla pierwszego obsługiwanego serwa
            Case 5 : Key = S2_incr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla drugiego obsługiwanego serwa
                     Tmp4 = Kbd And S2_incr                     '"wyłuskanie" stanu przycisku (bitu) inkrementacji czasu wysokiego impulsu dla drugiego obsługiwanego serwa
            Case 6 : Key = S3_incr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla trzeciego obsługiwanego serwa
                     Tmp4 = Kbd And S3_incr                     '"wyłuskanie" stanu przycisku (bitu) inkrementacji czasu wysokiego impulsu dla trzeciego obsługiwanego serwa
            Case 7 : Key = S4_incr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla czwartego obsługiwanego serwa
                     Tmp4 = Kbd And S4_incr                     '"wyłuskanie" stanu przycisku (bitu) inkrementacji czasu wysokiego impulsu dla czwartego obsługiwanego serwa
            Case 8 : Key = S5_incr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla piątego obsługiwanego serwa
                     Tmp4 = Kbd And S5_incr                     '"wyłuskanie" stanu przycisku (bitu)inkrementacji czasu wysokiego impulsu dla piątego obsługiwanego serwa
          End Select
          If Key = Tmp4 And Servo(tmp3) < T_serwo_max Then      'jeżeli dla wyliczonego indeksu (serwa) naciśnięto przycisk inkrementacji i położenie serwa nie jest skrajne to:
            Servo(tmp3) = Servo(tmp3) + T_inc_decr              'inkrementuj stałą czas wysokiego impulsu dla wyliczonego (obsługiwanego) serwa
          End If
          Incr Tmp3                                             'Wyliczanie indeksu następnego (obsługiwanego) serwa
        Loop Until Tmp3 > 8                                     'powtarzane dla każdego z pięciu serw
        Tmp3 = 4                                                'Ponieważ jest wykorzystywane tyko pięć serw (od czwartego do ósmego) przypisanie wartości cztery
        Do                                                      'Poniższe instrukcje umożliwiają (dla dekrementacji czasu wysokiego impulsu)
          Select Case Tmp3                                      '"jednoczesny" ruch serw, jednoczesne naciskanie przycisków (klawiatura z diodami)
            Case 4 : Key = S1_decr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla pierwszego obsługiwanego serwa
                     Tmp4 = Kbd And S1_decr                     '"wyłuskanie" stanu przycisku (bitu) dekrementacji czasu wysokiego impulsu dla pierwszego obsługiwanego serwa
            Case 5 : Key = S2_decr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla drugiego obsługiwanego serwa
                     Tmp4 = Kbd And S2_decr                     '"wyłuskanie" stanu przycisku (bitu) dekrementacji czasu wysokiego impulsu dla drugiego obsługiwanego serwa
            Case 6 : Key = S3_decr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla trzeciego obsługiwanego serwa
                     Tmp4 = Kbd And S3_decr                     '"wyłuskanie" stanu przycisku (bitu) dekrementacji czasu wysokiego impulsu dla trzeciego obsługiwanego serwa
            Case 7 : Key = S4_decr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla czwartego obsługiwanego serwa
                     Tmp4 = Kbd And S4_decr                     '"wyłuskanie" stanu przycisku (bitu) dekrementacji czasu wysokiego impulsu dla czwartego obsługiwanego serwa
            Case 8 : Key = S5_decr                              'przepisanie stałej (wyliczonego) przycisku w celu późniejszego sprawdzenia czy naciśnięto przycisk inkrementacji czasu wysokiego impulsu dla piątego obsługiwanego serwa
                     Tmp4 = Kbd And S5_decr                     '"wyłuskanie" stanu przycisku (bitu) dekrementacji czasu wysokiego impulsu dla piątego obsługiwanego serwa
          End Select
          If Key = Tmp4 And Servo(tmp3) > T_serwo_min Then      'jeżeli dla wyliczonego indeksu (serwa) naciśnięto przycisk dekrementacji i położenie serwa nie jest skrajne to:
            Servo(tmp3) = Servo(tmp3) - T_inc_decr              'inkrementuj stałą czas wysokiego impulsu dla wyliczonego (obsługiwanego) serwa
          End If
          Incr Tmp3                                             'Wyliczanie indeksu następnego (obsługiwanego) serwa
        Loop Until Tmp3 > 8

        Tmp4 = Kbd And S6_incr                                  'obsługa chwytaka "otwórz" (nie zaimplementowane)
        If Tmp4 = S6_incr Then
         '"otwórz"
        End If
        Tmp4 = Kbd And S6_decr                                  'obsługa chwytaka "zaciśnij" (nie zaimplementowane)
        If Tmp4 = S6_decr Then
         '"zaciśnij"
        End If
        Reset Kbd_presset                                       'Wyzerowanie flagi zezwolenia na interpretację stanu klawiatury
    '    Print "S1=" ; Servo(1)             'Do testowania
    '    Print "S2=" ; Servo(2)
    '    Print "S3=" ; Servo(3)
    '    Print "S4=" ; Servo(4)
    '    Print "S5=" ; Servo(5)
    '    Print "S6=" ; Servo(6)
    '    Print "S7=" ; Servo(7)
    '    Print "S8=" ; Servo(8)
    '    Print "Tmp_kbd=" ; Bin(tmp_kbd)
      End If
    Loop
    End

    '----------------------------------------------------------------------------------------
    'definicje procedur
    '----------------------------------------------------------------------------------------
    Set_servos:                                                 'Definicja obsługi przerwania od porównania zawartości licznika z rejestrem.
    If Pinb.1 = 1 Then                                          'Impuls wysoki
      Tmp0 = Clk + 1                                            'Dla wyliczenia indeksu tablicy z czasami wysokich impulsów dla serw
      Compare1a = Servo(tmp0)                                   'Podstawienie do porównania czasu wysokiego dla wyliczonego serwa
      'Set Portd.clk                        'W przypadku korzystania z wszystkich możliwości programu (osiem serw), należy od komentować
      Select Case Clk                                           'W przypadku korzystania z wszystkich możliwości programu (osiem serw), należy za komentować
        Case 3 To 7 : Set Portd.clk                             'Ustawienie w stan wysoki wyliczone wyjście dla serwa, W przypadku korzystania z wszystkich możliwości programu (osiem serw), należy za komentować
      End Select                                                'W przpadku korzystania z wszystkich możliwości programu (osiem serw), należy za komentować
      T_pulse_low = T_servo - Servo(tmp0)                       'Obliczenie czasu trwania impulsu niskiego dla wyliczonego serwa
      Select Case Clk
        Case 0 :                                                'Config Portc.0 = Output 'W przypadku klawiatury bez diod, należy od komentować
                 Reset Portc.0                                  'Ustawienie pierwszej kolumny klawiatury w stan niski
                 nop                                            'opóźnienie (cztery cykle maszynowe)
                 nop
                 nop
                 nop
                 Tmp1 = Not Pinc                                'Odczyt wierszy pierwszej kolumny
                 Tmp1 = Tmp1 And &H3C                           'Wyłuskanie potrzebnych czterech bitów wierszy
                 Shift Tmp1 , Left , 2                          'w celu późniejszego "scalenia" z bitami wierszy następnej kolumny przesunięcie o dwa w lewo
        Case 1 :                                                'Config Portc.1 = Output 'W przypadku klawiatury bez diod, należy od komentować
                 Reset Portc.1                                  'Ustawienie drugiej kolumny klawiatury w stan niski
                 nop                                            'opóźnienie (cztery cykle maszynowe)
                 nop
                 nop
                 nop
                 Tmp2 = Not Pinc                                'Odczyt wierszy drugiej kolumny
                 Tmp2 = Tmp2 And &H3C                           'Wyłuskanie potrzebnych czterech bitów wierszy
                 Shift Tmp2 , Right , 2                         'W celu późniejszego "scalenia" z bitami wierszy poprzedniej kolumny przesunięcie o dwa w prawo
                 Tmp1 = Tmp1 Or Tmp2                            '"Scalenie" bitów wierszy pierwszej i drugiej kolumny
        Case 2 :                                                'Config Portb.0 = Output 'W przypadku klawiatury bez diod, należy od komentować
                 Reset Portb.0                                  'Ustawienie trzeciej kolumny klawiatury w stan niski
                 nop                                            'opóźnienie (cztery cykle maszynowe)
                 nop
                 nop
                 nop
                 Tmp2 = Not Pinc                                'Odczyt wierszy trzeciej kolumny
                 Tmp2 = Tmp2 And &H3C                           'Wyłuskanie potrzebnych czterech bitów wierszy
                 Shift Tmp2 , Right , 2                         'W celu późniejszego "scalenia" z bitami wierszy poprzednich kolumn przesunięcie o dwa w prawo
                 Tmp_kbd = Tmp1                                 'Przepisanie bitów wierszy poprzednich kolumn do "tymczasowej zmiennej klawiaturowej"
                 Shift Tmp_kbd , Left , 4                       'W celu późniejszego "scalenia" z bitami wierszy aktualnej kolumny o cztery w lewo
                 Tmp_kbd = Tmp_kbd Or Tmp2                      '"Scalenie" bitów wierszy poprzednich kolumn z bitami wierszy aktualnej kolumny
        Case 3 : If Tmp_kbd = Kbd And Tmp_kbd <> &H0000 Then    'Jeżeli wartość "tymczasowej zmiennej klawiaturowej"
                                                                   'była wczytana w poprzednim cyklu 20ms (zapamiętana w zmiennej KBD, eliminacja drgań zestyków przycisków)
                                                                   'oraz naciśnięto przycisk to:
                   Set Kbd_presset                              'Ustawienie flagi zezwolenia na interpretację stanu klawiatury
                 End If
                 Kbd = Tmp_kbd                                  'Zapamiętanie stanu klawiatury
      End Select
    Else                                                        'Impuls niski
      Compare1a = T_pulse_low                                   'Podstawienie do porównania czasu impulsu niskiego dla wyliczonego serwa
      'Reset Portd.clk                      'W przypadku korzystania z wszystkich możliwości programu (osiem serw), należy od komentować
      'Config Portc.0 = Input               'W przypadku klawiatury bez diod, należy od komentować
      'Config Portc.1 = Input               'W przypadku klawiatury bez diod, należy od komentować
      'Config Portb.0 = Input               'W przypadku klawiatury bez diod, należy od komentować
      Set Portc.0                                               'Ustawienie pierwszej kolumny klawiatury w stan wysoki
      Set Portc.1                                               'Ustawienie drugiej kolumny klawiatury w stan wysoki
      Set Portb.0                                               'Ustawienie trzeciej kolumny klawiatury w stan wysoki
      Select Case Clk                                           'W przypadku korzystania z wszystkich możliwości programu (osiem serw), należy za komentować
        Case 3 To 7 : Reset Portd.clk                           'Ustawienie w stan niski wyliczone wyjście dla serwa, W przypadku korzystania z wszystkich możliwości programu (osiem serw), należy za komentować
      End Select                                                'W przypadku korzystania z wszystkich możliwości programu (osiem serw), należy za komentować
      Incr Clk                                                  'inkrementacja zmiennej zegarowej liczącej szczeliny czasowe dla kolejnych serw
      If Clk = 8 Then                                           'Jeżeli obsłużona była szczelina czasowa dla ostatniego serwa to:
        Clk = 0                                                 'Początek 20ms okresu (szczelina czasowa dla pierwszego serwa)
      End If
    End If
    Return                                                      'Powrót z obsługi przerwania
    'ENDE----------------------------------------------------------------------------------------
  • Computer ControlsComputer Controls