roboforum.ru

[EdGull]

Т.к. разработка механики на основе автомобильного пылесоса подходит к концу viewtopic.php?f=58&t=6794 , то пора писать код.
Здесь будем выкладывать все этапы программирования робота-пылесоса.
За основу будет браться заводская тестовая прошивка, но переносить будем по частям.

Допускаются небольшие обсуждения, но только в свете кода.
Категориеские запрещен троллинг, филосовствование и т.д.

И так, что хотелось бы сделать:
1. Управление с ИК-пульта по прерываниям
2. Запись и исполнение программ с пришедших с пульта
3. Поиск зарядки

[galex1981]

Я раньше тебе Эд предлагал по количеству принятых комманд: каждая команда имеет свое время исполнения (находится экспериментально). Пультом при записи, по этой же длительности команды пишутся в исполняемый файл. А при воспроизведении команды из файла по своей длительности и исполняются.

[EdGull]

ну это я и указал в пункте 4

[repository]

Господа, может сразу в CVS проект заведете?
А то потом будет мучительно больно:)

[EdGull]

не для людей эти ваши CVS, все какие-то нет от мира сего
как пользоваться этими вашими CVS ваще не понятно
так что мы уж какнибудь по старинке

[MiBBiM]

репозиторий советует CVS. монументально
без энкодеров никак не получится сохранение команд, связанных с перемещением. (вариант "ехать с шимом 50 три миллисекунды" не очень точен)

[EdGull]

1. энкодеры от проскальзывания колес тоже не спасут.
2. можно относительно УЗ-маяков знать свое положение.

[repository]

не для людей эти ваши CVS, все какие-то нет от мира сего
как пользоваться этими вашими CVS ваще не понятно
так что мы уж какнибудь по старинке

Я к тебе слишком хорошо отношусь, что бы не посоветовать контроль версий. А уж что и как - дело твое:)

[EdGull]

ну так и завел бы этот свой CVS, подготовил бы почву так сказать...
а здесь бы написал, что так мол и так, всё готово и смотрите как всё просто и удобно там держать все ваши версии...

[repository]

Как называется по англицки проект?

[EdGull]

ты не поверишь, MiniBot

Добавлено спустя 28 секунд:
или BroomBot, на твой выбор

[SMT]

Выложу для примера код, использованный на прототипе #6. Здесь используется плата практически аналогичная по контактам\сигналам Минибот-у - по этому должно быть повторяемо на Минибот-совместимых контроллерах (да и подкорректировать легко). Переменные и метки были написаны на английском - это я перевел на русский, чтобы показать начинающим как просто на самом деле выглядит программа (этот вариант не загружал в робота, но он компилируется).

Компоновка треугольная - два колеса сзади, большой "вынос" корпуса вперед. Три датчика в виде кнопок "на нажатие", замыкающих цепь с резистором 1к от +питания на ножки 0,1,3 порта A (датчики линии и энкодера на Миниботе).
Используется режим уборки "улиткой", с постепенным расширением радиуса поворота. При встрече с препятствием - один из датчиков бампера нажат - переключение на отворот (или отъезд и отворот для переднего датчика), с дальнейшим движением вперед. Через некоторое время снова включается движение по "улитке".

Код разрабатывался для тестирования, из недостатков - короткое движение по прямой (легко исправить в метке "Двигаться_вперед"), может зациклиться в углу (yak40 предложил считать отвороты и менять стратегию движения).

Код: Выделить всё • Развернуть

$regfile = "m32DEF.dat"
$crystal = 8000000
$hwstack = 32
$swstack = 10
$framesize = 40

Portc = &B11111111
Config Pinc.2 = Output : Левый_назад Alias Portc.2
Config Pinc.3 = Output : Правый_назад Alias Portc.3

Porta = &B00000000
Config Pina.0 = Input : Левый_датчик Alias Pina.0
Config Pina.1 = Input : Правый_датчик Alias Pina.1
Config Pina.3 = Input : Передний_датчик Alias Pina.3        'была проблема с контактом на PINA.2, по этому не по порядку

Config Timer1 = Pwm , Pwm = 8 , Prescale = 1 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down

Const Нажат = 1 : Const Light = 0
Dim Минимальная_скорость As Integer
Dim Максимальная_скорость As Integer
Dim Скорость_левого As Integer

Минимальная_скорость = 150
Максимальная_скорость = 255
Скорость_левого = Минимальная_скорость

Dim Период As Long
Dim Увеличение As Long
Период = 50
Увеличение = 1

Левый_назад = 0
Правый_назад = 0

Do
   Скорость_левого = Минимальная_скорость                   'left
   Pwm1a = Максимальная_скорость                            'right
   Gosub Двигаться_по_спирали
   Gosub Двигаться_вперед
Loop
End

Двигаться_по_спирали:
   While Скорость_левого < Максимальная_скорость
      Pwm1b = Скорость_левого
      Gosub Время_ожидания
      Скорость_левого = Скорость_левого + Увеличение
   Wend
Return

Двигаться_вперед:
   Dim Счетчик_движения_вперед As Integer
   Счетчик_движения_вперед = 3                              '3 sec
   While Счетчик_движения_вперед >= 0
      Pwm1b = Максимальная_скорость
      Pwm1a = Максимальная_скорость
      Gosub Время_ожидания
      Decr Счетчик_движения_вперед
   Wend
        Pwm1b = 0 : Pwm1a = 0
 Return

