Делаем простого робота. Часть третья

Блог им. namoaton

Для нашего робота осталось совсем немного — добавить управление ультразвуковым датчиком.
Обычно все используют готовые библиотеки на Arduino. Это не наш путь.
Мы будем использовать прерывания. О принципах работы HC-SR04 датчика не писал только ленивый. Вот ссылка с подробным описанием ТЫЦ!
Вся суть сводится к одному. Подаем импульс импульс длительностью 10 мкс, датчик излучает 8 импульсов на 40 кГц, затем замеряем длительность «эха», которую выдаёт датчик на соответствующей ноге.
Посмотрим на наш код.

void sonar_init(void)
{
  P1DIR |=TRIGGER_PIN;
  P1OUT &= ~TRIGGER_PIN;
  P1SEL  &=  (~ECHO_PIN);
	P1DIR  &=  (~ECHO_PIN);
  P1OUT |= ECHO_PIN;
  P1REN &= ~ECHO_PIN;
  P1IES  |=  (ECHO_PIN); //  Falling Edge    1   ->  0
	P1IFG  &=  (~ECHO_PIN);    //  Clear   interrupt   flag    for P2.1
	P1IE   |=  (ECHO_PIN); //  Enable  interrupt   for P2.1
	TA0CTL |= TASSEL_2 | ID_3| MC_2;
 
}
unsigned int sonar_distance(void)
{
  
  unsigned long time =0;
  float distance = 0;
  P1OUT &= ~TRIGGER_PIN;
  delay_ms(2);
  P1OUT |=TRIGGER_PIN;
  __delay_cycles(6);
  P1OUT &= ~TRIGGER_PIN;
 TAR = 0;
  do{
    
  }while(echo_flag ==0);
 time = TAR;
    echo_flag = 0;
  distance = ((time*10)/58)-11;
  return distance;
}
  #pragma vector=PORT1_VECTOR
  __interrupt void  Port_1(void)
  {
    	echo_flag = 1;
      P1OUT |= BIT6;
    	P1IFG &= ~ECHO_PIN;    //     IFG clear
    }

В функции sonar_init() ми запускаем таймер который будет измерять промежутки времени. Также мы включаем прерывание на ноге ECHO по спаду сигнала. Это значит, что как только произойдет переход с высокого уровня на ноль, сработает прерывание и выполнение программы перейдет в обработчик прерываний.

Далее рассмотрим функцию sonar_distance(). В ней нужно обратить внимание на такую запись

TAR = 0;
  do{
    
  }while(echo_flag ==0);
 time = TAR;

TAR это значение в таймере.
TAR = 0 — так мы обнуляем значение таймера.
далее ожидаем пока echo_flag не будет равен единичке (а это произойдет лишь после того как сработает прерывание), и сразу записываем значение из таймера в переменную time. Time это именно тот отрезок времени, который нам необходим для формулы.

distance = ((time*10)/58)-11;

Таким образом наша функция вернет на значение расстояния.
Добавим все это в нашу прошивку.

#include <msp430g2553.h>
#define LEFT_MOTOR_ENABLE BIT0
#define LEFT_MOTOR_PIN1   BIT1
#define LEFT_MOTOR_PIN2   BIT2
#define RIGHT_MOTOR_ENABLE BIT3
#define RIGHT_MOTOR_PIN1  BIT4
#define RIGHT_MOTOR_PIN2  BIT5
#define MOTOR_PORT        P2OUT
#define MOTOR_PORT_DIR    P2DIR
#define LED               BIT6
//UART
#define TXLED BIT6
#define RXLED BIT0
#define TXD BIT2
#define RXD BIT1
const char string[] = { "Hello World\r\n" };
//ultrasound sensor
#define TRIGGER_PIN BIT4
#define ECHO_PIN    BIT5
volatile unsigned int echo_flag = 0;

