roboforum.ru

Добавил дебажный вывод и сразу нашел ошибку.
Исправил, запушил.

Также добавил в игнор меркуриала local_config.mk

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

vovan@vovan-pc:~/dev/project/orfa$ miniterm.py -e /dev/rfcomm0
--- Miniterm on /dev/rfcomm0: 9600,8,N,1. ---                 
--- Quit: Ctrl+]  |  Upload: Ctrl+U ---                       
v                                                             
V1.0                                                          
l00                                                           
L00                                                           
s000000s0102p                                                 
# > cmd_start(0x00, 0)                                        
# > cmd_txc(0x00)                                             
# > cmd_txc(0x00)                                             
# > cmd_start(0x01, 1)                                        
# \-> gate_register_write(0x00, buf, 1)                       
# \-> gate_register_read(0x00, buf, 1)                        
# > cmd_rxc(0x05, 1)                                          
# /-> gate_register_read(0x00, buf, 1)                        
# > cmd_rxc(0x05, 0)                                          
# > cmd_stop()                                                
SWAASR0505P                                                   
s000001p                                                      
# > cmd_start(0x00, 0)                                        
# > cmd_txc(0x00)                                             
# > cmd_txc(0x01)                                             
# > cmd_stop()                                                
# \-> gate_register_write(0x00, buf, 1)                       
SWAAP                                                         
s0102p                                                        
# > cmd_start(0x01, 1)                                        
# \-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x40, 0)
# > cmd_stop()
SR0040P
s000002p
# > cmd_start(0x00, 0)
# > cmd_txc(0x00)
# > cmd_txc(0x02)
# > cmd_stop()
# \-> gate_register_write(0x00, buf, 5)
SWAAP
s010cp
# > cmd_start(0x01, 1)
# \-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x40, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x10, 1)
# > cmd_rxc(0x02, 1)
# /-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x40, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x10, 1)
# > cmd_rxc(0x02, 0)
# > cmd_stop()
SR004001001002004001001002P

--- exit ---
vovan@vovan-pc:~/dev/project/orfa$

Добавлено спустя 1 минуту 1 секунду:

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

vovan@vovan-pc:~/dev/project/orfa$ orintro.py /dev/rfcomm0 
PyOR version: 0.1.6                                        
UIDs database version: 1.0.2                                                                                                                                     
Serial Gate protocol version: 1.0                                                                                                                                
Selected device: 0x00                                                                                                                                            
                                                                                                                                                                 
Drivers:                                                                                                                                                         
#1 RoboGPIO ADC driver                                                                                                                                           
ADC driver                                                                                                                                                       
ADC_CONF — write 2 byte (WO):                                                                                                                                    
|  byte 1: adc mode & refrence mode                                                                                                                              
|    bit 2: 8 bit/10 bit                                                                                                                                         
|    bit 0-1:                                                                                                                                                    
|      00: External refrence                                                                                                                                     
|      01: AVCC refrence                                                                                                                                         
|      10, 11: Internal refrence                                                                                                                                 
|  byte 2: enabled channels mask                                                                                                                                 
ADC_DATA — write 1 byte start channel, output 1 or 2 bytes.                                                                                                      

UID: 0x0040
Version: 1.0                                                                                                                                                     
Registers: 0x10, 0x11                                                                                                                                            
 ADC_CONF: 0x10                                                                                                                                                  
 ADC_DATA: 0x11                                                                                                                                                  
                                                                                                                                                                 
#2 RoboMD2 motor driver                                                                                                                                          
Motor driver                                                                                                                                                     
PWMn — motor n speed register (R/W)                                                                                                                              
DIRn — motor n direction register (R/W)                                                                                                                          

UID: 0x0060
Version: 1.0                                                                                                                                                     
Registers: 0x0C, 0x0D, 0x0E, 0x0F                                                                                                                                
     DIR1: 0x0E                                                                                                                                                  
     DIR2: 0x0F
     PWM1: 0x0C
     PWM2: 0x0D

