Универсальный модуль управления серво-приводами

Доброго времени суток!
Данный пост является своего рода возмещением заимствованного кода различных авторов, а также труда всех помогавших в создании данного устройства.
Разговор пойдет об универсальном блоке управления серво на 16 каналов, в первой роли по BlueTooth, что не состоялось, и во второй роли — напрямую от компьютера.


Как сказано выше, данный модуль разрабатывался для управления сервоприводами для четырёхногого шагающего робота в рамках одной курсовой.
Робот — если кому интересно.


Двигается при помощи сервомашинок Turnigy TG9, но интересно было не это, а то, чем эти 9 серво управляются! А для этого используется целый системник под MS-DOS, с программой на Turbo Pascal-е, которая дергает LPT портом… Ну претензии не ко мне, а к создателю комплекса. (Кстати говоря, тов.создатель-комплекса, если Вы это читаете, я очень рад =D читайте на здоровье, а Ваша фирма нам не вперлась, всего-то хотели себе оборудование сделать)
Так вот, камнем преткновения была программная часть.

Таким образом было сформировано Т.З. на создание блока управления для создания автономного шагающего робота.
Образцом подражания послужил контроллер SYSCONE 20-Channel Serial Servo Controller.
Но естественно ни желания, ни возможности данную штуку (с закрытым кодом) приобретать не обнаружилось, а для создания собственной базы было решено… создать эту базу.

Основой всему послужил код DI HALT из статьи Управление множеством сервомашинок, за что ему Огромное человеческое Спасибо!
Как было упомянуто в статье — количество сервомашинок можно расширить, чем я и занялся.
В качестве базового контроллера была выбрана ATmega8.
Количество сервомашинок было увеличено до 16, этого было достаточно с запасом для данной задачи.

Отдельной темой разговора является теория построения контроллера управления шагающим роботом. В данном случае я использовал ноги контроллера на прямую, так оказалось проще и быстрее. Однако, в процессе изучения различных конструкций я пришел к выводу, что 16 каналов для робота может быть маловато, особенно если в конце мы хотим получить достаточно большое количество степеней свободы. Таким образом желательно начинать с 24 и выше каналов, но тут вопрос железа — многоногие контроллеры становятся нерентабельным решением и надо использовать сдвиговые регистры. Дальше необходимо определиться с точностью позиционирования серв, с скоростью их работы учитывая их количество. В этом плане я наверное отведу время для разработки такого модуля на 32 канала. Но в другой раз.

В данном случае я пошел по пути наименьшего сопротивления.
Полный код программы я приведу в конце статьи. (жаль рейтинг не позволяет вставить его ;))
Хочу обратить внимание на проблему вывода массива битов, состоящего из кусков портов, в эти-же порты. В данном случае используется контсрукция union, за что выражается Спасибо тов. ZoneRR-у — Спасибо, помогло!
Второй проблемой был для меня алгоритм принятия данных по UART-у. В данном месте я использовал машину состояний.
С МК общаемся мы так:
[B]{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}[E]

[B]{S0}{S1}{S2}{S3}{S4}{S5}{S6}{S7}{S8}{S9}{S10}{S11}{S12}{S13}{S14}{S15}[E]

B      - начало посылки  0x42
E      - конец посылки   0x45
S0..S15 - значение сервопривода  0..255

Меня это устроило =)

Интересным моментом оказалась ответная программная часть на стороне ПК — её у меня не было.
Тов. dcoder обещался нашаманить простенький декодер, который будет читать такие строчки их тхт файла и плеваться ими в СОМ порт.
Ну а тем временем я приспособился под «сиськоновскую» программулину, там алгоритм другой, но я его тоже учёл, только там скорость обмена 9600 бод, хватит для тестирования.
Но в конечном итоге до меня дошло — что таблицы хотьбы надо размещать в самом МК или в микросхеме памяти неподалёку на коротком I2C поводке.

Еще одной проблемой стал сам UART — он не работал при внутреннем тактировании — пришлось поставить (не подходящий для UART-а) квартц на 14.318МГц. Но ничего, работает. И на 9600, и на 19200, и на 115200. Живём!

Также МК был снабжен бутлоадером ATMEL AVR UART Bootloader for AVR-GCC/avr-libc, очень удобно загружать в коробку новые прошивки и применять в других местах.

А вот для связи и автономизации было решено взять модуль LMX9838(дорогой сука), о нём я уже писал. (отдельная история как я оторвал пару контактных площадок, — не паяйте его навесом!!! Только сразу на плату!)

Но как известно тов.создателю-комплекса от автономизации пришлось отказаться. Как и от синезуба, и от загона хотьбы в МК.
Всё было запихано в коробку от 2.5 дюймового внешнего HDD и обильно смазано термосоплями.
В качестве преобразователя USB — UART выступает вырезанная кое-откуда плата на PL-2303.

