roboforum.ru

'==============================================================================
'Платформа применения- МИНИБОТ-1.
'Язык - Basic
'Среда - BASCOM-AVR MCS Electronics v1.11.7.7
'Назначение программы - пример для некоммерческого использования.
'Действие программы - уравнивание угловых скоростей колес одноосевой схемы
'                     с двумя независимыми двигателями.
'Результат работы программы - езда прямо в одном направлении.
'Пояснения - в программе использован ШИМ для управления двигателями
'            и прерывание от таймера0 для обработки информации от датчиков
'            оборотов двигателей.
'Дата: 10.03.2008
'Автор: Чернышев А.А
'==============================================================================
$regfile = "m32DEF.dat"
$crystal = 8000000
$hwstack = 32
$swstack = 10
$framesize = 40

'---------------------------порт А---------------------------------------------

Config Pina.0 = Input : Датчик_линии_Правый Alias Pina.0
Config Pina.1 = Input : Обороты_Правый Alias Pina.1
Config Pina.2 = Input : Обороты_Левый Alias Pina.2
Config Pina.3 = Input : Датчик_линии Alias Pina.3
Config Pina.4 = Input : Датчик_линии_Левый Alias Pina.4
Config Pina.5 = Input : Ток_Двиг_Левый Alias Pina.5
Config Pina.6 = Input : Ток_Двиг_Правый Alias Pina.6
Config Pina.6 = Input : Батарея Alias Pina.6

'---------------------------порт В---------------------------------------------

Portb = &B11111111
Config Pinb.0 = Output : Локатор_Тыл Alias Portb.0
Config Pinb.1 = Output : Локатор_Правый Alias Portb.1
Config Pinb.2 = Input : Зарядка Alias Pinb.2
Config Pinb.3 = Output : Локатор_Левый Alias Portb.3

'---------------------------порт С---------------------------------------------

Portc = &B00000011
Config Pinc.0 = Output : И2С_такт Alias Portc.0
Config Pinc.1 = Output : И2С_данные Alias Portc.1      'реконфигурируется
Config Pinc.2 = Output : Реверс_Левый Alias Portc.2    ': Реверс_Левый = 0
Config Pinc.3 = Output : Реверс_Правый Alias Portc.3   ': Реверс_Правый = 0
Config Pinc.4 = Output : Кр_Правый Alias Portc.4       ': Кр_Правый = 1
Config Pinc.5 = Output : Зел_Правый Alias Portc.5      ': Зел_Правый = 1
Config Pinc.6 = Output : Кр_Левый Alias Portc.6        ': Кр_Левый = 1
Config Pinc.7 = Output : Зел_Левый Alias Portc.7       ': Зел_Левый = 1

'---------------------------порт D---------------------------------------------

Portd = &B00111111
Config Pind.0 = Input : УСАРТ_вход Alias Pind.0
Config Pind.1 = Output : УСАРТ_выход Alias Portd.1
Config Pind.2 = Input : Приемник_Тыл Alias Pind.2
Config Pind.3 = Input : Приемник_Фронт Alias Pind.3
Config Pind.4 = Output                                 'шим левого двигателя
Config Pind.5 = Output                                 'шим правого двигателя
Config Pind.6 = Output : ИК_свет Alias Portd.6
Config Pind.7 = Output : Динамик Alias Portd.7

'---------------------------конфигурация железа--------------------------------
'настройка ШИМ
Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down

'счетчик_0 - для измерений периода сигнала с датчиков оборотов двигателей
Config Timer0 = Timer , Prescale = 1                   'таймер
Stop Timer0                                            'стоп таймер
Tcnt0 = 0                                              'чистим регистр
'-----------------------------переменные---------------------------------------
Dim Installspeed As Byte
Dim ФлагЛ As Bit , ФлагП As Bit
Dim СчетчикЛ As Byte , СчетчикП As Byte
'------------------------присвоение начальных значений-------------------------
'На разных скоростях возможны флуктуации по курсу - это связано с точностью
'сборки механики (пробуксовка двгателей относительно колес), настройки
'датчиков оборотов, а также их сильной внешней паразитной засветки.
'В данной программе датчики используются как логические - без использования АЦП.

Installspeed = 120

'настройка прерываний
On Timer0 Время_истекло
Start Timer0
Enable Timer0
Enable Interrupts

   СчетчикЛ = 0
   СчетчикП = 0
   ИК_свет = 1                                         'включаем ИК излучатели
'==============================================================================
'основная программа
Do
      Pwm1b = Pwm1b + 1                                'загрузка в ШИМ
      Pwm1a = Pwm1a + 1
'фиксация скорости
   If Pwm1a >= Installspeed Then
      Pwm1a = Installspeed
   End If

   If Pwm1b >= Installspeed Then
      Pwm1b = Installspeed
   End If
Loop

'==============================================================================
'ПП - прерывания от таймера 0
Время_истекло:
'увеличение счетчика левого
   If ФлагЛ = 0 Then
      If Обороты_Левый = 1 Then
         Incr СчетчикЛ : ФлагЛ = 1
      End If
   Else
      If Обороты_Левый = 0 Then
         ФлагЛ = 0
      End If
   End If
'увеличение счетчика правого
   If ФлагП = 0 Then
      If Обороты_Правый = 1 Then
         Incr СчетчикП : ФлагП = 1
      End If
   Else
      If Обороты_Правый = 0 Then
         ФлагП = 0
      End If
   End If
'уравнивание счетчиков
   If СчетчикП > СчетчикЛ Then
      Pwm1a = Pwm1a + 1                                'ускорить правый
      If Pwm1b > 0 Then
         Pwm1b = Pwm1b - 1
      End If                                           'замедлить левый
   End If
      If СчетчикП < СчетчикЛ Then
         If Pwm1a > 0 Then
            Pwm1a = Pwm1a - 1                          'замедлить правый
         End If
         Pwm1b = Pwm1b + 1                             'ускорить левый
      End If
'сброс счетчиков
    If СчетчикП = СчетчикЛ Then
      СчетчикП = 0 : СчетчикЛ = 0
    End If
Return