$regfile = "m32def.dat"                    'используем мк ATmega32
$crystal = 8000000                    'частота кварцевого резонатора 8MHz

$lib "mcsbyte.lbx"

Config Rc5 = Pinb.3
Config Pinb.0 = Output : Led1 Alias Portb.0                 'ногу сконфигурировали как выход и обозвали Led1
Config Pinb.2 = Output : Led2 Alias Portb.2                 'ногу сконфигурировали как выход и обозвали Led2

Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down       ' таймер для ШИМ двигателей

Config Pind.6 = Output : Dr1 Alias Portd.6                  'ногу сконфигурировали как выход и обозвали Dr1
Config Pind.7 = Output : Dr2 Alias Portd.7                  'ногу сконфигурировали как выход и обозвали Dr2
Config Pind.5 = Output                    'ногу ШИМа 1-ого  движка сконфигурировали как выход
Config Pind.4 = Output                    'ногу ШИМа 2-ого движка сконфигурировали как выход

Dim Address As Byte , Command As Byte
Dim Speed As Integer
Enable Interrupts                    ' разрешаем прерывания

Do

Getrc5(address , Command)
    If Address <> 255 And Command <> 255 Then
    Command = Command And &B01111111
    End If

    If Command = 5 Then : Reset Led1
       Gosub Стоп
    Elseif Command = 2 Then : Set Led1
       Gosub Вперёд
       Waitms 100
    Elseif Command = 8 Then
       Gosub Назад
       Waitms 100
    Elseif Command = 4 Then
       Gosub Разворот_влево
       Waitms 100
    Elseif Command = 6 Then
       Gosub Разворот_вправо
       Waitms 100
    Elseif Command = 3 Then
       Gosub Вперёд_вправо
       Waitms 100
    Elseif Command = 1 Then
       Gosub Вперёд_влево
       Waitms 100
    Elseif Command = 7 Then
       Gosub Назад_влево
       Waitms 100
    Elseif Command = 9 Then
       Gosub Назад_вправо
       Waitms 100
    End If

Loop

'********************  Проект Капанда ******************************************
'******************** Гусеничный робот *****************************************
'******************* С АТмега32 на борту ***************************************
'************* частота системного кварца 8 МГц *********************************
'************ возможность расширения функционала *******************************
'*********** Котроллер собран на платформе MRC-40 ******************************
'*** Благодарности: ************************************************************
'**** Setar, SkayStorm, FireFly, Spase, DeaD ***********************************
'*******************************************************************************

$regfile = "m32def.dat"                    'используем мк ATmega32
$crystal = 8000000                    'частота кварцевого резонатора 8MHz

'****************************Подключаемые библиотеки****************************

$lib "mcsbyte.lbx"                    ' Библиотека для использования протокола RC5
'$lib "lcd4.lbx"                    'используем 4-х битное подключение дисплея с помощью альтернативной библиотеки
'Cursor Off : Cls                    'отключаем курсор и очищаем дисплей

'****************************Настройка таймеров и сервоприводов*****************

Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down       ' таймер для ШИМ двигателей
Config Servos = 1 , Servo1 = Portc.2 , Reload = 12          ' Указываем кол-во серв, присваеваем имя Servo1 порту PC2, указываем длину импульса.

'*************************** Порт A ********************************************

' PortA.0 - Левый нижний
' PortA.1 - Правый нижний
' PortA.2 - Левый верхний
' PortA.3 - Правый верхний
' Поключили дальномер, порт A7, настройки АДС ниже

'*************************** Порт В ********************************************

'Библиотека lcd4.lbx использует следующее подключение:
'Config Lcdpin = Pin , Rs = Portb.0 , E = Portb.2 , Db4 = Portb.4 , Db5 = Portb.5 , Db6 = Portb.6 , Db7 = Portb.7
'Config Lcd = 16 * 2                    'используем дисплей на 2 строчки по 16 знакомест в каждой
Config Rc5 = Pinb.3
Config Pinb.0 = Output : Led1 Alias Portb.0                 'ногу сконфигурировали как выход и обозвали Led1
Config Pinb.2 = Output : Led2 Alias Portb.2                 'ногу сконфигурировали как выход и обозвали Led2
Config Pinb.1 = Output : Led3 Alias Portb.1                 'ногу сконфигурировали как выход и обозвали Led2

'*************************** Порт C ********************************************

Config Pinc.2 = Output                    ' Подключили серву1 к порту С2, настройки сервы выше

'*************************** Порт D ********************************************

Config Pind.6 = Output : Dr1 Alias Portd.6                  'ногу сконфигурировали как выход и обозвали Dr1
Config Pind.7 = Output : Dr2 Alias Portd.7                  'ногу сконфигурировали как выход и обозвали Dr2
Config Pind.5 = Output                    'ногу ШИМа 1-ого  движка сконфигурировали как выход
Config Pind.4 = Output                    'ногу ШИМа 2-ого движка сконфигурировали как выход
' PortD.0 - RXD
' PortD.1 - TXD
' Свободные порты: PortD.2 - PortD.3

'*****************************Настройка ADC*************************************