Ну вот и кончилась лирика.
Схема.


Железо. В первом явлении.


Железо. Во втором явлении. Готовая коробка в шапке топика.


И код программы.
#include <avr/io.h>                                 
#include <util/delay.h>
#include <avr/interrupt.h>//библиотека прерываний
// datatype definitions macros
typedef unsigned char  u08;
typedef   signed char  s08;
typedef unsigned short u16;
typedef   signed short s16;
typedef unsigned long  u32;
typedef   signed long  s32;
typedef unsigned long long u64;
typedef   signed long long s64;

#define SWAP(A, B) { int t = A; A = B; B = t; }

// Прототипы задач ============================================================
void Servo_add(u08 Number, u08 Pos);
void Servo_sort(void);
void Servo_upd(void);
void Servo_Init(void);

	union shit{ 
	long someshit; 
	char anyshit[4]; 
	};
	union shit happens; 

//============================================================================
//MultiServo
#define MaxServo 16 // Число сервомашинок
u08 servo_need_update = 0; 
u08 servo_state=0;	// Состояние конечного автомата. 
u32 ServoPortState[MaxServo+1];	// Значение порта которое надо вывести
u08 ServoNextOCR[MaxServo+1];	// Время вывода значения

typedef struct 	{
		u08 Position;
		u32 Bit;
		}  SArray_def;
 
