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


Для нашего робота осталось совсем немного — добавить управление ультразвуковым датчиком.
Обычно все используют готовые библиотеки на 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. И Все наш робот можем перед препятствиями менять свое направление


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

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение