Робот сумо. Часть II.

Посмотрел на календарь — уже, практически, конец сентября! Я ведь хотел попробовать свои силы в соревнованиях роботов, а деле создания собственного сумоиста еще и конь не валялся. поиск жилья, переезд, бытовые проблемы — преследовали меня все лето, и время, которого мне казалось достаточно, пролетело, на  кануне первые соревнования, в которых можно было принять участие. поэтому надо продолжать работу.

В первой части повествования о роботе я описал его конструкцию, в которой смело применил бесколлекторные двигатели. Как оказалось с ними (а точнее с их регуляторами) работать довольно сложно.  До этого я вертел сервой только на arduino, где все таймеры были за меня настроены. Мне только нужно было воспользоваться соответствующей библиотекой. А здесь, на своей отладочной платформе, все намного интереснее. Обнаружилось, что необходим PWM с постоянным периодом примерно 50Гц, что требует иной настройки таймера МК, чем та, которой я ранее пользовался.

Так же, было решено, закончить с изобретением  велосипедов своих алгоритмов расчета контрольных сумм, и наконец использовать CRC8 для верификации данных, при обмене через UART. Получилось не с первого раза. Точнее для МК на Си реализации есть даже в википедии, а вот для java, пришлось поискать.

Ниже представлен код для МК Atmega. Расшифровывать подробно пока нет возможности (точнее времени), однако я думаю все будет понятно.

#include <avr/io.h>//библиотека ввода/вывода
#include <stdio.h>//Библиотека функций
#include <util/delay.h>//Библиотека функций задержки
#include <avr/interrupt.h> //Библиотека прерываний
 
/*----Порты подключения датчиков C----*/
#define OpticR  0  // датчик передний правый
#define OpticL  1  // датчик передний левый
/*----Порты подключения двигателей A----*/
#define MootrRR  0  // двигатель правый канал правый 
#define MootrRL  1  // двигатель правый канал левый
#define MootrLR  2  // двигатель левый канал правый 
#define MootrLL  3  // двигатель леыый канал левый
#define BrMtrRPin  4  // вывод генератора правого БК двигателя
#define BrMtrLPin  5  // вывод генератора левого БК двигателя
/*----Порт генератора для TSOP D----*/
#define GenPinR  3  // вывод генератора правого TSOP
#define GenPinL  7  // вывод генератора левого TSOP
#define LedPin  6  // вывод индикаторного светодиода
/*----скважность ШИМ на выходах БК двигателей----*/
#define MtrRghtPWM OCR1B     // вывод порта D - PD4
#define MtrLftPWM  OCR1A     // вывод порта D - PD5
/* буфер-----------------------------------------------------------*/
#define RxBfrSz 6          //размер буфера
unsigned char RxBfr[RxBfrSz]; //кольцевой (циклический) буфер
unsigned char RxBfrTail = 0;  //"указатель" хвоста буфера
unsigned char RxBfrHead = 0;  //"указатель" головы буфера
unsigned char RxBfrCntr = 0;  //счетчик символов
unsigned char in_ok;
 
unsigned char MinPWM = 0;     //минимальная скважность ШИМ при которой двигатели останавливаются.
unsigned char CurPWM_R = 0; // текущие значение скважностей для левого и правго БК двигателя
unsigned char CurPWM_L = 0; 
 