#3 RoboBus SPI driver
SPI driver
<need more information about this driver>

UID: 0x0001
Version: 1.0
Registers: 0x0A, 0x0B
 SPI_CONF: 0x0B
 SPI_DATA: 0x0A

#4 RoboGPIO digital driver
Digital I/O driver
PORTc — port register (R/W)
DDRc — port direction (WO)

UID: 0x0020
Version: 1.0
Registers: 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09
     DDRA: 0x06
     DDRB: 0x07
     DDRC: 0x08
     DDRD: 0x09
    PORTA: 0x02
    PORTB: 0x03
    PORTC: 0x04
    PORTD: 0x05

Meta information:
     rbgp: PORTB.4, PORTD.7, PORTD.6
     gpio: 0, 0xe3, 0x03, 0x33

#5 Introspection driver
ORFA Introspection driver
INTRO register always 0x00
00 00 — get driver count
00 nn — get driver nn info

UID: 0x0000
Version: 1.0
Registers: 0x00
    INTRO: 0x00

vovan@vovan-pc:~/dev/project/orfa$

Добавлено спустя 2 часа 20 минут 44 секунды:
Добавил инкрементное чтение интроспектора:

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

vovan@vovan-pc:~/dev/project/orfa$ miniterm.py -e /dev/rfcomm0 
--- Miniterm on /dev/rfcomm0: 9600,8,N,1. ---                  
--- Quit: Ctrl+]  |  Upload: Ctrl+U ---                        
v                                                              
V1.0                                                           
l00                                                            
L00                                                            
s000000s0101p                                                  
# > cmd_start(0x00, 0)                                         
# > cmd_txc(0x00)                                              
# > cmd_txc(0x00)                                              
# > cmd_start(0x01, 1)                                         
# \-> gate_register_write(0x00, buf, 1)                        
# \-> gate_register_read(0x00, buf, 1)                         
# > cmd_rxc(0x05, 0)                                           
# > cmd_stop()                                                 
SWAASR05P
s000001p
# > cmd_start(0x00, 0)
# > cmd_txc(0x00)
# > cmd_txc(0x01)
# > cmd_stop()
# \-> gate_register_write(0x00, buf, 1)
SWAAP
s011ep
# > cmd_start(0x01, 1)
# \-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x40, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x10, 1)
# > cmd_rxc(0x02, 1)
# /-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x60, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x0c, 1)
# > cmd_rxc(0x04, 1)
# /-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x0a, 1)
# > cmd_rxc(0x02, 1)
# /-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x20, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x02, 1)
# > cmd_rxc(0x08, 1)
# /-> gate_register_read(0x00, buf, 6)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x01, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x00, 1)
# > cmd_rxc(0x01, 0)
# > cmd_stop()
SR004001001002006001000C04000101000A02002001000208000001000001P

В общем как я понял остался 1 драйвер недобитым - под сервы на OR-AVR-M32-D.

Давайте интерфейс что ли приземлим?

Предлагаю побитовую маску на 2 байта для выбора портов GPIO, которые будут настроены как выходы сервоприводов.

2blindman: А какой формат сейчас работы с сервами? Правильно ли я понимаю, что там сейчас более 255 положений и положение сервы это 2 байта? И еще - что передаются тройки чисел, каждая - номер сервы и её положение (при этом сначала старший, а потом младший байты) ?

А то я взялся писать драйвер для серв под GPIO и хочу совместимый с твоим форматом сделать.

Ну почему один драйвер?
Работы больше:

Нужно реализовать:
1. Драйвер серво
2. Драйвер емкостных датчиков

Нужно дополнить:
1. В драйвер моторов нужно добавить очередь команд, обработку энкодеров и ПИД.
2. В драйвер АЦП желательно добавить простую обработку

