roboforum.ru

#include <avr/io.h> //Библиотека ввода/вывода
#include <avr/interrupt.h> //Библиотека прерываний
#include <util/delay.h>

#define BAUDRATE 9600 // Скорость обмена данными
#define F_CPU 16000000UL // Рабочая частота контроллера

//Прототипы подпрограмм
void  USART_Init( void );
void  USART_Byte_Transmit( unsigned char data );
void  USART_Transmit( char* string );
void Forklift_Go( int place );
void Rotate(uint8_t arr, uint8_t durr);
void RotateLeft();
void RotateRight();
void GoStraight();
void Forklift_Get( int item );
void Forklift_Release();
void Forklift_Stop();
void Timer_Init();
uint16_t GetDistance();
uint16_t GetTime();

//Подпрограмма-обработчик прерывания по приходу байта на модуль  USART
ISR( USART0_RX_vect )
{
int data;
data = UDR0; //Присваиваем переменной b содержимое регистра UDR, в котором хранится принятый байт

sei(); // Разрешаем вложенные прерывания для возможности отправки сигнала остановки погрузчика..

// Выполняем обработку принятого байта
if (data=='1')
{
int number;
int finalplace;
number=data;
USART_Transmit("Ready to go\r\n"); //Сообщение о готовности к выполнению задания
Forklift_Go(number); //Передвижение к месту расположения конкретного груза
Forklift_Get(number); //Взятие груза
USART_Transmit("Got it!\r\n"); //Сообщение об успешной идентификации груза
Forklift_Go( finalplace ); //Передвижение к месту выдачи груза
Forklift_Release(); //Отпускаем груз
USART_Transmit("Done!\r\n"); //Сообщение об успешной доставке груза
}
else
if (data=='2')
{
Forklift_Stop();
USART_Transmit("Stopped\r\n"); //Сообщение об остановке
}
}

void  USART_Init( void ) //Инициализация модуля  USART
{
/* Задаем скорость работы  USART  */   
UBRR0H = (F_CPU/BAUDRATE/16-1)>>8;
UBRR0L = (F_CPU/BAUDRATE/16-1);

UCSR0B=(1<<RXEN)|( 1<<TXEN); // Разрешаем прием и передачу по  USART
UCSR0B |= (1<<RXCIE);  // Разрешаем прерывания по завершению приема

UCSR0C = (1<<USBS)|(3<<UCSZ0); //Устанавливаем формат данных 8 бит данных, 2 стоп бита
}

void  USART_Byte_Transmit( unsigned char data ) //Функция отправки бита данных
{
while ( !(UCSR0A & (1<<UDRE)) ); //Ожидание опустошения буфера приема
UDR0 = data; //Начало передачи данных                
}

void USART_Transmit( char* string ) //Функция отправки строки
{
while(*string != '\0')
{
USART_Byte_Transmit(*string);
string++;
}
}


void Forklift_Go( int place )
{

int thisplace; //Переменная отражающая текущее местоположение погрузчика (надо как-то привязать это к картированию)
uint8_t stat=0;

while ( thisplace != place) {     // Пока не доехал до места назначения проводим измерения до препятствий
   uint16_t dist=GetDistance();

   if( dist < 10) {Rotate(stat, 255);} else   
    if( dist < 20) {Rotate(stat, 200);} else
    if( dist < 40) {Rotate(stat, 130);} else
                   {GoStraight(); stat=~stat;}   
}
}

void Rotate(uint8_t arr, uint8_t dur) {

switch (arr)
    {
    case 0:    RotateRight();
      break;
    case 1:    RotateLeft();
      break;   
    }
    _delay_ms(dur);   
    Forklift_Stop();

}

void RotateLeft() {

;//Управление каким-то образом поворотом погрузчика влево

}

void RotateRight() {

;//Управление каким-то образом поворотом погрузчика вправо

}

void GoStraight() {

;//Движение прямо

}

void Forklift_Get( int item ) {

;//Функция захвата груза

}
void Forklift_Release() {

;//Функция опускания груза

}
void Forklift_Stop() {

;//Остановить погрузчик

}

void Timer_Init() {  //Инициализация 16-разр. таймера-счетчика

    TCCR1A = 0b00000000;
    TCCR1B = 0b00000000;
    TCCR1C = 0b00000000;
    TCNT1H = 0b00000000;
    TCNT1L = 0b00000000;
    TIMSK = 0b00000000;
}


uint16_t GetDistance() {

uint16_t distance;
PORTB |= (1<<0); // Подаем напряжение на выход PB0 (к пину Trig)
_delay_us(10); // Ожидание 10 мкс (запускается процедура измерения сигнала)
PORTB &= ~(1<<0); // Сбрасываем PB0
distance = GetTime(); // Измеряем длительность ответного сигнала
return distance;
}

uint16_t GetTime() { //Функция определения времени с помощью таймера

uint16_t distance;
  while( (PINB & (1<<1)) == 0 ); //Ожидание появления сигнала Echo
TCCR1B = 0b00000010; // clk/8   Запуск таймера
while( (PINB & (1<<1)) != 0 ); //Ожидание окончания сигнала
TCCR1B = 0b00000000; // Остановка таймера
distance = TCNT1L; //Запись значения длительности импульса в переменную
distance |= TCNT1H;
return distance/7; //Перевод в сантиметры
}


int main(void) //Главная  программа
{
USART_Init ();
Timer_Init();

DDRB |=(1<<0); // Устанавливаем линию PB0 на выход (к пину дальномера Trig)
DDRB &=~(1<<1);  // Устанавливаем линию PB1 на вход (от пина Echo)

USART_Transmit("Ok!\r\n");//Передаем при включении
sei(); //разрешаем глобально прерывания

while(1) //вечный цикл
{ }
}