/*---------------Инициализация портов МК--------------------*/
void PortInit(void) 
{
    DDRC = (0<<OpticR)|(0<<OpticL);  //Инициализация портов оптических датчиков (порт С) как входов 
    PORTC = (1<<OpticR)|(1<<OpticL); // вход с подтяжкой к +
    DDRB = 1<<GenPinR;
    DDRD = (1<<BrMtrRPin)|(1<<BrMtrLPin)|(1<<LedPin)|(1<<GenPinL); // порты PD6 и PD7 как выводs
    DDRA = (1<<MootrRR)|(1<<MootrRL)|(1<<MootrLR)|(1<<MootrLL); // порты РА0,..,3 как выходы 
    /*-----Настройка таймера Т0--------
    режим СТС, предделитель 1, сигнал на PD7 меняется на противоположный--*/
    TCCR0 = 1<<FOC0|1<<WGM01|1<<COM00|1<<CS00;
    OCR0 = 222; // формирует частоту 36кГц (формула расчета в даташите)
    /*-----Настройка таймера Т2--------
    режим СТС, предделитель 1, сигнал на PD7 меняется на противоположный--*/
    TCCR2 = 1<<FOC2|1<<WGM21|1<<COM20|1<<CS20;
    OCR2 = 222; // формирует частоту 36кГц (формула расчета в даташите)
    /*----инициализация PWM---*/
    /*Настраиваем второй 16-и битный таймер*/ 
    TCCR1A = 1<<COM1A1|1<<COM1B1|1<<WGM11; //PWM non inventor
    TCCR1B = 1<<WGM12|1<<WGM13|1<<CS10|1<<CS11;// предделитель 64 Fast PWM
    ICR1=4999; // 50 Гц
    // начальные значения скважности
    MtrRghtPWM = 10; 
    MtrLftPWM = 10;
 
}
 
/*----------------Инициализация USART------------------*/
void USART_Init( unsigned long baud, unsigned long SsFrqnc )
{
    #define XTAL SsFrqnc
    #define baudrate baud
    #define bauddivider (XTAL/(16*baudrate)-1)
    #define HI(x) ((x)>>8)
    #define LO(x) ((x)& 0xFF)
 
    UBRRL = LO(bauddivider);
    UBRRH = HI(bauddivider);
    UCSRA = 0;
    UCSRB = 1<<RXEN|1<<TXEN|1<<RXCIE|0<<TXCIE;
    UCSRC = 1<<URSEL|1<<UCSZ0|1<<UCSZ1;
 
    sei();                   //Разрешение глобальных прерываний
 
}
 
/*---------------Процедура записи в USART-------------------*/
void put(unsigned char data) { // Put a char
    while (!(UCSRA & (1<<UDRE)));
    UDR = data;
}
 
/*---------------Процедура очистки буфера-------------------*/
void FlushBuf(void)
{
    int i;
    RxBfrTail = 0;
   // RxBfrHead = 0;
    RxBfrCntr = 0;
    for (i=0; i<RxBfrSz; i++) {
        RxBfr[i]=0;
    }
    in_ok = 0;
}
 
/*---------------Процедура пополнения буфера символом-------*/
void PutCharBuf(unsigned char sym)
{
  if (sym == 255) {
    in_ok = 1;
  }
  if (in_ok) {
    if (RxBfrCntr < RxBfrSz){   //если в буфере еще есть место
      RxBfr[RxBfrTail] = sym;    //помещаем в него символ
      RxBfrCntr++;                    //инкрементируем счетчик символов
      RxBfrTail++;                           //и индекс хвоста буфера
    }
  }
}
 
/*--------------Процедура изъятия символа из буфера----------*/
unsigned char GetCharBuf(void)
{
   unsigned char sym = 0;
   if (RxBfrCntr > 0){                     //если буфер не пустой
      sym = RxBfr[RxBfrHead];              //считываем символ из буфера
 
      cli();
      RxBfrCntr--;                         //уменьшаем счетчик символов
      sei();
      RxBfrHead++;                         //инкрементируем индекс головы буфера
      if (RxBfrHead == RxBfrSz) RxBfrHead = 0;
   }
   return sym;
}
 