Изменить:
1. В serial gate нужно сделать буферизированный ввод (по прерываниям)
2. Также добавить проверку ввода в serialgate_supertask() и выход если пусто.
(нужно что бы разрешить работу других задач.)
3. Нужно включить сторожевой таймер.

Кроме того нужно оттестировать.

Так что работы хватает

Ну и всё-таки отсутствующий драйвер серво под OR-AVR-M32-D не позволяет его полноценно юзать.

О! И еще главное забыл: задокументировать интерфейс каждого драйвера.
Короче написать документацию.

Документацию - это да!

В общем я так понял регистры для драйвера начинаются с нуля. Так?

Да. Кстати нужно поправить в ядре этот момент.

В смысле сейчас не так?

Кстати, вот эскиз:

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

/*
 *  ORFA -- Open Robotics Firmware Architecture
 *
 *  Copyright (c) 2009 Vladimir Ermakov, Andrey Demenev, Anton Botov
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in
 *  all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 *  THE SOFTWARE.
 *****************************************************************************/

#ifndef OR_AVR_M32_D
#error servoGPIO: unsupported platform
#endif

#include <avr/io.h>
#include "core/common.h"
#include "core/driver.h"
#include <stdint.h>

#define RESOLUTION_IN_TICKS 160
#define RESOLUTION_TIME (7372800/RESOLUTION_IN_TICKS)

static uint8_t gpio_servo_pos[16];
static uint8_t gpio_servo_enb[16];

static GATE_RESULT driver_read(uint8_t reg, uint8_t* data, uint8_t* data_len);
static GATE_RESULT driver_write(uint8_t reg, uint8_t* data, uint8_t data_len);

static GATE_DRIVER driver = {
   .uid = 0x31,
   .major_version = 1,
   .minor_version = 0,
   .read = driver_read,
   .write = driver_write,
   .num_registers = 2,
};

static void set_enable(uint8_t n, uint8_t enable)
{
   gpio_servo_enb[n]=enable>0?1:0;
}

static void set_position(uint8_t n, uint16_t pos)
{
   if (n > 15) return;
   if (pos < 500) pos = 500;
   else if (pos > 2500) pos = 2500;

   gpio_servo_pos[n]=pos/RESOLUTION_TIME;
}


static GATE_RESULT driver_read(uint8_t reg, uint8_t* data, uint8_t* data_len)
{
   *data_len = 0;
   return GR_OK;
}

static GATE_RESULT driver_write(uint8_t reg, uint8_t* data, uint8_t data_len)
{
   if (reg > 1) {
      return GR_NO_ACCESS;
   }
   if (reg == 0){
      if (data_len != 4) {
         return GR_INVALID_DATA;
      };
      uint8_t byte=0;
      while(byte<4){
         uint8_t bit=0;
         while(bit<8){
            set_enable( (byte << 3) + bit , *data & 0x01);
            *data=*data>>1;
            bit++;
         };
         data++;
         byte++;
      };
      return GR_OR;
   };
   if (data_len < 3) {
      return GR_INVALID_DATA;
   }
   while (data_len) {
      uint16_t pos = data[2] + (data[1] << 8);
      set_position(*data, pos);
      data += 3;
      data_len -= 3;
      if (data_len < 3 || data_len > 252) {
         return GR_INVALID_DATA;
      }
   }
   return GR_OK;
}

GATE_RESULT init_servo_driver(void)
{

   //Prepare servo interrupt

   return gate_driver_register(&driver);
}

=DeaD= писал(а):2blindman: А какой формат сейчас работы с сервами? Правильно ли я понимаю, что там сейчас более 255 положений и положение сервы это 2 байта? И еще - что передаются тройки чисел, каждая - номер сервы и её положение (при этом сначала старший, а потом младший байты) ?

Все верно. Положение задаётся в микросекундах

Там нужна была эта правка: http://hg.vehq.ru/orfa/rev/555d7cf51e8f

Вот в процессе:

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

