roboforum.ru

Концепцию обсуждаем тут viewtopic.php?f=65&t=7241

Здесть только сам код.

Цель данного кода:
1. ИК-дальномером Sharp посаженому на серву выполнять автоматическое сканирование пространства перед роботом
2. При уменьшении дистанции ниже определенного уровня передать в уарт соответвующий пакет


Текущий код

Код: Выделить всё

'$prog &HFF , &HE0 , &HDD , &HF9                             ' для минибота 2.x

$regfile = "m88DEF.dat"
$crystal = 7372800
$baud = 115200
'Config Serialin = Buffered , Size = 10

$hwstack = 64
$swstack = 64
$framesize = 64

Enable Interrupts                                           'разрешаем прерывания
'Enable Urxc
On Urxc Getchar                                             'переопределяем прерывание приема usart

Config Adc = Single , Prescaler = Auto , Reference = Internal       'опора 1.1В
Dim Дистанция As Word

Dim Text As String * 15                                     'строка для отправки/приема (длина строки 15 символов)
Dim Text_tmp As String * 15
Dim Txt_ As Byte

Config Servos = 1 , Servo1 = Portb.2 , Reload = 20
Config Pinb.2 = Output
Enable Interrupts

Dim Пеленг As Byte

Waitms 100
Print "Start Mega88 MiniBot 2.1"


'Config Pinb.2 = Input


Do
   Основной_цикл:
   Gosub Сканирование
Loop




Сканирование:
Do
   For Пеленг = 35 To 65
     Servo(1) = Пеленг
     Waitms 4
     Gosub Sharp
   Next
   For Пеленг = 65 To 35 Step -1
     Servo(1) = Пеленг
     Waitms 4
     Gosub Sharp
   Next
Loop
Return

Сканирование_стоп:
   Servo(1) = 50
   Goto Основной_цикл
Return

Getchar:
   Txt_ = Inkey()
   If Txt_ > 13 Then                                        'если пришел не служебный символ
      Text_tmp = Chr(txt_)
      Text = Text + Text_tmp
   End If
   If Txt_ = 13  Then'нажали ввод
         'строка в Text принята
   End If
   If Text = "M88_Scan_on" Then Gosub Сканирование
   If Text = "M88_Scan_off" Then Gosub Сканирование_стоп
Return

Sharp:
   Start Adc
   Дистанция = Getadc(0) : Дистанция = Getadc(0) : Дистанция = Getadc(0):
   Дистанция = Дистанция / 10
   Stop Adc
   Print Chr(12);
   Print Пеленг ; "-" ; Дистанция
Return



1. проблема, если включить Enable Urxc то серва начинает тормозить безбожно
2. так как пол относительно шарпа "бегает" по радиусу, то нужно что-то придумывать с оперативным запоминанием дистанции на каждом пеленге, т.к. при верчении головой дистанция относительно шарпа постоянно изменяется

Добавлено спустя 18 минут 29 секунд:
по второму вопросу я так понимаю нужно строить табличку пеленга с шагом 5 попугаев и постоянно ее перезаписывать, перед этим сравния с предыдущим значением и если разница больше чем позволительно по нашей религии, то кричим в уарт меге32 что у нас по такому-то пеленгу изменение дистанции до препятсвия
я правильно понимаю или это всё глупости?

попробуем так:

Код: Выделить всё

'$prog &HFF , &HE0 , &HDD , &HF9                             ' для минибота 2.x

$regfile = "m88DEF.dat"
$crystal = 7372800
$baud = 115200
'Config Serialin = Buffered , Size = 10

$hwstack = 64
$swstack = 64
$framesize = 64

Enable Interrupts                                           'разрешаем прерывания
'Enable Urxc
On Urxc Getchar                                             'переопределяем прерывание приема usart

Config Adc = Single , Prescaler = Auto , Reference = Internal       'опора 1.1В
Dim Дистанция As Word

Dim Text As String * 15                                     'строка для отправки/приема (длина строки 15 символов)
Dim Text_tmp As String * 15
Dim Txt_ As Byte

Config Servos = 1 , Servo1 = Portb.2 , Reload = 20
Config Pinb.2 = Output
Dim Пеленг As Byte

Dim Расстояния(7) As Word
Dim Буфер As Byte

Enable Interrupts

Waitms 100
Print "Start Mega88 MiniBot 2.1"


'Config Pinb.2 = Input

Gosub Считывание_расстояний
Do
   Основной_цикл:
   Gosub Сканирование
Loop

Сканирование:
Do
   For Пеленг = 35 To 65 Step 5
     Servo(1) = Пеленг
     Waitms 15
     Gosub Sharp
   Next
   For Пеленг = 65 To 35 Step -5
     Servo(1) = Пеленг
     Waitms 15
     Gosub Sharp
   Next
Loop
Return

Считывание_расстояний:
   Start Adc
   Буфер = 1
   For Пеленг = 35 To 65 Step 5
     Servo(1) = Пеленг
     Waitms 15
     Дистанция = Getadc(0) : Дистанция = Getadc(0) : Дистанция = Getadc(0)
     Расстояния(Буфер) = Дистанция : Incr Буфер
   Next
   Stop Adc
   ' вывод
   Print "Dist: " ;
   For Буфер = 1 To 7
      Print Расстояния(Буфер) / 10 ; " ";
   Next
   Print
Return

Сканирование_стоп:
   Servo(1) = 50
   Goto Основной_цикл
Return

Getchar:
   Txt_ = Inkey()
   If Txt_ > 13 Then                                        'если пришел не служебный символ
      Text_tmp = Chr(txt_)
      Text = Text + Text_tmp
   End If
   If Txt_ = 13  Then'нажали ввод
         'строка в Text принята
   End If
   If Text = "M88_Scan_on" Then Gosub Сканирование
   If Text = "M88_Scan_off" Then Gosub Сканирование_стоп
Return

Sharp:
   Const Граница = 10
   Start Adc
   Дистанция = Getadc(0) : Дистанция = Getadc(0) : Дистанция = Getadc(0)
   Stop Adc
   Буфер = Пеленг - 30 : Буфер = Буфер \ 5
   Дистанция = Дистанция + Граница
   If Дистанция < Расстояния(Буфер) Then
      Дистанция = Дистанция - Граница : Дистанция = Дистанция / 10
      Print Chr(12);
      Print Пеленг ; "-" ; Дистанция
   End If
Return


а давай для наглядности выведем все 7 значений в одну строку

вывод для 35-65

Код: Выделить всё

'$prog &HFF , &HE0 , &HDD , &HF9                             ' для минибота 2.x

$regfile = "m88DEF.dat"
$crystal = 7372800
$baud = 115200
'Config Serialin = Buffered , Size = 10

$hwstack = 64
$swstack = 64
$framesize = 64

Enable Interrupts                                           'разрешаем прерывания
'Enable Urxc
On Urxc Getchar                                             'переопределяем прерывание приема usart

Config Adc = Single , Prescaler = Auto , Reference = Internal       'опора 1.1В
Dim Дистанция As Word

Dim Text As String * 15                                     'строка для отправки/приема (длина строки 15 символов)
Dim Text_tmp As String * 15
Dim Txt_ As Byte

Config Servos = 1 , Servo1 = Portb.2 , Reload = 20
Config Pinb.2 = Output
Dim Пеленг As Byte

Dim Расстояния(7) As Word
Dim Буфер As Byte

Enable Interrupts

Waitms 100
Print
Print "Start Mega88 MiniBot 2.1"


'Config Pinb.2 = Input

Gosub Считывание_расстояний
Do
   Основной_цикл:
   Gosub Сканирование
Loop

Сканирование:
Do
   For Пеленг = 35 To 65 Step 5
     Servo(1) = Пеленг
     Waitms 15
     Gosub Sharp
   Next
   'For Пеленг = 65 To 35 Step -5
   '  Servo(1) = Пеленг
   '  Waitms 15
   '  Gosub Sharp
   'Next
Loop
Return

Считывание_расстояний:
   Start Adc
   Буфер = 1
   For Пеленг = 35 To 65 Step 5
     Servo(1) = Пеленг
     Waitms 15
     Дистанция = Getadc(0) : Дистанция = Getadc(0) : Дистанция = Getadc(0)
     Расстояния(Буфер) = Дистанция : Incr Буфер
   Next
   Stop Adc
   ' вывод
   Print "Dist: " ;
   For Буфер = 1 To 7
      Print Расстояния(Буфер) ; " ";
   Next
   Print
Return

Сканирование_стоп:
   Servo(1) = 50
   Goto Основной_цикл
Return

Getchar:
   Txt_ = Inkey()
   If Txt_ > 13 Then                                        'если пришел не служебный символ
      Text_tmp = Chr(txt_)
      Text = Text + Text_tmp
   End If
   If Txt_ = 13  Then'нажали ввод
         'строка в Text принята
   End If
   If Text = "M88_Scan_on" Then Gosub Сканирование
   If Text = "M88_Scan_off" Then Gosub Сканирование_стоп
Return

Sharp:
   Const Граница = 20
   Start Adc
   Дистанция = Getadc(0) : Дистанция = Getadc(0) : Дистанция = Getadc(0)
   Stop Adc
   Буфер = Пеленг - 30 : Буфер = Буфер \ 5
   Дистанция = Дистанция / 10
   If Буфер = 7 Then
      Print Дистанция ;
      Waitms 200                                            ' задержка на посмотреть
      Print Chr(13);
   Else
      Print Дистанция ; " " ;
   End If


   'Дистанция = Дистанция + Граница
   'If Дистанция < Расстояния(Буфер) Then
   '   Дистанция = Дистанция - Граница : Дистанция = Дистанция / 10
   '   Print Chr(13);
   '   Print
   '   Print Пеленг ; "-" ; Дистанция ;
   'End If

Return


Текущая Пашина версия, выдает в уарт только при изменении дистанции выше порогового уровня константы "Граница"
Причешу завтра

Код: Выделить всё

'$prog &HFF , &HE0 , &HDD , &HF9                             ' для минибота 2.x

$regfile = "m88DEF.dat"
$crystal = 7372800
$baud = 115200
'Config Serialin = Buffered , Size = 10

$hwstack = 64
$swstack = 64
$framesize = 64

Enable Interrupts                                           'разрешаем прерывания
'Enable Urxc
On Urxc Getchar                                             'переопределяем прерывание приема usart

Config Adc = Single , Prescaler = Auto , Reference = Internal       'опора 2.04В
Dim Дистанция As Word
Dim Дифф As Integer

Dim Text As String * 15                                     'строка для отправки/приема (длина строки 15 символов)
Dim Text_tmp As String * 15
Dim Txt_ As Byte

Config Servos = 1 , Servo1 = Portb.2 , Reload = 20
Config Pinb.2 = Output
Dim Пеленг As Byte

Dim Расстояния(7) As Word
Dim Буфер As Byte

Enable Interrupts

Waitms 1000
Print
Print "Start Mega88 MiniBot 2.1"


'Config Pinb.2 = Input

Gosub Считывание_расстояний
Do
   Основной_цикл:
   Gosub Сканирование
Loop

Сканирование:
Do
   For Пеленг = 35 To 65 Step 5
     Servo(1) = Пеленг
     Waitms 50
     Gosub Sharp
   Next
   For Пеленг = 65 To 35 Step -5
     Servo(1) = Пеленг
     Waitms 15
     Gosub Sharp
   Next
Loop
Return

Считывание_расстояний:
   Start Adc
   Буфер = 1
   For Пеленг = 35 To 65 Step 5
     Servo(1) = Пеленг
     Waitms 50
     Дистанция = Getadc(0) : Дистанция = Getadc(0) : Дистанция = Getadc(0)
     Расстояния(Буфер) = Дистанция : Incr Буфер
   Next
   Stop Adc
   ' вывод
   Print "Dist: " ;
   For Буфер = 1 To 7
      Print Расстояния(Буфер) ; " ";
   Next
   Print
Return

Сканирование_стоп:
   Servo(1) = 50
   Goto Основной_цикл
Return

Getchar:
   Txt_ = Inkey()
   If Txt_ > 13 Then                                        'если пришел не служебный символ
      Text_tmp = Chr(txt_)
      Text = Text + Text_tmp
   End If
   If Txt_ = 13 Then                                        'нажали ввод
         'строка в Text принята
   End If
   If Text = "M88_Scan_on" Then Gosub Сканирование
   If Text = "M88_Scan_off" Then Gosub Сканирование_стоп
Return

Sharp:
  Const Граница = 70
   Start Adc
   Дистанция = Getadc(0) : Дистанция = Getadc(0) : Дистанция = Getadc(0)
   Stop Adc
   Буфер = Пеленг - 30 : Буфер = Буфер \ 5
   Дифф = Дистанция - Расстояния(Буфер)
   Дифф = Abs(Дифф)
   Расстояния(Буфер) = Дистанция
   If Дифф > Граница Then
      Print Chr(13);
      Дистанция = Дистанция / 10
      Print Пеленг ; "-" ; Дистанция ;
   End If
Return



причесал я. завел репо проекта - http://minibot.googlecode.com/svn/trunk/m88_head

Код: Выделить всё

'$prog &HFF , &HE0 , &HDD , &HF9                             ' для минибота 2.x
$regfile = "m88DEF.dat"
$crystal = 7372800
$baud = 115200
$hwstack = 64
$swstack = 64
$framesize = 64

' Голова
Config Adc = Single , Prescaler = Auto , Reference = Internal       'опора 2.04В
   Dim дистанция As Word
   Dim дифф As Integer

Config Servos = 1 , Servo1 = Portb.2 , Reload = 20
   Config Pinb.2 = Output
'Config Timer1 = Pwm , Pwm = 10 , Prescale = 64 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down
'   Servo1 Alias Pwm1b
   'Config Pinb.2 = Output
   Dim пеленг As Byte
   Dim расстояния(7) As Word
   Dim буфер As Byte

' Связь
Dim Text As String * 15                                     'строка для отправки/приема (длина строки 15 символов)
Dim Text_tmp As String * 15
Dim Txt_ As Byte

Enable Urxc
On Urxc Getchar                                             'переопределяем прерывание приема usart


Waitms 1000
   Reset Ucsr0b.rxen0
      Print
      Print "Start Mega88 MiniBot 2.1"
   Set Ucsr0b.rxen0
Enable Interrupts                                           'разрешаем прерывания


Do
   основной_цикл:
   Gosub сканирование
Loop

Getchar:
   Txt_ = Inkey()
   If Txt_ > 13 Then                                        'если пришел не служебный символ
      Text_tmp = Chr(txt_)
      Text = Text + Text_tmp
   End If
   If Txt_ = 13 Then                                        'нажали ввод
         'строка в Text принята
   End If
   If Text = "M88_Scan_on" Then Gosub сканирование
   If Text = "M88_Scan_off" Then Gosub сканирование_стоп
Return

сканирование:
Do
   For пеленг = 35 To 65 Step 5
     Servo(1) = пеленг
     'Servo1 = пеленг
     Waitms 50
     Gosub Sharp
   Next
   For пеленг = 65 To 35 Step -5
     Servo(1) = пеленг
     'Servo1 = пеленг
     Waitms 50
     Gosub Sharp
   Next
Loop
Return

сканирование_стоп:
   Servo(1) = 50
   'Servo1 = пеленг
   Goto основной_цикл
Return

Sharp:
  Const порог_срабатывания = 70
   Start Adc
   дистанция = Getadc(0) : дистанция = Getadc(0) : дистанция = Getadc(0)
   Stop Adc
   буфер = пеленг - 30 : буфер = буфер \ 5
   дифф = дистанция - расстояния(буфер)
   дифф = Abs(дифф)
   расстояния(буфер) = дистанция
   If дифф > порог_срабатывания Then
      дистанция = дистанция / 10
      Reset Ucsr0b.rxen0
         Print Chr(13);
         Print пеленг ; "-" ; дистанция ;
      Set Ucsr0b.rxen0
   End If
Return


Добавлено спустя 1 час 30 минут 8 секунд:
избавился от зависаний при включении прерывания (причина - не отключен приемник во время передачи)

с последним кодом после каждого поворота головы мега88 сбрасывается

посаженная батарея?

не... у акб заряд 100%

вместе с Эдом исправили, вкл/выкл сканирования на on/off (временно).

Код: Выделить всё

'$prog &HFF , &HE0 , &HDD , &HF9                             ' для минибота 2.x
$regfile = "m88DEF.dat"
$crystal = 7372800
$baud = 115200
$hwstack = 64
$swstack = 64
$framesize = 64

' Голова
Config Adc = Single , Prescaler = Auto , Reference = Internal       'внешняя опора 2.04В
   Dim дистанция As Word
   Dim дифф As Integer

'Config Servos = 1 , Servo1 = Portb.2 , Reload = 20
'   Config Pinb.2 = Output
Config Timer1 = Pwm , Pwm = 10 , Prescale = 64 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down
   Servo1 Alias Pwm1b
   Config Pinb.2 = Output
   Dim пеленг As Byte
   Dim расстояния(7) As Word
   Dim буфер As Byte
   Dim сканировать As Byte
      Const да = 0
      Const нет = 1
   сканировать = нет

' Функции и процедуры
Declare Sub Printf(byval S As String)
   Dim Text As String * 50                                  ' буфер-строка для отправки (нужна из-за недопустимости вложенных функция)

' Связь
Enable Urxc
On Urxc Getchar                                             'переопределяем прерывание приема usart
   Dim In_string As String * 50                             'строка для приема (длина строки 15 символов)
   Dim In_key As Byte                                       ' текущий принятый символ

Waitms 1000
   Call Printf( "start minibot mega88" )
   Call Printf( "test")

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


Do
   основной_цикл:
   If сканировать = да Then Gosub сканирование
Loop

Getchar:
   In_key = Inkey()
   If In_key <> 13 And In_key <> 10 Then                    ' принимаем пока не встретим конец строки
         In_string = In_string + Chr(in_key)
   Else                                                     'text_tmp содержит принятую строку
      If In_string = "on" Then сканировать = да
      If In_string = "off" Then сканировать = нет
      'Mid(in_string , 1 , 1) = "-" ' отладочный вывод
      'Call Printf(in_string)
      In_string = ""
   End If
Return

сканирование:
Do
   For пеленг = 35 To 65 Step 5
     'Servo(1) = пеленг
     Servo1 = пеленг
     Waitms 50
     Gosub Sharp
   Next
   For пеленг = 65 To 35 Step -5
     'Servo(1) = пеленг
     Servo1 = пеленг
     Waitms 50
     Gosub Sharp
   Next
Loop Until сканировать = нет
'Servo(1) = 50
Servo1 = 50
Return

Sharp:
  Const порог_срабатывания = 100
   Start Adc
   дистанция = Getadc(0) : дистанция = Getadc(0) : дистанция = Getadc(0)
   Stop Adc
   буфер = пеленг - 30 : буфер = буфер \ 5
   дифф = дистанция - расстояния(буфер)
   дифф = Abs(дифф)
   расстояния(буфер) = дистанция
   If дифф > порог_срабатывания Then
      дистанция = дистанция / 10
      Text = Str(пеленг) + "-" + Str(дистанция) : Call Printf(text)
   End If
Return

Sub Printf(byval S As String)
   Local I As Byte
   Local Len1 As Byte
   Local Buf As String * 1
   Reset Ucsr0b.rxen0
   Len1 = Len(s)
   For I = 1 To Len1
      While Ucsr0a.udre0 = 0                                ' ожидаю момента, когда можно загрузить новые данные
      Wend
      Buf = Mid(s , I , 1)
      Udr0 = Asc(buf)
   Next
      While Ucsr0a.udre0 = 0
      Wend
      Udr0 = 13
      While Ucsr0a.udre0 = 0
      Wend
      Udr0 = 10
      Set Ucsr0a.txc0                                       ' очищаю флаг прерывания
      While Ucsr0a.txc0 = 0                                 ' жду пока передатчик не передаст весь пакет (из внутреннего буфера, не UDR)
      Wend
   Set Ucsr0b.rxen0
End Sub


возможные проблемы - коллизии на шине

проверил, код отлично работает.
пояснения для тех кто не в курсе
если послать в уарт "on" начинает крутить сервой и шлет в уарт изменения дистанции на пеленге
если послать "off", то соответсвенно перестает.