Config Adc = Single , Prescaler = Auto , Reference = Internal

'*******************************************************************************

Enable Interrupts                    ' разрешаем прерывания

'************************Переменные и константы*********************************

Dim Address As Byte , Command As Byte
Dim Speed As Integer , Sharp As Integer , Lv As Integer , Pv As Integer , Ln As Integer , Pn As Integer
Dim I As Integer , P As Integer , G As Integer
Dim Vout As Word
Dim A As Byte

Speed = 155
A = 1

'*****************************Основной цикл*************************************

'Do

  While A > 0                    ' ждем команды с пульта и переходим к соответствующей программе
   Getrc5(address , Command)
    If Address <> 255 And Command <> 255 Then
    Command = Command And &B01111111
    End If
   If Command = 1 Then : P = 1 : Waitms 100 : Gosub 1        ' Самостоятельное движение с объездом препядствий и использованием датчиков поверхности
   Elseif Command = 2 Then : P = 2 : Waitms 100 : Gosub 2    ' Самостоятельное движение с объездом препядствий без использованием датчиков поверхности
   Elseif Command = 3 Then : P = 3 : Waitms 100 : Gosub 3    ' Прием команд и управление с пульта
   End If
  Wend

1:
' P = 1
Gosub Вперёд                    ' Если сигнала от ик-датчика(Sharp) не поступает, значит двигаемся всё время прямо

'******************Вращение сервопривода и датчика Sharp************************

For I = 45 To 90 Step 1                    ' Вращаем сервой справа на лево
   Gosub Sharp_right                    ' считаем что шарп смотрит вправо
   Servo(1) = I
   Gosub Датчики                    ' проверяем датчики пов-ти
Next

For I = 90 To 135 Step 1                    ' Вращаем сервой справа на лево
   Gosub Sharp_left                    ' считаем что шарп смотрит влево
   Servo(1) = I
   Gosub Датчики                    ' проверяем датчики пов-ти
Next

For I = 135 To 90 Step -1                    ' Вращаем сервой слева на право
   Gosub Sharp_left                    ' считаем что шарп смотрит вправо
   Servo(1) = I
   Gosub Датчики                    ' проверяем датчики пов-ти
Next

For I = 90 To 45 Step -1                    ' Вращаем сервой слева на право
   Gosub Sharp_right                    ' считаем что шарп смотрит влево
   Servo(1) = I
   Gosub Датчики                    ' проверяем датчики пов-ти
Next

Gosub 1

2:
' P = 2
Gosub Вперёд                    ' Если сигнала от ик-датчика(Sharp) не поступает, значит двигаемся всё время прямо

'******************Вращение сервопривода и датчика Sharp************************

For I = 45 To 90 Step 1                    ' Вращаем сервой справа на лево
   Gosub Sharp_right                    ' считаем что шарп смотрит вправо
   Servo(1) = I
   Waitms 12
Next

For I = 90 To 135 Step 1                    ' Вращаем сервой справа на лево
   Gosub Sharp_left                    ' считаем что шарп смотрит влево
   Servo(1) = I
   Waitms 12
Next

For I = 135 To 90 Step -1                    ' Вращаем сервой слева на право
   Gosub Sharp_left                    ' считаем что шарп смотрит вправо
   Servo(1) = I
   Waitms 12
Next

For I = 90 To 45 Step -1                    ' Вращаем сервой слева на право
   Gosub Sharp_right                    ' считаем что шарп смотрит влево
   Servo(1) = I
   Waitms 12
Next

Gosub 2

3:
' P = 3
Getrc5(address , Command)
    If Address <> 255 And Command <> 255 Then
    Command = Command And &B01111111
    End If

    If Command = 5 Then : Reset Led1
       Gosub Стоп
    Elseif Command = 2 Then : Set Led1
       Gosub Вперёд
       Waitms 100
    Elseif Command = 8 Then : Set Led1
       Gosub Назад
       Waitms 100
    Elseif Command = 4 Then : Set Led1
       Gosub Разворот_влево
       Waitms 100
    Elseif Command = 6 Then : Set Led1
       Gosub Разворот_вправо
       Waitms 100
    Elseif Command = 3 Then : Set Led1
       Gosub Вперёд_вправо
       Waitms 100
    Elseif Command = 1 Then : Set Led1
       Gosub Вперёд_влево
       Waitms 100
    Elseif Command = 7 Then : Set Led1
       Gosub Назад_влево
       Waitms 100
    Elseif Command = 9 Then : Set Led1
       Gosub Назад_вправо
       Waitms 100
    Elseif Command = 16 Then : Set Led2 : Waitms 100 : Reset Led2
       Speed = Speed + 5
       Waitms 100
    Elseif Command = 17 Then : Set Led2 : Waitms 100 : Reset Led2
       Speed = Speed - 5
       Waitms 100
    End If

Gosub 3

'Loop

'*******************************************************************************
'      Включение комбинации моторов для движения в заданном направлении
'*******************************************************************************

Стоп:                    ' стоим на месте
Dr1 = 0 : Dr2 = 0 : Pwm1a = 0 : Pwm1b = 0
Return