/*
 *  ORFA -- Open Robotics Firmware Architecture
 *
 *  Copyright (c) 2009 Vladimir Ermakov, Andrey Demenev, Anton Botov
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in
 *  all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 *  THE SOFTWARE.
 *****************************************************************************/

#ifndef OR_AVR_M32_D
#error servoGPIO: unsupported platform
#endif

#include <avr/io.h>
#include "core/common.h"
#include "core/driver.h"
#include <stdint.h>

#define RESOLUTION_IN_TICKS 160
#define RESOLUTION_TIME (7372800/RESOLUTION_IN_TICKS)
#define REST_IN_TICKS (255-RESOLUTION_IN_TICKS)
#define MAX_TMR (7372800/50/RESOLUTION_IN_TICKS)
#define MAXSERVO (7372800/400/RESOLUTION_IN_TICKS)
#define MINSERVO (7372800/2000/RESOLUTION_IN_TICKS)
#define WORKSPACE (MAXSERVO+1)
#define CODE_FOR_SERVO(servo_id,port,pin) if(gpio_servo_enb[servo_id]==1){\
                                 if(tmp==0){ outb(port,inb(port) | (1<<pin)); };\
                                    if(tmp==gpio_servo_pos[servo_id]){ outb(port,inb(port) & ~(1<<pin)); };\
                                 };



static uint8_t gpio_servo_pos[16];
static uint8_t gpio_servo_enb[16];
static uint16_t tmr,tmp;

static GATE_RESULT driver_read(uint8_t reg, uint8_t* data, uint8_t* data_len);
static GATE_RESULT driver_write(uint8_t reg, uint8_t* data, uint8_t data_len);

static GATE_DRIVER driver = {
   .uid = 0x31,
   .major_version = 1,
   .minor_version = 0,
   .read = driver_read,
   .write = driver_write,
   .num_registers = 2,
};

ISR(SIG_OVERFLOW1)
{
   TCNT0=REST_IN_TICKS;
   if(tmr<WORKSPACE*8){
      if(tmr<WORKSPACE*4){
         if(tmr<WORKSPACE*2){
            if(tmr<WORKSPACE){
               tmp=tmr;
                CODE_FOR_SERVO(0,PORTA,0);
               CODE_FOR_SERVO(1,PORTA,1);
            }else{
                uint8_t tmp=tmr-WORKSPACE;
               CODE_FOR_SERVO(2,PORTA,2);
               CODE_FOR_SERVO(3,PORTA,3);
            };
          }else{
            if(tmr<WORKSPACE*3){
               tmp=tmr-WORKSPACE*2;
               CODE_FOR_SERVO(4,PORTA,4);
               CODE_FOR_SERVO(5,PORTA,5);
            }else{
               tmp=tmr-WORKSPACE*3;
               CODE_FOR_SERVO(6,PORTA,6);
               CODE_FOR_SERVO(7,PORTA,7);
            };
          };
      }else{
         if(tmr<WORKSPACE*6){
            if(tmr<WORKSPACE*5){
               tmp=tmr-WORKSPACE*4;
               CODE_FOR_SERVO(8,PORTC,7);
               CODE_FOR_SERVO(9,PORTC,6);
            }else{
               tmp=tmr-WORKSPACE*5;
               CODE_FOR_SERVO(10,PORTC,5);
               CODE_FOR_SERVO(11,PORTC,4);
            };
         }else{
            if(tmr<WORKSPACE*7){
                tmp=tmr-WORKSPACE*6;
               CODE_FOR_SERVO(12,PORTB,3);
               CODE_FOR_SERVO(13,PORTB,2);
            }else{
                tmp=tmr-WORKSPACE*7;
               CODE_FOR_SERVO(14,PORTD,5);
               CODE_FOR_SERVO(15,PORTD,4);
            };
         };
      };
   };
   tmr++;
   if(tmr==MAX_TMR){ tmr=0; };
}

static void set_enable(uint8_t n, uint8_t enable)
{
   gpio_servo_enb[n]=enable>0?1:0;
}