Время_ожидания:
Dim Count As Long
Count = Период
While Count > 0
      If Правый_датчик <> Нажат And Левый_датчик <> Нажат And Передний_датчик <> Нажат Then
         Decr Count
         Waitms 20
      Else
         Pwm1b = Максимальная_скорость : Pwm1a = Максимальная_скорость
         If Передний_датчик = Нажат Then
               Левый_назад = 1
               Правый_назад = 1
               Wait 1
               Левый_назад = 0
               Wait 1
               Правый_назад = 0
         Elseif Правый_датчик = Нажат Then
               Левый_назад = 1
               Правый_назад = 0
               Waitms 500
               Левый_назад = 0
         Elseif Левый_датчик = Нажат Then
               Левый_назад = 0
               Правый_назад = 1
               Waitms 500
               Правый_назад = 0
         End If
         Скорость_левого = Максимальная_скорость
         Return
      End If
Wend
Return

[EdGull]

Как написать чтобы, чем дольше я жму кнопку "Влево" тем на больший угол и более резче он бы поворачивал?
И без остановок.
А то сейчас он повернется, подождет, дальше повернется и т.д.

[MiBBiM]

моделька повеселее, код почти на русском =)

Код: Выделить всё • Развернуть

'$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
скорость_левый 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


' Процедуры
Declare Sub Processcommand()

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

' Запуск
Enable Interrupts

Waitms 200
Print "Start MiniBot v2.1"

Do
   Idle
Loop

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

Sub Processcommand()
   Const прибавка_скорости = 20
   Const точка_останова = 50
   Local возможная_скорость As Integer
   Select Case команда_пульта
      Case Btn_up:
         возможная_скорость = скорость_левый + скорость_правый       ' выравниваем скорости движения
         возможная_скорость = возможная_скорость \ 2
         скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
         If направление_левый = назад And направление_правый = назад Then       ' движемся вперед или назад
            возможная_скорость = скорость_правый - прибавка_скорости
            If возможная_скорость < точка_останова Then
               направление_левый = вперед : направление_правый = вперед
               скорость_левый = точка_останова : скорость_правый = точка_останова
            Else
               скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
            End If
         Else
            возможная_скорость = скорость_правый + прибавка_скорости
            If возможная_скорость > 255 Then
               возможная_скорость = 255
            End If
            скорость_левый = возможная_скорость : скорость_правый = возможная_скорость
         End If
      Case Btn_down:
         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 возможная_скорость < 255 Then
            скорость_левый = возможная_скорость
         Else
            скорость_левый = 255
            возможная_скорость = скорость_правый - прибавка_скорости
            If возможная_скорость > точка_останова Then
               скорость_правый = возможная_скорость
            Else
               скорость_правый = точка_останова
            End If
         End If
      Case Btn_right:
         возможная_скорость = скорость_правый + прибавка_скорости
         If возможная_скорость < 255 Then
            скорость_правый = возможная_скорость
         Else
            скорость_правый = 255
            возможная_скорость = скорость_левый - прибавка_скорости
            If возможная_скорость > точка_останова Then
               скорость_левый = возможная_скорость
            Else
               скорость_левый = точка_останова
            End If
         End If
      Case Btn_ok:
         направление_левый = вперед : направление_правый = вперед
         скорость_левый = точка_останова : скорость_правый = точка_останова
      Case Btn_power:
         направление_левый = вперед : направление_правый = вперед
         скорость_левый = 0 : скорость_правый = 0
      Case Else:
         ' ни одна из вышеперечисленных кнопок
   End Select
End Sub

нужно настраивать:

• параметр точка_останова - должен быть минимальным значением скважности шим, при котором двигатели не крутяться
• параметр прибавка_скорости - прибавка скважности шим с каждым приходом команды, от неё зависит резкоть управления

управление осуществляется 6тью кнопками - вверх, вниз, влево, вправо, ок, питание. конпка ок сбрасывает скважность шим до точки_останова, конпка питание - в ноль. таким образом на парковку бот ставится нажатием "питания", а оставнавливается (или снимается с парковки) - нажатием "ок". остальные кнопки - чем дольше жмем, тем выраженнее становится движение бота в эту сторону.
кнопка вперед/назад выравнивает скорости двигателей и одновременно увеличивает/уменьшает.
кнопки влево/вправо вначале увеличивают скорость соответствующео двигателя, при достижении максимума уменьшают скорость противоположного до точки_останова, танковый разворот не предусмотрен.
вообще идея такая, что для смнеы направления движения нужно нажимать вперед/стоп/назад. вотнекоторые ситуации:

• едем вперед, жмем кнопку налево, бот поворачивает. когда считаем, что поворот закончен, жмем либо стоп, либо вперед/назад.
• стоим на месте, жмем вправо, бот поворачивает. когда поворот окончен, жмем либо стоп, что остановит пылесос, либо вперед(назад), что выравняет скорости двигателей по направлению вперед(назад).
• стоим на месте, жмем вначале влево, потом вправо. бот начинает движение вперед. чтобы сразу повернуть вправо нужно после нажатия "влево" скомандовать либо стоп, либо вперед/назад

[MiBBiM]

добавил движение улиткой, переключение между режимами на кнопки

• S - управление с пульта
• P - движение улиткой (вместо сенсоров заглушка в виде кнопки pind.2)
• F - поиск станции (ещё нет)

ещё не тестировал, вечером отпишусь

Код: Выделить всё • Развернуть

'$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

Добавлено спустя 2 минуты 27 секунд:
P.S. а зачем пылесосу режим сохранения команд?

[EdGull]

что бы он как правильный дрессированный пёс умел выполнять команды которыми его научили