Login▼/회원가입BASIC4MCU | 질문게시판 | 아트메가 128 & 초음파 센서..
페이지 정보
작성자 초심자2 작성일2019-01-31 14:55 조회287회 댓글0건본문
현재 간단한 실내주행 카트를 만들기 위해 초음파센서를 사용중입니다.
용도는 차체 양쪽에 부착하여 복도의 중앙으로 주행할수 있기 위함인데,,
먼저 간단한 주행정도라도 해보려고 하여
작성한 코드로 초음파 센서의 cm값이 잘 나오는데, 모터는 굴러가나 각 조건에 대하여 직 접 사용중인 MOTOR관련 LEFT(왼쪽 바퀴속도 조정) , RIGHT(오른쪽바퀴속도조정) 함수가 동작하질 않습니다.
이유좀 알수 있을까요...?
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdio.h>
#include <math.h>
#include "UART.h"
#include "delay_timer2.h"
#include "Motor.h"
#include "timer3_pwm.h"
#include "avr_lcd_1602_4bit.h"
#include "delay_u_mSec.h"
#include "PSD.h"
//
volatile unsigned int buf[2],dist[2],start=0,end=0;
volatile unsigned char cnt=0,flag[2]={0,};
void SCI_OutChar(char letter){
// 이 예는 UART를 표준 출력 장치로 사용 하는 예 이다.
// 아래 함수를 다른 장치로 문자를 출력 하는 함수로 바꾸면
// 다른 장치를 표준 출력 장치로 사용 할 수 있다.
//tx0_char(letter);
}
void SCI_OutUDec(unsigned short n){
if(n >= 10){
SCI_OutUDec(n / 10); // Recursive Operation
n = n % 10;
}
SCI_OutChar(n + '0'); // n(0-9)을 문자('0' - '9')로 변환 한다.
}
//
ISR(INT0_vect){ if(EICRA==0x03){start=TCNT1; } else{ end=TCNT1; dist[0]=(end-start)*0.276; EIMSK=0; flag[0]=1; } EICRA^=0x01; }//인터럽트 서비스 루틴 (왼쪽 초음파 센서)
ISR(INT1_vect){ if(EICRA==0x0C){start=TCNT1; } else{ end=TCNT1; dist[1]=(end-start)*0.276; EIMSK=0; flag[1]=1; } EICRA^=0x04; }//인터럽트 서비스 루틴 (오른쪽 초음파 센서)
// EICRA - External Interrupt Control Register A (외부 인터럽트의 0번~3번의 인터럽트 센싱 모드 설정)
// EIMSK - INT7-0 Interrupt를 개별적으로 허용하는데 사용
// TCNTx - 타이머/카운터 레지스터 (실제 카운트되는 수치가 저장되는 레지스터
// TCCRx - 타이머/카운터 제어 레지스터(주로 동작모드 및 프리스케일러 설정
ISR(TIMER1_COMPA_vect){
switch(cnt){
case 0: PORTF|=0x01; _delay_us(10); PORTF&=~0x01; EICRA=0x03; EICRB=0x00; EIFR=0xFF; EIMSK=0x01; break;
case 1: PORTF|=0x02; _delay_us(10); PORTF&=~0x02; EICRA=0x0C; EICRB=0x00; EIFR=0xFF; EIMSK=0x02; break;
}
if(++cnt>2)cnt=0;
}
//
void sonar_init(void)
{
DDRD &= ~(0x03);
DDRF|=0xFF; // 트리거
TCCR1B=0x0C; OCR1A=3124; TIMSK=0x10; //16000000/256/(1+ 3124)=20Hz=50ms
SREG=0x80;
/* while(1){
if(flag[0]){ flag[0]=0; dist[0]=(int)((float)buf[0]/14.5); }
if(flag[1]){ flag[1]=0; dist[1]=(int)((float)buf[1]/14.5); }
}*/
}
void sonar(){
//dist[0]=(int)((float)buf[0]*(0.276));
// dist[1]=(int)((float)buf[1]*(0.276));
/* OCR1A = PWM_Max; //왼
OCR1B = PWM_Max; //오
*/
// SCI_OutChar('F');
// SCI_OutChar(' ');
LCD_Clear();
SCI_OutChar('a');SCI_OutUDec(dist[0]);
LCD_OutChar('a');LCD_OutUDec(dist[0]);
SCI_OutChar('b');SCI_OutUDec(dist[1]);
LCD_OutChar('b');LCD_OutUDec(dist[1]);
SCI_OutChar(' ');
LCD_SetCsr(2,1);
//기본적인 초음파 센서 주행 알고리즘 (PID 제어 적용 이전)
if(fabs(dist[0]-dist[1])<3 ){
MotorGoFoward();
}
else if(dist[0]>(dist[1]+2)) { //왼쪽에 치우쳤을때
if(dist[0]>(dist[1]+10)){
Right1(200);
}
else if(dist[0]>(dist[1]+20)){
Right1(300);
}
else{
Right1(100);
}
}
else if((dist[0]+2)<(dist[1])) { //오른쪽에 치우쳤을때
if(dist[1]>(dist[0]+10)){
Left1(200);
}
else if(dist[1]>(dist[0]+20)){
Left1(300);
}
else{
Left1(100);
}
}
delay_1mSec(20);
// SCI_OutChar(' ');
LCD_OutChar(' ');
}
/*******************************************************Motor.C 파일**********************************************************/
#include <avr/io.h>
#include <avr/interrupt.h>
#include "8_sonar.h"
#include "Motor.h"
#include "timer3_pwm.h"
#include "delay_timer2.h"
#include "PSD.h"
void Motor_init(){
//MotorPort &= ~(Motor_Mask);
MotorPort_DDR |= Motor_Mask;
MotorPort_PWMDDR = 0x18;
asm volatile(" NOP");
}
void MotorGoFoward()//직진
{
// MotorStop();
// delay_1mSec(10);
// PORTF = 0x02;
MotorPort |= (1<<MotorL_1)|(1<<MotorR_1);
MotorPort &= ~((1<<MotorL_2)|(1<<MotorR_2));
asm volatile(" NOP");
}
void MotorGoBackward()//후진
{
// PORTF = 0x03;
MotorPort &= ~((1<<MotorL_1)|(1<<MotorR_1));
MotorPort |= (1<<MotorL_2)|(1<<MotorR_2);
asm volatile(" NOP");
}
void TurnRight()//오른쪽턴
{
// PORTF = 0x04;
OCR3A = 700;
OCR3B = 600;
MotorPort |= (1<<MotorL_1)|(1<<MotorR_2);
MotorPort &= ~((1<<MotorL_2)|(1<<MotorR_1));
}
void TurnLeft() //왼쪽턴
{
// PORTF = 0x05;
OCR3A = 700;
OCR3B = 600;
MotorPort |= (1<<MotorL_2)|(1<<MotorR_1);
MotorPort &= ~((1<<MotorL_1)|(1<<MotorR_2));
}
void MotorStop()//정지
{
// PORTF = 0x06;
MotorPort &= ~((1<<MotorL_2)|(1<<MotorR_1)|(1<<MotorL_1)|(1<<MotorR_2));
asm volatile(" NOP");
}
void MotorCommand(char command)//유아트용 명령어
{
MotorPort |= 0x00;
if ( command == 'w') avoid_drive();
else if ( command == 'x') MotorGoBackward();
else if ( command == 's')
{ DDRE=1;
MotorStop();
PORTE = 1;
}
else if ( command == 'a') TurnLeft();
else if ( command == 'd') TurnRight();
else if ( command == 'q') Left3();
else if ( command == 'z') Left4();
else if ( command == 'e') Right3();
else if ( command == 'c') Right4();
}
void Left1(int a)//Custom 좌회전
{
MotorGoFoward();
OCR3A = 1000 - a;//필요한 만큼 조절
OCR3B = 1000;
}
void Left2(void)//강한 좌회전
{
MotorGoFoward();
OCR3A = 1000 - 400;
OCR3B = 1000;
}
void Left3(void)//강한 좌회전
{
MotorGoFoward();
OCR3A = 1000 - 800;
OCR3B = 1000;
}
void Left4(void)//강한 좌회전
{
MotorGoFoward();
OCR3A = 0;
OCR3B = 1023;
}
void Right1(int b)//Custom 우회전
{
MotorGoFoward();
OCR3A = 1000;
OCR3B = 1000-b;//필요한 만큼 조절
}
void Right2(void)//강한 우회전
{
MotorGoFoward();
OCR3A = 1023;
OCR3B = 900-500;
}
void Right3(void)//강한 우회전
{
MotorGoFoward();
OCR3A = 1023;
OCR3B = 900-800;
}
void Right4(void)//강한 우회전
{
MotorGoFoward();
OCR3A = 1023;
OCR3B = 0;
}
- BASIC4MCU 작성글 SNS에 공유하기
댓글 0
조회수 287등록된 댓글이 없습니다.