SArray_def Servo[MaxServo];
SArray_def *Servo_sorted[MaxServo];
//============================================================================
ISR(TIMER2_COMP_vect)				// Прерывание по совпадению
{
if (servo_state)				// Если не нулевое состояние то
	{
	OCR2 = ServoNextOCR[servo_state];	// В регистр сравнения кладем следующий интервал

	happens.someshit=ServoPortState[servo_state];
//	PORTB &= ~ServoPortState[servo_state];	// Сбрасываем биты в порту, в соответствии с маской в массиве масок.
    PORTB &= ~happens.anyshit[0];
    PORTC &= ~happens.anyshit[1];
    PORTD &= ~happens.anyshit[2];		


	servo_state++;				// Увеличиваем состояние автомата
 
	if (OCR2 == 0xFF)				// Если значение сравнения равно FF значит это заглушка
		{				// И мы достигли конца таблицы. И пора обнулить автомат
		servo_state = 0;			// Выставляем нулевое состояние.
 
		TCNT2 = 105;			// Программируем задержку 
		TCCR2 &= 0b11111000;		// Сбрасываем предделитель таймера
		TCCR2 |= 0x05;			// Устанавливаем предделитель 
 
		if (servo_need_update)		// Если поступил приказ обновить таблицы автомата
			{
			Servo_upd();		// Обновляем таблицы.
			servo_need_update = 0;	// Сбрасываем сигнал обновления.
			}
		}
	}
else						// Нулевое состояние автомата. Новый цикл
	{
	OCR2 = ServoNextOCR[servo_state];		// Берем первую выдержку.
 
	TCCR2 &= 0b11111000;			// Сбрасываем предделитель
	TCCR2 |= 0x04;				// Предделитель 
	PORTB = 0xFF;
	PORTC = 0xFF;				// Выставялем все сервоканалы в 1 - начало импульса
 	PORTD = 0xFF;
	servo_state++;				// Увеличиваем состояние конечного автомата. 
	}
}
//============================================================================
void Servo_add(u08 Number, u08 Pos)
{
u08 i,k;
SArray_def *tmp;
Servo[Number].Position = Pos;
// Сортируем массив
for(i=1;i<MaxServo;i++)
	{
	for(k=i;((k>0)&&(Servo_sorted[k]->Position < Servo_sorted[k-1]->Position));k--)
		{
		tmp = Servo_sorted[k];					// Swap [k,k-1] 
		Servo_sorted[k]=Servo_sorted[k-1];
		Servo_sorted[k-1]=tmp;
		}
	}
}
//============================================================================
void Servo_sort(void) 
{
u08 i,k;
SArray_def *tmp;

for(i=1;i<MaxServo;i++)
	{
	for(k=i;((k>0)&&(Servo_sorted[k]->Position < Servo_sorted[k-1]->Position));k--)
		{	
		tmp = Servo_sorted[k];					// Swap [k,k-1] 
		Servo_sorted[k]=Servo_sorted[k-1];
		Servo_sorted[k-1]=tmp;
		}

	}
}
//============================================================================
void Servo_upd(void)
{
u08 i,j,k;
 
for(i=0,k=0;i<MaxServo;i++,k++)
{
	if(Servo_sorted[i]->Position!=Servo_sorted[i+1]->Position)	//Если значения уникальные
	{
	ServoNextOCR[k] = Servo_sorted[i]->Position;			// Записываем их как есть
	ServoPortState[k+1] = Servo_sorted[i]->Bit;			// И битмаску туда же
	}
	else								// Но если совпадает со следующим
	{
	ServoNextOCR[k] = Servo_sorted[i]->Position;			// Позицию записываем
	ServoPortState[k+1] = Servo_sorted[i]->Bit;			// Записываем битмаску
 
	// И в цикле ищем все аналогичные позиции, склеивая их битмаски в одну.
 
	for(j=1;(Servo_sorted[i]->Position == Servo_sorted[i+j]->Position)&&(i+j<MaxServo);j++)
		{
		ServoPortState[k+1] |= Servo_sorted[i+j]->Bit;
		}
	i+=j-1;						// Перед выходом корректируем индекс
	}						// На глубину зарывания в повторы
}	
ServoNextOCR[k] = 0xFF;					// В последний элемент вписываем заглушку FF.
}
//============================================================================
void Servo_Init(void)
{
Servo_sorted[0]   = &Servo[0];
Servo_sorted[1]   = &Servo[1];
Servo_sorted[2]   = &Servo[2];
Servo_sorted[3]   = &Servo[3];
Servo_sorted[4]   = &Servo[4];
Servo_sorted[5]   = &Servo[5];
Servo_sorted[6]   = &Servo[6];
Servo_sorted[7]   = &Servo[7]; 
Servo_sorted[8]   = &Servo[8];
Servo_sorted[9]   = &Servo[9];
Servo_sorted[10]  = &Servo[10];
Servo_sorted[11]  = &Servo[11];
Servo_sorted[12]  = &Servo[12];
Servo_sorted[13]  = &Servo[13];
Servo_sorted[14]  = &Servo[14];
Servo_sorted[15]  = &Servo[15];  
//PORT B
Servo[0].Bit   = 0b000000000000000000000001;
Servo[1].Bit   = 0b000000000000000000000010;
Servo[2].Bit   = 0b000000000000000000000100;
Servo[3].Bit   = 0b000000000000000000001000;
Servo[4].Bit   = 0b000000000000000000010000;
Servo[5].Bit   = 0b000000000000000000100000;
//PORT C
Servo[6].Bit   = 0b000000000000000100000000;
Servo[7].Bit   = 0b000000000000001000000000;  
Servo[8].Bit   = 0b000000000000010000000000;
Servo[9].Bit   = 0b000000000000100000000000;
//PORT D
Servo[10].Bit  = 0b000001000000000000000000;
Servo[11].Bit  = 0b000010000000000000000000;
Servo[12].Bit  = 0b000100000000000000000000;
Servo[13].Bit  = 0b001000000000000000000000;
Servo[14].Bit  = 0b010000000000000000000000;
Servo[15].Bit  = 0b100000000000000000000000;  

Servo[0].Position   = 127;
Servo[1].Position   = 127;
Servo[2].Position   = 127;
Servo[3].Position   = 127;
Servo[4].Position   = 127;
Servo[5].Position   = 127;
Servo[6].Position   = 127;
Servo[7].Position   = 127;  
Servo[8].Position   = 127;
Servo[9].Position   = 127;
Servo[10].Position  = 127;
Servo[11].Position  = 127;
Servo[12].Position  = 127;
Servo[13].Position  = 127;
Servo[14].Position  = 127;
Servo[15].Position  = 127; 

Servo_sort();
Servo_upd();

TIMSK |= 1<<OCIE2;	
TCCR2 |= 0x05;
}
//============================================================================
void usart_tx (unsigned char data) {
    while ( !(UCSRA & (1<<5)) );    //ждём очистки регистра данных USART       
    UDR=data;    //отправить
} 
 unsigned char usart_rx (void) { 
    while ( !(UCSRA & (1<<7)) );    //ждём очистки регистра данных USART      
    return UDR;    //читаем данные
} 
//============================================================================
void Accept (void) {  //Принятие посылки
static unsigned char curr_serv,usart_data,state;
u08 Num,Pos;


	if(UCSRA&_BV(RXC)) {  //есть данные в usart
                         usart_data = UDR;

    switch(state) { // автомат с 2 состояниями
        case 0: // принимаем коды команд
            switch(usart_data)
            {
                case 'E': Servo_sort(); Servo_upd(); break; // конец посылки, обновление серв
                case 'B': state = 1; break; // переход в другое состояние, будем принимать параментры команды B
				case 0xFF:  
				Num = usart_rx ();  
				Pos = usart_rx (); 
				Servo[Num].Position = Pos; 
				state = 0; 	
				Servo_sort(); 
				Servo_upd();  	
				break;
            }
            break;
        case 1: // обработка параметров команды B
            Servo[curr_serv].Position = usart_data; // кладем в массив аргумент команды
            curr_serv++;
			if(curr_serv == 16) // повторять будем, сколько у нас серв
            {
                curr_serv = 0; // обнуление счетчика, для следующего раза
                state = 0; // будем ожидать команды
            }
            break;
    				}
}}
//============================================================================
int main (void) {

 DDRB=0b00111111;
PORTB=0b00000000;
 DDRC=0b00001111;
PORTC=0b00000000;
 DDRD=0b11111100;
PORTD=0b00000000; 



 /*/ USART0 settings: 115200 baud 8-n-1
// WARNING: real baud = 119316: err = 3,57291666666666%
	UBRRH = 0;
	UBRRL = 14;
	UCSRA = (1<<U2X);
	UCSRB = (1<<RXEN) | (1<<TXEN);
	UCSRC = (1<<URSEL) | (1<<UCSZ1) | (1<<UCSZ0); */

// USART0 settings: 9600 baud 8-n-1
// WARNING: real baud = 9622: err = 0,229166666666658%
	UBRRH = 0;
	UBRRL = 92;
	UCSRB = (1<<RXEN) | (1<<TXEN);
	UCSRC = (1<<URSEL) | (1<<UCSZ1) | (1<<UCSZ0); 


Servo_Init();

asm("sei");			
while (1) {
		Accept();
		  }
return 0;
				}