Вперёд:                    ' едим вперёд на заданной скорости
Dr1 = 0 : Dr2 = 0 : Pwm1a = Speed : Pwm1b = Speed
Return

Вперёд_вправо:                    ' поварачиваем вправо на заданной скорости
Dr1 = 0 : Dr2 = 0 : Pwm1a = Speed : Pwm1b = 0
Return

Вперёд_влево:                    ' поворачиваем влево на заданной скорости
Dr1 = 0 : Dr2 = 0 : Pwm1a = 0 : Pwm1b = Speed
Return

Назад:                    ' едим назад на заданной скорости
Dr1 = 1 : Dr2 = 1 : Pwm1a = Speed : Pwm1b = Speed
Return

Назад_вправо:                    ' поворачиваем назад-вправо на заданной скорости                    ' едим вперёд на заданной скорости
Dr1 = 1 : Dr2 = 1 : Pwm1a = Speed : Pwm1b = 0
Return

Назад_влево:                    ' поворачиваем назад-влево на заданной скорости                    ' едим вперёд на заданной скорости
Dr1 = 1 : Dr2 = 1 : Pwm1a = 0 : Pwm1b = Speed
Return

Разворот_вправо:                    ' делаем Разворот_вправо на заданной скорости
Dr1 = 0 : Dr2 = 1 : Pwm1a = Speed : Pwm1b = Speed
Return

Разворот_влево:                    ' делаем Разворот_влево на заданной скорости
Dr1 = 1 : Dr2 = 0 : Pwm1a = Speed : Pwm1b = Speed
Return

'*******************************************************************************
'                        Проверка датчиков поверхности
'*******************************************************************************

Левый_верхний:

Config Porta.2 = Output : Set Porta.2 : Waitms 3
Config Porta.2 = Input : Reset Porta.2 : Waitms 3
If Pina.2 = 1 Then : Lv = 0 : Else : Lv = 1 : End If
Return

Левый_нижний:

Config Porta.0 = Output : Set Porta.0 : Waitms 3
Config Porta.0 = Input : Reset Porta.0 : Waitms 3
If Pina.0 = 1 Then : Ln = 0 : Else : Ln = 1 : End If
Return

Правый_верхний:

Config Porta.3 = Output : Set Porta.3 : Waitms 3
Config Porta.3 = Input : Reset Porta.3 : Waitms 3
If Pina.3 = 1 Then : Pv = 0 : Else : Pv = 1 : End If
Return

Правый_нижний:

Config Porta.1 = Output : Set Porta.1 : Waitms 3
Config Porta.1 = Input : Reset Porta.1 : Waitms 3
If Pina.1 = 1 Then : Pn = 0 : Else : Pn = 1 : End If
Return

'*******************************************************************************
'                        Анализ поверхности стола
'*******************************************************************************

Датчики:
' Gosub Подъем

   If Lv = 0 Then
   Set Led1                    ' если обрыв слева то
    For G = 1 To 6                    ' цикл отъезда от края стола
     Gosub Левый_нижний : Gosub Правый_нижний                ' проверяем задние датчики
     If Ln = 0 Or Pn = 0 Then : Reset Led1 : Gosub Вперёд : Return       ' если сработали задние датчики то прекращаем отъезд, трогаемся вперед и возвращаемся в цикл
     Else : Gosub Назад_влево : Waitms 300 : Gosub Вперёд    ' если датчики не сработали то продолжаем отъезд
     End If
    Next
   End If
   If Pv = 0 Then
   Set Led2                    ' все тоже самое для правой сороны
    For G = 1 To 6
     Gosub Левый_нижний : Gosub Правый_нижний
     If Ln = 0 Or Pn = 0 Then : Reset Led2 : Gosub Вперёд : Return
     Else : Gosub Назад_вправо : Waitms 300 : Gosub Вперёд
     End If
    Next
   End If
  Reset Led1
  Reset Led2
Return

'Подъем:

' Gosub Левый_верхний                    ' анализ "на подъем"
' Gosub Левый_нижний
' Gosub Правый_верхний
' Gosub Правый_нижний
'   If Pv = 0 And Ln = 0 Or Pn = 0 And Lv = 0 Then           ' проверяем на полу мы или уже в воздухе
'   Gosub Стоп : Waitms 3000                    ' если в воздухе то останавливаемся
'   Else : Return
'   End If
'Gosub 2

'***********************************Sharp***************************************

Sharp_right:
Do
Start Adc : Vout = Getadc(7)                    'Sharp connected for example to PA=0(ADC0)
If Vout > 240 Then : Led3 = 1 : Gosub Разворот_влево : End If : Loop Until Vout < 240 : Led3 = 0 : Gosub Вперёд
Return

Sharp_left:
Do
Start Adc : Vout = Getadc(7)                    'Sharp connected for example to PA=0(ADC0)
If Vout > 240 Then : Led3 = 1 : Gosub Разворот_вправо : End If : Loop Until Vout < 240 : Led3 = 0 : Gosub Вперёд
Return

End

'*******************************************************************************