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