계단올라가는 로봇 프로그램.

서보 위치 변경, 상태 측정용 센서(수은 스위치) 추가 등으로 하여

장애물 넘어가는 프로그램 완성. 서보가 토크가 약간 부족한거 같은데,

서보와 DC모터가 동시에 회전하면서 아답터에서 전류가 부족하여 발생하는 현상으로 생각된다.

계속 움직이거나 잠깐 섰다가 다시 돌리면 넘어간다.

어쨋든 넘어가는데 큰 문제는 없는거 같고, 또한 뒤집어지는 일도 별로 없다.

할일

1. 배터리및 파워로 전류문제 확인.

2. 방향 전환 프로그래밍(직진이나 후진하다가 방향 전환키를 누르면 동작이 좀 애매하다.)

방향전환키는 아예 제자리 회전으로 만들어버리던지 해야겠다.

3. 선 정리.(얇은 케이블 타이 구입)

4. 사진 찍어서 올려주마.ㅋ

//06.06.29
//포트 셋팅, 시리얼통신, LCD체크
//06.07.04
//DC, 서보 모터 동작 완료.

//06.07.14

//로봇 제어 프로그램 완성

#include
#include

#include "delay.h"
#include "lcd.h"

#define SERVO_ON 0x01
#define SERVO_OFF 0x02

//volatile u08 right_motor_dir;
//volatile u08 left_motor_dir;
volatile u08 timer0_count =0;
volatile u08 right_motor_speed=128;//정지 상태
volatile u08 left_motor_speed=128;//정지 상태
volatile u08 motor_speed = 128;
volatile u16 servo_counter =0 ;
//volatile u08 servo_flag =0 ;
//volatile u08 motor_temp;

volatile u16 ultrasonic_range;
volatile int servo_up_time = 0;

volatile u08 mercury_sensor;
volatile u08 int_count = 0;

void USART_Init(void){
UBRR1H= 0;
UBRR1L= 8; //16MHZ 115200BPS
UCSR1A=0x00;
UCSR1B=0x98;
UCSR1C=0x06;

/*UBRR0H= 0;
UBRR0L= 51;
UCSR0A=0x00;
UCSR0B=0x98;
UCSR0C=0x06; */
}

void CONVERT_value2array(char temp [6], int value){
char i,j;

if( value < 10) i=0;
else if ( value < 100) i=1;
else if ( value < 1000) i=2;
else if ( value < 10000) i=3;
else i=4;

for( j=0; j<=i; j++){
temp [i-j] = (value % 10) + 48;
value /= 10;
}
temp[i+1] = 0;

}

void TX1_char(unsigned char data){
while((UCSR1A & 0x20) == 0x00);
UDR1 = data;
}

void TX1_string(unsigned char *string){
while(*string != ''){
TX1_char(*string);
string++;
}
}

void LF_CR(void){
TX1_char(0x0a);//line feed
TX1_char(0x0d);//carriage return
}

void TX_servo_angle(void){
char temp[6];

TX1_string("servo1= ");
CONVERT_value2array(temp, OCR0);
TX1_string(temp);
LF_CR();

TX1_string("servo2= ");
CONVERT_value2array(temp, OCR2);
TX1_string(temp);
LF_CR();
}

void TX_motor_speed(void){
char temp[6];

TX1_string("motor speed= ");
CONVERT_value2array(temp, motor_speed);
TX1_string(temp);
LF_CR();
}

void TX_ultrasonic(void){
char temp[6];

TX1_string("Ultrasonic range= ");
CONVERT_value2array(temp,ultrasonic_range);
TX1_string(temp);
LF_CR();
}

void servo_on(void){
TCCR0 = TCCR0 | (1<TCCR2 = TCCR2 | (1<servo_counter = 50;
}

void servo_off(char num){
if(num == 1) TCCR0 = TCCR0 & ~(1<else TCCR2 = TCCR2 & ~(1<}

void servo_up(char num){
if(num == 1) {
if( OCR0 > 57 ) OCR0-=1;
}
else if( OCR2 < 130 ) OCR2+=1;
}

void servo_down(char num){
if(num == 1) {
if( OCR0 < 130 ) OCR0+=1;
}
else if( OCR2 >60 ) OCR2-=1;
}

void servo_center(char num){
if(num == 1) {
OCR0 =84;
}
else OCR2 =92;
}

void dc_motor_on(void){
PORTA = 0x03;
}

void RX1_check(u08 data){
switch (data) {
case 117: //'u'//서보 1 상승
servo_up(1); break;

case 106://'j'//서보 1 하강
servo_down(1); break;

case 105://'i'//서보 2 상승
servo_up(2); break;

case 107://'k'//서보 2 하강
servo_down(2); break;

case 111://'o': 서보 동작 시작
servo_on();
servo_counter = 2000; break;

case 108://'l' : 서보 동작 해제
servo_off(1); servo_off(2); break;

case 112://'p' : 서보 각도 값 표시
TX_servo_angle();
TX_motor_speed(); break;

case 116://'t'
TX_ultrasonic(); break;

case 119://'w'
dc_motor_on();

if(motor_speed <= 252) motor_speed+=3;
right_motor_speed = left_motor_speed = motor_speed;
break;

case 115 : //'s'
PORTA = 0;
servo_on();
servo_center(1); servo_center(2);
motor_speed = 128; break;

case 120 : //'x'
dc_motor_on();

if(motor_speed >= 3) motor_speed-=3;
right_motor_speed = left_motor_speed = motor_speed;
break;

case 97 : //'a'
dc_motor_on();
motor_speed = 128;
if(right_motor_speed <= 252) right_motor_speed+=3;
if(left_motor_speed >= 3) left_motor_speed-=3;
break;

case 100 : //'d'
dc_motor_on();
motor_speed = 128;
if(right_motor_speed >= 3) right_motor_speed-=3;
if(left_motor_speed <=252) left_motor_speed+=3;
break;
}
OCR3AL = right_motor_speed;
OCR3BL = left_motor_speed;
}

void servo_position(void){//서보의 동작을 결정
char temp[6];
u08 i;

if( (mercury_sensor & 0x03) == 0 ){//뒤집어 질려고 할때라면
servo_on();
servo_center(2);
for(i=0; i<5; i++) servo_down(1);
TX1_string("dngr ");
if( (mercury_sensor & 0x04) ==0){
for(i=0; i<5; i++) servo_down(1);
motor_speed--;
TX1_string("over ");
}
}

if(ultrasonic_range <20) {
if(OCR2 >= 120) servo_up(1);

else servo_up(2);
servo_up_time++;
TX1_string("up ");

}

else if( (ultrasonic_range > 20) && (servo_up_time >0) ) {
servo_up_time--;
if (OCR2 > 100) servo_down(1);
else if(OCR2 >92) servo_down(2);
TX1_string("down ");
}

if ( (servo_up_time == 0) ){
servo_center(2);
if( (mercury_sensor&0x03) ){
servo_center(1);
TX1_string("end ");
}
//motor_speed++;

}

//CONVERT_value2array(temp, servo_up_time);
//TX1_string("sut=");
//TX1_string(temp); TX1_string(" ");

//TX1_string("m_s=");
//TX1_char( (mercury_sensor& 0x03)+48); TX1_string(" ");

}

SIGNAL(SIG_UART1_RECV){
u08 rx_temp;
rx_temp=UDR1;
//lcd_display_string(0,0,"RX= ");
//lcd_display(rx_temp);
RX1_check(rx_temp);
}

SIGNAL(SIG_INTERRUPT0){
ultrasonic_range = TCNT1L;
ultrasonic_range = ultrasonic_range | TCNT1H<<8;//소리가 돌아온 시간을 저장한 TCNT1 읽어들임

if(ultrasonic_range >36000) lcd_display_string(7,1,"SonicOut");

else {

ultrasonic_range = ultrasonic_range /120;//소리의 속도와 거리 계산.
//근사값

lcd_display_string(0,1,"R=");
lcd_display_de3(2,1,ultrasonic_range);
lcd_display_string(5,1," ");

//lcd_display_string(11,1,"cm");
}
}

SIGNAL(SIG_OVERFLOW0){
if(timer0_count == 15 ) {
PORTF = 0x80;
Delay_us(7);
PORTF = 0x00;//초음파 센서를 구동하는 10us 펄스

TCNT1 = 0x00;
timer0_count =0;
}
else timer0_count++;

if (servo_counter == 0) {
servo_off(1); servo_off(2);

}
else servo_counter--;

if(motor_speed > 128) servo_position();

TCNT0 = 0x00;
}

int main(void){
DDRA = 0xff;//DC모터 enable: A0 -오른쪽 모터, A1 - 왼쪽 모터
DDRB = 0xff; //PB4 - 서보1, PB7 - 서보2
DDRC = 0xff;//LCD data(PC0-7)
DDRD = 0xf4; //LCD control(PD5-7), PD2 - RX1, PD3 - TX1, PD0 - 초음파 센서 에코 체크
DDRE = 1<//PE4 - 왼쪽 모터 PWM, PE5,6 = 수은 스위치 입력
PORTE = 1<DDRF = 0x80;//센서용(PF0 - LM35, PF7 - 초음파 센서 트리거)

TCCR0 = 1<TCNT0 = 0x00;
//타이머 0은 일정 시간마다 초음파 센서를 동작시키는 펄스 공급 및 서보모터 0의 PWM 공급

TCCR2 = 1<TCNT2 =0x00;
servo_center(1); servo_center(2);//로봇이 평행한 상태

TIMSK = 1<//타이머 0 오버플로우 인터럽트 사용(초음파 구동)

TCCR1A = 0x00;
TCCR1B = 1<TCCR1C = 0x00;
//타이머 1은 시간을 측정하기 위하여 사용
//100us : TCNT1 = 200 , 18ms : TCNT1 = 36000;

TCCR3A = 1<TCCR3B = 1<TCCR3C = 0x00;
OCR3AL = 128;//DC모터 속도 0
OCR3BL = 128;//DC모터 속도 0

EICRA = 1<//EICRB = 1<

EIMSK = 1<//EIMSK = EIMSK | 1<

USART_Init();

servo_on();
servo_center(1); servo_center(2);

Delay_ms(50);
init_lcd();

lcd_display_string(0,0,"System Ready");
TX1_string("System Ready================================================"); LF_CR();

sei();

for(;;){
mercury_sensor = PINE;
mercury_sensor = (mercury_sensor & 0xe0) >>4;
PORTB = mercury_sensor;
}
}