void delay_ms(unsigned int ms){
	while(ms--){
	__delay_cycles(1000);
	}
}
void delay_us(unsigned int ms){
	while(ms--){
	__delay_cycles(0);
  // asm("nop");
	}
}
void sonar_init(void)
{
  P1DIR |=TRIGGER_PIN;
  P1OUT &= ~TRIGGER_PIN;
  P1SEL  &=  (~ECHO_PIN);
	P1DIR  &=  (~ECHO_PIN);
  P1OUT |= ECHO_PIN;
  P1REN &= ~ECHO_PIN;
  P1IES  |=  (ECHO_PIN); //  Falling Edge    1   ->  0
	P1IFG  &=  (~ECHO_PIN);    //  Clear   interrupt   flag    for P2.1
	P1IE   |=  (ECHO_PIN); //  Enable  interrupt   for P2.1
	TA0CTL |= TASSEL_2 | ID_3| MC_2;
  //  __bis_SR_register( GIE);
  //  __enable_interrupt();
}
unsigned int sonar_distance(void)
{
  // int i = 0;
  unsigned long time =0;
  float distance = 0;
  P1OUT &= ~TRIGGER_PIN;
  delay_ms(2);
  P1OUT |=TRIGGER_PIN;
  __delay_cycles(6);
  P1OUT &= ~TRIGGER_PIN;
  // __delay_cycles(6000);
	TAR = 0;
  do{
    //time++;
    // __delay_cycles();
    // P1OUT &= ~BIT6;
  }while(echo_flag ==0);
	time = TAR;
  // P1OUT |=BIT6;
  echo_flag = 0;
  distance = ((time*10)/58)-11;
  return distance;
}
void uart_init(void)
{
  DCOCTL = 0; // Select lowest DCOx and MODx settings<
  BCSCTL1 = CALBC1_1MHZ; // Set DCO
  DCOCTL = CALDCO_1MHZ;
  P1SEL |= RXD + TXD ; // P1.1 = RXD, P1.2=TXD
  P1SEL2 |= RXD + TXD ; // P1.1 = RXD, P1.2=TXD
  P1DIR |= RXLED + TXLED;
  P1OUT &= 0x00;
  UCA0CTL1 |= UCSSEL_2; // SMCLK
  UCA0BR0 = 0x08; // 1MHz 115200
  UCA0BR1 = 0x00; // 1MHz 115200
  UCA0MCTL = UCBRS2 + UCBRS0; // Modulation UCBRSx = 5
  UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
  // UC0IE |= UCA0RXIE; // Enable USCI_A0 RX interrupt
   __bis_SR_register( GIE);
}

void uart_send_byte(char data )
{
	// while(!UCA0TXIE);
	while (!(IFG2&UCA0TXIFG)) ;
	UCA0TXBUF = data;
}
void uart_send_string(char *string, int len)
{
	int k = 0;
	while(k < len){
		uart_send_byte(string[k]);
		k++;
	}
	}
void send_uart_distance(unsigned long distance )
{
  char buf[]={"0000\n\r"};
	char message[]= {"Distance\n\r"};
	uart_send_string(message,10);
	buf[4] = (distance%10) + 0x30;
	distance = distance / 10;
	buf[3] = (distance%10) + 0x30;
	distance = distance / 10;
	buf[2] = (distance%10) + 0x30;
	distance = distance / 10;
	buf[1] = (distance%10) + 0x30;
	distance = distance / 10;
	buf[0] = (distance%10) + 0x30;
	distance = distance / 10;
	uart_send_string(buf,6);
	// int k = 0;
	// uart_send_byte('N');
	// for (k=0; k < (sizeof(buf) / sizeof(char));k++)
	// {
	// 	uart_send_byte(buf[k]);
	// }
}

