roboforum.ru

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Выложу для примера код, использованный на прототипе #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

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

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

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

'$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тью кнопками - вверх, вниз, влево, вправо, ок, питание. конпка ок сбрасывает скважность шим до точки_останова, конпка питание - в ноль. таким образом на парковку бот ставится нажатием "питания", а оставнавливается (или снимается с парковки) - нажатием "ок". остальные кнопки - чем дольше жмем, тем выраженнее становится движение бота в эту сторону.
кнопка вперед/назад выравнивает скорости двигателей и одновременно увеличивает/уменьшает.
кнопки влево/вправо вначале увеличивают скорость соответствующео двигателя, при достижении максимума уменьшают скорость противоположного до точки_останова, танковый разворот не предусмотрен.
вообще идея такая, что для смнеы направления движения нужно нажимать вперед/стоп/назад. вотнекоторые ситуации:

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

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

• 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. а зачем пылесосу режим сохранения команд?

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