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