roboforum.ru

'$prog &HFF , &HBD , &HC9 , &H00
$regfile = "m32def.dat"                                     ' файл спецификации Меги32
$crystal = 7372800                                          ' указываем на какой частоте будем работать
$baud = 115200

$include "pult.bas"

Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down
Config Pinc.2 = Output : направление_левый Alias Portc.2    'ногу сконфигурировали как выход и обозвали Drl
Config Pinc.3 = Output : направление_правый Alias Portc.3   'ногу сконфигурировали как выход и обозвали Drr
Config Pind.4 = Output                                      'ногу ШИМа левого движка сконфигурировали как выход
Config Pind.5 = Output                                      'ногу ШИМа правого движка  сконфигурировали как выход
Const вперед = 0
Const назад = 1
Const точка_останова = 50
скорость_левый Alias Pwm1b
скорость_правый Alias Pwm1a

Config Pinc.4 = Output : диод1_красный Alias Portc.4
Config Pinc.5 = Output : диод1_зеленый Alias Portc.5
Config Pinc.6 = Output : диод2_красный Alias Portc.6
Config Pinc.7 = Output : диод2_зеленый Alias Portc.7
Const зажечь = 1
Const потушить = 0

Config Pind.2 = Input : кнопка Alias Pind.2
Config Pinb.2 = Input : питание Alias Pinb.2

Config Pind.7 = Output : Portd.7 = 0 : динамик Alias Portd.7       ' динамик

' Прерывание по спаду уровня для обработки команд RC5
Config Int1 = Falling
On Int1 принять_команду_пульта
Enable Int1
Config Rc5 = Pind.3

' Глобальные переменные
Dim команда_пульта_адрес As Byte , команда_пульта As Byte
Dim Adc_temp As Word , Akb As Single
Dim Errbyte As Byte

Dim состояние_робота As Byte
   Const управление_пультом = 1
   Const движение_улиткой = 2
   Const поиск_станции = 3
   состояние_робота = 0

' Процедуры
Declare Function выполнить_команду() As Byte
   Const команда_старт = 1
Declare Sub двигаться_улиткой()
Declare Sub искать_станцию()
Declare Sub улитка_ожидание(byval период As Word)

диод2_зеленый = зажечь

' Запуск
Enable Interrupts

Waitms 200
Print "Start MiniBot v2.1"

Старт:
Do
   Select Case состояние_робота
      Case движение_улиткой:
         Disable Int1
         команда_пульта = Btn_power : Gosub выполнить_команду       ' ставим в режим парковки
         Enable Int1
         Gosub двигаться_улиткой
      Case поиск_станции:
         Disable Int1
         команда_пульта = Btn_power : Gosub выполнить_команду       ' ставим в режим парковки
         Enable Int1
         Gosub искать_станцию
      Case управление_пультом:
         Disable Int1
         команда_пульта = Btn_ok : Gosub выполнить_команду  ' снимаем с режима парковки
         Enable Int1
         Idle
      Case Else
         ' первый запуск, ставим в режим парковки
         Disable Int1
         команда_пульта = Btn_power : Gosub выполнить_команду       ' ставим в режим парковки, управление с пульта
         состояние_робота = управление_пультом
         Enable Int1
         Idle
   End Select
Loop

Sub двигаться_улиткой()
' описатели движения
   Dim I As Byte
   Local возможная_скорость As Integer
   Const улитка_прибавка_скорости = 10
   Const улитка_разбалансировка = 10
   Const улитка_время_движения = 50
' движение
   Do
      For I = точка_останова To 255 Step улитка_прибавка_скорости
         направление_левый = вперед : направление_правый = вперед
         возможная_скорость = I + улитка_разбалансировка
         скорость_левый = возможная_скорость
         возможная_скорость = I - улитка_разбалансировка
         скорость_правый = возможная_скорость
         Call улитка_ожидание(улитка_время_движения)
      Next
   Loop
End Sub

Sub улитка_ожидание(byval период As Word)
' датчики
   Const нажат = 0
   Const отжат = 1
   левый_датчик Alias Pind.2
   правый_датчик Alias Pind.2
   центральный_датчик Alias Pind.2
   While период > 0
      If правый_датчик = отжат And левый_датчик = отжат And центральный_датчик = отжат Then
         Waitms 20
      Else
         скорость_левый = 255 : скорость_правый = 255
         If центральный_датчик = нажат Then
            направление_левый = назад
            направление_правый = назад
            Wait 1
            направление_левый = вперед
            Wait 1
            направление_правый = вперед
         Elseif правый_датчик = нажат Then
            направление_левый = назад
            направление_правый = вперед
            Waitms 500
         Elseif левый_датчик = нажат Then
            направление_левый = вперед
            направление_правый = назад
            Waitms 500
         End If
      End If
      Decr период
   Wend
End Sub

Sub искать_станцию()
End Sub

принять_команду_пульта:
   Disable Int1
   Enable Interrupts
   Getrc5(команда_пульта_адрес , команда_пульта)
   Enable Interrupts
   Dim флаг As Byte
   If команда_пульта_адрес = Ir_adress And команда_пульта <> 255 Then       ' используется пульт Минибота с кодом адреса = 0
      команда_пульта = команда_пульта And &B01111111
      Print Chr(12);
      'Print "IR_Address - " ; команда_пульта_адрес
      Print "IR_command - " ; команда_пульта
   End If
   ' обработка команды
   флаг = выполнить_команду()
   Gifr = Gifr Or &B10000000                                'Clear Flag Int1
   Enable Int1
   If флаг = команда_старт Then
      Goto Старт
   End If
Return

Function выполнить_команду()
   Const прибавка_скорости = 20
   Local возможная_скорость As Integer
   Local возвращаемое_значение As Byte : возвращаемое_значение = 0
   Select Case команда_пульта
   ' команды движения
      Case Btn_up:
         If состояние_робота <> управление_пультом Then
            выполнить_команду = возвращаемое_значение
            Exit Function
         End If
         возможная_скорость = скорость_левый + скорость_правый       ' выравниваем скорости движения
         возможная_скорость = возможная_скорость \ 2
         скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
         If направление_левый = назад And направление_правый = назад Then       ' движемся вперед или назад
            возможная_скорость = скорость_правый - прибавка_скорости
            If возможная_скорость < точка_останова Then
               направление_левый = вперед : направление_правый = вперед
               скорость_левый = точка_останова : скорость_правый = точка_останова
            Else
               скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
            End If
         Else
            возможная_скорость = скорость_правый + прибавка_скорости
            If возможная_скорость > 255 Then
               возможная_скорость = 255
            End If
            скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
         End If
      Case Btn_down:
         If состояние_робота <> управление_пультом Then
            выполнить_команду = возвращаемое_значение
            Exit Function
         End If
         If скорость_левый = точка_останова Or скорость_правый = точка_останова Then       ' если хотим ехать назад из танокового разворота
            направление_левый = назад : направление_правый = назад
         End If
         возможная_скорость = скорость_левый + скорость_правый       ' выравниваем скорости движения
         возможная_скорость = возможная_скорость \ 2
         скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
         If направление_левый = вперед And направление_правый = вперед Then       ' движемся вперед или назад
            возможная_скорость = скорость_правый - прибавка_скорости
            If возможная_скорость < точка_останова Then
               направление_левый = назад : направление_правый = назад
               скорость_левый = точка_останова : скорость_правый = точка_останова
            Else
               скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
            End If
         Else
            возможная_скорость = скорость_правый + прибавка_скорости
            If возможная_скорость > 255 Then
               возможная_скорость = 255
            End If
            скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
         End If
      Case Btn_left:
         If состояние_робота <> управление_пультом Then
            выполнить_команду = возвращаемое_значение
            Exit Function
         End If
         возможная_скорость = скорость_левый + прибавка_скорости
         If возможная_скорость < 255 Then
            скорость_левый = возможная_скорость
         Else
            скорость_левый = 255
            возможная_скорость = скорость_правый - прибавка_скорости
            If возможная_скорость > точка_останова Then
               скорость_правый = возможная_скорость
            Else
               скорость_правый = точка_останова
            End If
         End If
      Case Btn_right:
         If состояние_робота <> управление_пультом Then
            выполнить_команду = возвращаемое_значение
            Exit Function
         End If
         возможная_скорость = скорость_правый + прибавка_скорости
         If возможная_скорость < 255 Then
            скорость_правый = возможная_скорость
         Else
            скорость_правый = 255
            возможная_скорость = скорость_левый - прибавка_скорости
            If возможная_скорость > точка_останова Then
               скорость_левый = возможная_скорость
            Else
               скорость_левый = точка_останова
            End If
         End If
      Case Btn_ok:
         If состояние_робота <> управление_пультом Then
            Exit Sub
         End If
         направление_левый = вперед : направление_правый = вперед
         скорость_левый = точка_останова : скорость_правый = точка_останова
      Case Btn_power:
         ' ПОДУМАТЬ НАД РЕЖИМАМИ, может возращать бота в состояние_робота = 0, т.е. точку старта
         направление_левый = вперед : направление_правый = вперед
         скорость_левый = 0 : скорость_правый = 0
   ' команды состояния
      Case Btn_s:
         состояние_робота = управление_пультом
         возвращаемое_значение = команда_старт
         выполнить_команду =  возвращаемое_значение
      Case Btn_p
         состояние_робота = движение_улиткой
         возвращаемое_значение = команда_старт
         выполнить_команду =  возвращаемое_значение
      Case Btn_f:
         состояние_робота = поиск_станции
         возвращаемое_значение = команда_старт
         выполнить_команду =  возвращаемое_значение
      Case Else:
         ' ни одна из вышеперечисленных кнопок
   End Select
End Function