void motor_init(void)
{
  MOTOR_PORT_DIR |=(LEFT_MOTOR_ENABLE +RIGHT_MOTOR_ENABLE + LEFT_MOTOR_PIN1 + LEFT_MOTOR_PIN2 + RIGHT_MOTOR_PIN1 +RIGHT_MOTOR_PIN2);
  MOTOR_PORT = 0x00;
  MOTOR_PORT |=(LEFT_MOTOR_ENABLE + RIGHT_MOTOR_ENABLE);
}

 void motor_stop(void)
 {
    MOTOR_PORT = 0x00;
 }

 void move_forward(void)
 {
   MOTOR_PORT |=(LEFT_MOTOR_ENABLE + RIGHT_MOTOR_ENABLE);
   MOTOR_PORT |=(LEFT_MOTOR_PIN2 + RIGHT_MOTOR_PIN1);
 }

 void move_back(void)
 {
   MOTOR_PORT |=(LEFT_MOTOR_ENABLE + RIGHT_MOTOR_ENABLE);
   MOTOR_PORT |=(LEFT_MOTOR_PIN1 + RIGHT_MOTOR_PIN2);
 }
void right(void)
{
	MOTOR_PORT |=(LEFT_MOTOR_ENABLE + RIGHT_MOTOR_ENABLE);
	MOTOR_PORT|=RIGHT_MOTOR_PIN1 + LEFT_MOTOR_PIN1;
}
void left(void)
{
	MOTOR_PORT |=(LEFT_MOTOR_ENABLE + RIGHT_MOTOR_ENABLE);
	MOTOR_PORT|=RIGHT_MOTOR_PIN2 + LEFT_MOTOR_PIN2;
}


unsigned long i=0;
unsigned long j=0;
unsigned long distance = 0;
int main(void) {
  WDTCTL = WDTPW + WDTHOLD;
  P1DIR |= LED;
  // P1OUT &=~LED;
  motor_init();
  sonar_init();
	uart_init();
  // P1OUT |= TXLED;
    // P1OUT ^= BIT6;
  while(1)
  {
    // P1OUT ^= LED;
    delay_ms(50);
		P1OUT &=~BIT6;
		// delay_ms(1000);
    echo_flag = 0;
    distance = sonar_distance();
    // send_uart_distance(distance);
		if(distance >10)
		{
			move_forward();
			delay_ms(100);
			motor_stop();
		}
		else
		{
			move_back();
			delay_ms(500);
			motor_stop();
			left();
			delay_ms(700);
			motor_stop();
		}
		// move_forward();
		// right();
		// delay_ms(100);
		// motor_stop();
		// move_back();
		// delay_ms(100);
		// motor_stop();
  }
  return 0;
}



#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void)
{
   P1OUT |= RXLED;
   if (UCA0RXBUF == 'a') // 'u' received?
   {
      i = 0;
      UC0IE |= UCA0TXIE; // Enable USCI_A0 TX interrupt
      UCA0TXBUF = string[i++];
   }
   P1OUT &= ~RXLED;
}

#pragma vector=USCIAB0TX_VECTOR
__interrupt void USCI0TX_ISR(void)
{
   P1OUT |= TXLED;
     UCA0TXBUF = string[i++]; // TX next character
    if (i == sizeof string - 1) // TX over?
       UC0IE &= ~UCA0TXIE; // Disable USCI_A0 TX interrupt
    P1OUT &= ~TXLED; }

  #pragma vector=PORT1_VECTOR
  __interrupt void  Port_1(void)
  {
    	echo_flag = 1;
      P1OUT |= BIT6;
    	P1IFG &= ~ECHO_PIN;    //     IFG clear
    }

Через меню PLatformio выбираем Upload. И Все наш робот можем перед препятствиями менять свое направление

0
24 мая 2016, 08:04
namoaton

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.

Материалы сайта являются авторскими. Копирование и публикация материалов без разрешения запрещены!

Делаем простого робота. Часть третья

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

Блоги

Прямой эфир

Авторизация

Делаем простого робота. Часть третья

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

Блоги

Прямой эфир