/*--------------Функция проверки контрольной суммы------------*/
/*
  Name  : CRC-8
  Poly  : 0x31    x^8 + x^5 + x^4 + 1
  Init  : 0xFF
  Revert: false
  XorOut: 0x00
  Check : 0xF7 ("123456789")
  MaxLen: 15 байт(127 бит) - обнаружение
    одинарных, двойных, тройных и всех нечетных ошибок
*/
unsigned char Crc8(unsigned char *pcBlock, unsigned int len)
{
    unsigned char crc = 0xFF;
    unsigned int i;
    int k;
    for (k = 0; k < len; k++) 
    //while (len--)
    {
        crc ^= *pcBlock++;
        for (i = 0; i < 8; i++)
            crc = crc & 0x80 ? (crc << 1) ^ 0x31 : crc << 1;
    //}
    }
    return crc;
}
 
/*--------------Процедура ручного управления через USART------*/
void _use_control (void)
{
    unsigned char i, Val_PWM;
    if (RxBfrCntr == 6) {
        //Проверка контрольной суммы
        if (Crc8(RxBfr, 5) == RxBfr[5]) {
 
            MtrRghtPWM = 10+RxBfr[1]*1.95;
            MtrLftPWM = 10+RxBfr[2]*1.95;
 
            PORTD = PORTD ^(1<<LedPin);
 
            //put(Crc8(RxBfr, 5));
 
            FlushBuf();
        } else {
        //  put(Crc8(RxBfr, 5));
            put(RxBfr[5]);
            FlushBuf();
        };
    };
 
}
 
/*--Прерывания--------------------------------------------------*/
 
// Прерывание по приходу байта в USART
ISR(USART_RXC_vect)
{
    unsigned char data;
    data = UDR;
    // помещаем принятый символ в буфер
    PutCharBuf(data);
}   
 
/*--Основная программа-----------------------------------------*/
int main(void)
{ 
    //Инициализация портов и таймеров
    PortInit();
    //Иницализация USART
    USART_Init( 19200,16000000);
// бесконечный цикл
  while (1)
    {   
        _use_control();
    }
    return 1;
 }
#include <avr/io.h>//библиотека ввода/вывода
#include <stdio.h>//Библиотека функций
#include <util/delay.h>//Библиотека функций задержки
#include <avr/interrupt.h> //Библиотека прерываний

/*----Порты подключения датчиков C----*/
#define OpticR  0  // датчик передний правый
#define OpticL  1  // датчик передний левый
/*----Порты подключения двигателей A----*/
#define MootrRR  0  // двигатель правый канал правый 
#define MootrRL  1  // двигатель правый канал левый
#define MootrLR  2  // двигатель левый канал правый 
#define MootrLL  3  // двигатель леыый канал левый
#define BrMtrRPin  4  // вывод генератора правого БК двигателя
#define BrMtrLPin  5  // вывод генератора левого БК двигателя
/*----Порт генератора для TSOP D----*/
#define GenPinR  3  // вывод генератора правого TSOP
#define GenPinL  7  // вывод генератора левого TSOP
#define LedPin  6  // вывод индикаторного светодиода
/*----скважность ШИМ на выходах БК двигателей----*/
#define MtrRghtPWM OCR1B     // вывод порта D - PD4
#define MtrLftPWM  OCR1A     // вывод порта D - PD5
/* буфер-----------------------------------------------------------*/
#define RxBfrSz 6          //размер буфера
unsigned char RxBfr[RxBfrSz]; //кольцевой (циклический) буфер
unsigned char RxBfrTail = 0;  //"указатель" хвоста буфера
unsigned char RxBfrHead = 0;  //"указатель" головы буфера
unsigned char RxBfrCntr = 0;  //счетчик символов
unsigned char in_ok;

unsigned char MinPWM = 0;	  //минимальная скважность ШИМ при которой двигатели останавливаются.
unsigned char CurPWM_R = 0; // текущие значение скважностей для левого и правго БК двигателя
unsigned char CurPWM_L = 0;	