//============================================================================

Всем спасибо за внимание!

ЗЫ Кому не нравится что-то, делайте свой блек-джек.
  • 0
  • 25 июня 2011, 21:56
  • qic

Комментарии (7)

RSS свернуть / развернуть
Еще одной проблемой стал сам UART — он не работал при внутреннем тактировании
… а на каких баудах и внутренней частоте?
0
Внутренняя частота 8МГц, по максимуму, бауды 19200 и 115200 (бутлоадер перестал отвечать, ну и сама программа). Ни на что не претендую — полагаю что такая мега попалась.
0
А ты калибровочные константы в OSCCAL загонял? У меня на пинборде UART нормально работает, на 38400 пробовал. Тактирование — внутренний генератор на 8 МГц.
0
Не загонял конечно-же. Я касаемо калибровки даже не углублялся. Просто ознакомлен что есть и всё. В любом случае — если есть кварц, почему бы его и не поставить. Стабильнее и надежнее.
0
В таком случае, если откалибровать и не трогать I2C, то да, можно получит 20 каналов с несчастной ATmega8, а если оставить TX и Reset, то 22.
Но пошло оно — я лучше сдвиговых регистров поставлю 3 штуки.
0
а где можно узнать подробности о реализации приема-передачи данных со стороны ПК?


u08 bC=0,Pos161,Pos162;
void UART_wait (void) {  
static unsigned char curr_serv,uart_data,state;

    if(UCSRA&_BV(RXC)) {
                       uart_data = UDR;
    switch(state) { 
        case 0: 
            switch(uart_data)
            {
                case 'E': Servo_sort(); Servo_upd(); break; 
                case 'B': LCD_Clear(); state = 1; break; 
            }
            break;
        case 1: 
			Pos161 = uart_data;
			state = 2;
			break;
		case 2: 
			
			Pos162 = uart_data;
            Servo[curr_serv].Position = (Pos161 << 8) | Pos162;
            curr_serv++;
			state = 1;
            if(curr_serv == 3) 
            {
                curr_serv = 0; 
                state = 0;
            }
            break;
        }
		uart_tx(uart_data);
}
SetTimerTask(UART_wait,10);
}


Когда я отправляю из терминала по одному байту, МК все нормально отрабатывает. А когда я сразу подряд несколько байт в цикле отправляю. Некоторые из отправленных байт МК пропускает.
Быть может существует какой-то типичный алгоритм передачи байт с ПК на МК?
0
Вроде разобрался. Используя Delphi7 и компонент BComPort, процедура последовательной передачи байт у меня выглядит так:

Здесь три сервы управляются одновременно, позиция каждой сервы — 16 бит. Поэтому заодно и делим двубайтные числа на старшие и младшие байты, отправляем по одному байту, а МК потом их обратно сшивает.

procedure TForm1.SetServo(s1,s2,s3:integer);
var
  Tx: array [1..8] of byte;
  i: integer;
  Rx: string;
Begin
  Tx[1]:=$42;

  Tx[2]:=Hi(s1);
  Tx[3]:=Lo(s1);
  Tx[4]:=Hi(s2);
  Tx[5]:=Lo(s2);
  Tx[6]:=Hi(s3);
  Tx[7]:=Lo(s3);

  Tx[8]:=$45;
  if comPort.Connected then
  begin
    for i:=1 to 8 do
    begin
      comPort.Write(Tx[i],Sizeof(Tx[i]));
      comPort.Timeouts.ReadInterval:=1;
      comPort.ReadStr(Rx, 250);
      Memo1.Lines.Add(Rx);
    end;
  end;
end;
0
Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.