static void set_position(uint8_t n, uint16_t pos)
{
   if (n > 15) return;
   pos=pos/RESOLUTION_TIME
   if (pos < MINSERVO) pos = MINSERVO;
   else if (pos > MAXSERVO) pos = MAXSERVO;;
   gpio_servo_pos[n]=pos;
}


static GATE_RESULT driver_read(uint8_t reg, uint8_t* data, uint8_t* data_len)
{
   *data_len = 0;
   return GR_OK;
}

static GATE_RESULT driver_write(uint8_t reg, uint8_t* data, uint8_t data_len)
{
   if (reg > 1) {
      return GR_NO_ACCESS;
   }
   if (reg == 0){
      if (data_len != 4) {
         return GR_INVALID_DATA;
      };
      uint8_t byte=0;
      while(byte<4){
         uint8_t bit=0;
         while(bit<8){
            set_enable( (byte << 3) + bit , *data & 0x01);
            *data=*data>>1;
            bit++;
         };
         data++;
         byte++;
      };
      return GR_OR;
   };
   if (data_len < 3) {
      return GR_INVALID_DATA;
   }
   while (data_len) {
      uint16_t pos = data[2] + (data[1] << 8);
      set_position(*data, pos);
      data += 3;
      data_len -= 3;
      if (data_len < 3 || data_len > 252) {
         return GR_INVALID_DATA;
      }
   }
   return GR_OK;
}

GATE_RESULT init_servo_driver(void)
{
   uint8_t i;
   for(i=0; i<16; i++){
     set_enable(i, 0);
     set_position(i, 1500);
   };

   //Prepare TIMER0 interrupt
   TCCR0=0x01;
   TCNT0=REST_IN_TICKS;
   tmr=0;
   TIMSK|=0x01;
   sei();

   return gate_driver_register(&driver);
}

Добавлено спустя 8 минут 42 секунды:
Еще мысль - нужно и можно ли сделать, чтобы генерируемая прошивка включала id контроллера и скорость уарт? Например, "or-avr-m32-d-9600.hex" ?

Добавлено спустя 28 минут 53 секунды:
Еще - может вынести определение частоты УАРТа туда же, где платформу определяем, а не внутрь платформы зашивать?

115200 стоит по умолчанию,задаётся в файле конфигурации платформы. Ничто не мешает в командной строке или локальном файле конфигурации прописать

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

BAUD = B9600

Ага, Vooon уже мне сказал, я в вику даже это уже прописал.

Почти доделал драйвер сервоприводов под OR-AVR-M32-D. Выложил в репо. Только залип с парсером похоже.
Не обрабатывается:
L00
W0010FFFF
выдаёт
ERROR 04

ну и W00110005FF тоже не хочет работать.

А так выставляет уже сервоприводы в положения, которые заданы в servo_gpio_position[] массиве.
Правда я не смотрел пока по длине импульсов - не подкосячил ли.

Добавлено спустя 15 минут 39 секунд:
Так, похоже это мои прерывания на сервы не дают обрабатываться нормально UART'у.

Добавлено спустя 3 минуты 36 секунд:
А нет, всё-таки:
L00
W0010FFFF парсером не отрабатывается нормально.

PS: На всякий случай замечу, что речь идёт о драйвере сервоприводов под GPIO-порты, под контроллер OR-AVR-M32-D.
К сервоконтроллеру отношения это не имеет.

Добавлено спустя 14 минут 26 секунд:
Что-то у меня грустные расчеты получаются.
На скорости 115200 получается 7372800/115200=62 такта всего есть на обработку между символами, а значит нельзя прерывать работу получалки уарта больше чем на примерно 50 тактов?

Добавлено спустя 42 секунды:
Зато на 9600 будет аж 768 тактов на работу между символами...

Добавлено спустя 38 секунд:
Прерывания одни в других могут срабатывать?

могут... если ты им разрешишь и хватит стека )