/*---------------Инициализация портов МК--------------------*/
void PortInit(void) 
{
	DDRC = (0<<OpticR)|(0<<OpticL);  //Инициализация портов оптических датчиков (порт С) как входов 
 	PORTC = (1<<OpticR)|(1<<OpticL); // вход с подтяжкой к +
	DDRB = 1<<GenPinR;
 	DDRD = (1<<BrMtrRPin)|(1<<BrMtrLPin)|(1<<LedPin)|(1<<GenPinL); // порты PD6 и PD7 как выводs
 	DDRA = (1<<MootrRR)|(1<<MootrRL)|(1<<MootrLR)|(1<<MootrLL); // порты РА0,..,3 как выходы 
 	/*-----Настройка таймера Т0--------
	режим СТС, предделитель 1, сигнал на PD7 меняется на противоположный--*/
 	TCCR0 = 1<<FOC0|1<<WGM01|1<<COM00|1<<CS00;
 	OCR0 = 222; // формирует частоту 36кГц (формула расчета в даташите)
	/*-----Настройка таймера Т2--------
	режим СТС, предделитель 1, сигнал на PD7 меняется на противоположный--*/
 	TCCR2 = 1<<FOC2|1<<WGM21|1<<COM20|1<<CS20;
 	OCR2 = 222; // формирует частоту 36кГц (формула расчета в даташите)
	/*----инициализация PWM---*/
	/*Настраиваем второй 16-и битный таймер*/ 
  	TCCR1A = 1<<COM1A1|1<<COM1B1|1<<WGM11; //PWM non inventor
  	TCCR1B = 1<<WGM12|1<<WGM13|1<<CS10|1<<CS11;// предделитель 64 Fast PWM
	ICR1=4999; // 50 Гц
	// начальные значения скважности
	MtrRghtPWM = 10; 
	MtrLftPWM = 10;

}

/*----------------Инициализация USART------------------*/
void USART_Init( unsigned long baud, unsigned long SsFrqnc )
{
    #define XTAL SsFrqnc
    #define baudrate baud
    #define bauddivider (XTAL/(16*baudrate)-1)
    #define HI(x) ((x)>>8)
    #define LO(x) ((x)& 0xFF)

    UBRRL = LO(bauddivider);
    UBRRH = HI(bauddivider);
    UCSRA = 0;
    UCSRB = 1<<RXEN|1<<TXEN|1<<RXCIE|0<<TXCIE;
    UCSRC = 1<<URSEL|1<<UCSZ0|1<<UCSZ1;

    sei();                   //Разрешение глобальных прерываний

}

/*---------------Процедура записи в USART-------------------*/
void put(unsigned char data) { // Put a char
    while (!(UCSRA & (1<<UDRE)));
    UDR = data;
}

/*---------------Процедура очистки буфера-------------------*/
void FlushBuf(void)
{
	int i;
    RxBfrTail = 0;
   // RxBfrHead = 0;
    RxBfrCntr = 0;
	for (i=0; i<RxBfrSz; i++) {
		RxBfr[i]=0;
	}
	in_ok = 0;
}

/*---------------Процедура пополнения буфера символом-------*/
void PutCharBuf(unsigned char sym)
{
  if (sym == 255) {
  	in_ok = 1;
  }
  if (in_ok) {
  	if (RxBfrCntr < RxBfrSz){   //если в буфере еще есть место
      RxBfr[RxBfrTail] = sym;    //помещаем в него символ
      RxBfrCntr++;                    //инкрементируем счетчик символов
      RxBfrTail++;                           //и индекс хвоста буфера
    }
  }
}

/*--------------Процедура изъятия символа из буфера----------*/
unsigned char GetCharBuf(void)
{
   unsigned char sym = 0;
   if (RxBfrCntr > 0){                     //если буфер не пустой
      sym = RxBfr[RxBfrHead];              //считываем символ из буфера

      cli();
      RxBfrCntr--;                         //уменьшаем счетчик символов
      sei();
      RxBfrHead++;                         //инкрементируем индекс головы буфера
      if (RxBfrHead == RxBfrSz) RxBfrHead = 0;
   }
   return sym;
}

/*--------------Функция проверки контрольной суммы------------*/
/*
  Name  : CRC-8
  Poly  : 0x31    x^8 + x^5 + x^4 + 1
  Init  : 0xFF
  Revert: false
  XorOut: 0x00
  Check : 0xF7 ("123456789")
  MaxLen: 15 байт(127 бит) - обнаружение
    одинарных, двойных, тройных и всех нечетных ошибок
*/
unsigned char Crc8(unsigned char *pcBlock, unsigned int len)
{
    unsigned char crc = 0xFF;
    unsigned int i;
	int k;
 	for (k = 0; k < len; k++) 
    //while (len--)
    {
        crc ^= *pcBlock++;
        for (i = 0; i < 8; i++)
            crc = crc & 0x80 ? (crc << 1) ^ 0x31 : crc << 1;
    //}
	}
    return crc;
}

/*--------------Процедура ручного управления через USART------*/
void _use_control (void)
{
    unsigned char i, Val_PWM;
    if (RxBfrCntr == 6) {
        //Проверка контрольной суммы
    	if (Crc8(RxBfr, 5) == RxBfr[5]) {

			MtrRghtPWM = 10+RxBfr[1]*1.95;
			MtrLftPWM = 10+RxBfr[2]*1.95;

			PORTD = PORTD ^(1<<LedPin);

			//put(Crc8(RxBfr, 5));

	    	FlushBuf();
		} else {
		//	put(Crc8(RxBfr, 5));
			put(RxBfr[5]);
	    	FlushBuf();
		};
    };

}

/*--Прерывания--------------------------------------------------*/

// Прерывание по приходу байта в USART
ISR(USART_RXC_vect)
{
    unsigned char data;
	data = UDR;
    // помещаем принятый символ в буфер
    PutCharBuf(data);
}   

/*--Основная программа-----------------------------------------*/
int main(void)
{ 
 	//Инициализация портов и таймеров
	PortInit();
	//Иницализация USART
	USART_Init( 19200,16000000);
// бесконечный цикл
  while (1)
 	{ 	
	    _use_control();
  	}
  	return 1;
 }

Структура пакета команд немного изменилась. Теперь в начале каждого пакета идет байт равный 255, сигнализируя МК о том, что нужно начать запись пакета в буфер. По достижению заданного количества байт в массиве буфера, происходит их обработка и вычисление CRC. Буфер после этого опустошается и МК ждет следующего стартового байта для начала приема.

Для расчета контрольной суммы на java подойдет такой код:

public static byte crc8(byte[] buffer) {
            byte crc = (byte) 0xFF;
 
            for (byte b : buffer) {
                crc ^= b;
 
                for (int i = 0; i < 8; i++) {
                    crc = (crc & 0x80) != 0 ? (byte) ((crc << 1) ^ 0x31) : (byte) (crc << 1);
                }
            }
 
            return crc;
        }
public static byte crc8(byte[] buffer) {
			byte crc = (byte) 0xFF;

			for (byte b : buffer) {
				crc ^= b;

				for (int i = 0; i < 8; i++) {
					crc = (crc & 0x80) != 0 ? (byte) ((crc << 1) ^ 0x31) : (byte) (crc << 1);
				}
			}

			return crc;
		}

Программа, написанная для Android’a аналогична проге для управления вертолетом. Соединения с роботом осуществляется с помощью bluetooth, передаются пока две величины — скорость вращения левого и правого двигателей. (для поиграться:)). Видео планирую выложить позже. Прогу для смартфона тоже могу выложить с исходниками, если нужно кому.

Запись опубликована в рубрике AVR, Разработки. Добавьте в закладки постоянную ссылку.

         
Подписаться на новые статьи блога:

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *

Лимит времени истёк. Пожалуйста, перезагрузите CAPTCHA.