วันจันทร์ที่ 23 กรกฎาคม พ.ศ. 2555

รถวิ่งตามเส้น



บทนำ

วิธีการเขียนโปรแกรมรถวิ่งตามเส้นนั้นมีได้หลายตามแต่ใครใครจะเขียนแต่ในที่นี้จะเน้นหลักการแบบ PID ซึ่งดูที่ error เป็นหักแล้วใช้การปรับค่า Kp Ki Kd และเวลาในการปรับ

หลักการ

1.ง่ายๆเรื่มจาก ให้เซนเซอร์ชุดทางซ้ายคุมล้อซ้าย และเซนเซอร์ทางขวาคุมล้อขวาถ้าแทรกพื้นสีขาวเส้นสีดำ
ให้เซนเซอร์ล้อซ้ายโดนเส้นดำ ส้อซ้ายหยุด
ให้เซนเซอร์ล้อขวาโดนเล้นดำล้อขวาหยุด
เพียงเท่านี้ก็สามารถทำรถให้วิ่งตามเส้นได้ง่ายๆ
2. ถ้าให้ละเอียดกว่านั้นหน่อยค่าที่อ่านจากเซนเซอร์เป็นค่า0-1024 ค่าที่เข้าไกล้สีดำตัวเลขยิ่งลด
ดังนั้นเราสามารถเอาค่าดังกล่าวคุมค่า PWM ของล้อนั้นๆได้โดยตรง การวิ่งของรถก็จะเพื่มความละเอียดมากๆขึ้น
PWM(L) = 1000 - เซนเซอร์ L
PWM(R) = 1000 - เซนเซอร์ R
3.จากสมการ PID (ดูบทความเรื่อง PID) ให้
PWM = Kp*error + Kd*deltar error + Ki * sum error
error = ADCx[0] + ADCx[1] + ADCx[2] - ADCx[3] - ADCx[4] - ADCx[5];
Delta_error = error - error_o;
error_o = error;
Sum_error =+ error;
ถ้า -PWM ให้ล้อซ้ายหยุด ถ้า + PWM ให้ล้อขวาหยุด
6. ให้ทำการคำนวณหา รัศมีการเลี้ยวของรถ
R = L/2((VR+VL)/(VR-VL))
โดย R = รัศมีการเลี้ยวของรถ
L = ความยาวจากจุดศูนย์ล้อซ้ายถึงล้อขวา
VR = ความเร็วล้อขวา
VL = ความเร็วล้อซ้าย

อุปกรณ์

วิธีต่ออุปกรณ์

ซอฟท์แวร์


//
//
//
//
//
/****************************************************************** Includes */
#include <stdio.h> // Standard Input/Output
#include <avr/io.h> // AVR device-specific IO definitions
#include <compat/deprecated.h> // Use sbi(), cbi() function
#include <avr/interrupt.h> // Interrupt Service routine
#include <math.h>
#define F_CPU 16000000UL // CPU clock frequency (in Hertz)
#include <util/delay.h> // util_delay
#define RX_BUFSIZE 80 // Buffer RX
/******************************************************* Prototype functions */
static int uart_putchar(char c, FILE *stream);
static int uart_getchar(FILE *stream);
static FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
void Init_Serial(unsigned int baudrate);
void car_move(char LR, char FB,unsigned int spreed);
unsigned int ADCx[8];
// ***********************************************************//
int error;
int error_o;
int Delta_error;
int Sum_error;
int pwm;
// ***********************************************************//
/************************************************************* USART putchar */
static int uart_putchar(char c, FILE *stream)
{
loop_until_bit_is_set(UCSRA, UDRE);
UDR = c;
return 0;
}
/************************************************************* USART getchar */
static int uart_getchar(FILE *stream) {

uint8_t c;

loop_until_bit_is_set(UCSRA, RXC);

if (UCSRA & _BV(FE)) return _FDEV_EOF;
// if (UCSRA & _BV(DOR)) return _FDEV_ERR;
c = UDR;
return c;
}
/*********************************************************** Initialize UART */
static void USART_Init(unsigned int baud)
{
// Set baud rate
UBRRH = (unsigned char) (baud>>8);
UBRRL = (unsigned char) baud;

UCSRA = (1<<U2X);
// Enable receiver and tramsmitter
UCSRB = (1<<RXEN) | (1<<TXEN);
// Set frame format: 8data, NoneParity, 1stop bit
UCSRC = (1<<URSEL)|(3<<UCSZ0);
// Set address uart_str to stdout/stdin
stdout = stdin = &uart_str;
}
/************************************************************ Main Functions */
/***************************************************************** delay_ms */
void delay_ms(uint16_t i)
{
for (;i > 0; i--)
_delay_ms(1);
}
/************************************************************* USART PWM */
void init_PWM1(void)
{
// PORT PD5 PD6 PD7 Output
DDRD =(1<<DDD5)|(1<<DDD6)|(1<<DDD7);
// PORT PB1(OC1A),PB2(OC1B) Output PWM
DDRB = (1<<DDB0)|(1<<DDB1)|(1<<DDB2);
// Clear up-counting Set downcounting
TCCR1A = (1<<COM1A1)|(0<<COM1A0);
TCCR1A |= (1<<COM1B1)|(0<<COM1B0);
// Mode 8: PWM,Phase and Frequency Corect (TOP = ICR1A)
TCCR1B = (1<<WGM13)|(0<<WGM12);
TCCR1A |= (0<<WGM11)|(0<<WGM10);

// clk_IO/1 (From prescaler)
TCCR1B |= (0<<CS12)|(1<<CS11)|(0<<CS10);
ICR1 = 0x3E8; // Freq = 1000 Hz(TOP = ICR1) //1000=100%

// Duty Cycle 50%
// OCR1A = 0x1F4; // Output Compare Register 1 A //500 = 50%
// OCR1B = 0x1F4; // Output Compare Register 1 B //500 = 50%
OCR1A = 0;
OCR1B = 0;
TCNT1 = 0; // Clear Timer

// T/C1 Output Compare A/B Match and Timer Overflow interrupt Enable
// TIMSK = (1<<OCIE1A)|(1<<OCIE1B)|(1<<TOIE1);
// sei(); // Set I-bit global interrupt enable


}
int ADC_init(void)
{
DDRC = 0x00;
// AVCC with external capacitor at AREF pin
ADMUX = (0<<REFS1)|(1<<REFS0);
// ADC Enable & Auto Trigger Disable
ADCSRA = (1<<ADEN);
ADCSRA |= (1<<ADPS2)|(0<<ADPS1)|(1<<ADPS0); // XTAL/8
ADCSRA |= (1<<ADSC); // ADC Start Conversion
while (!(ADCSRA &(1<<ADIF))); // Wait Coversion completes
return ADCW;
}
void ADC_read(void)
{
ADMUX = (0<<REFS1)|(1<<REFS0)|0x00;
ADCSRA |= (1<<ADSC); // ADC Start Conversion
while (!(ADCSRA &(1<<ADIF))) // Wait Coversion completes
;
ADCx[0] = ADCW; // Read ADC
ADMUX = (0<<REFS1)|(1<<REFS0)|0x01;
ADCSRA |= (1<<ADSC); // ADC Start Conversion
while (!(ADCSRA &(1<<ADIF))) // Wait Coversion completes
;
ADCx[1] = ADCW; // Read ADC
ADMUX = (0<<REFS1)|(1<<REFS0)|0x02;
ADCSRA |= (1<<ADSC); // ADC Start Conversion
while (!(ADCSRA &(1<<ADIF))) // Wait Coversion completes
;
ADCx[2] = ADCW; // Read ADC
ADMUX = (0<<REFS1)|(1<<REFS0)|0x03;
ADCSRA |= (1<<ADSC); // ADC Start Conversion
while (!(ADCSRA &(1<<ADIF))) // Wait Coversion completes
;
ADCx[3] = ADCW; // Read ADC
ADMUX = (0<<REFS1)|(1<<REFS0)|0x04;
ADCSRA |= (1<<ADSC); // ADC Start Conversion
while (!(ADCSRA &(1<<ADIF))) // Wait Coversion completes
;
ADCx[4] = ADCW; // Read ADC
ADMUX = (0<<REFS1)|(1<<REFS0)|0x05;
ADCSRA |= (1<<ADSC); // ADC Start Conversion
while (!(ADCSRA &(1<<ADIF))) // Wait Coversion completes
;
ADCx[5] = ADCW; // Read ADC
}
int main(void)
{
int i;
float k_p = 1.1; //???????????? ?????????????? ??????????????????
float k_i = 0;
float k_d = 10;
USART_Init(16); // baudrate to 115,200 bps using a 16MHz crystal
init_PWM1();
ADC_init();
while (1) {
ADC_read();
//-----------------------------------------------------------------------------------------//
//------------------------------------- PID-----------------------------------------------
error = ADCx[0] + ADCx[1] -ADCx[2] - ADCx[3];// ?????? ????????????
Delta_error = error - error_o;
error_o = error;
Sum_error =+ error;
pwm = k_p*error + k_d*Delta_error + k_i*Sum_error;
//------------------------------------- PID-----------------------------------------------
if(pwm < 0){
if(pwm < - 1000){
car_move('L','F',1000);
car_move('R','B',(-pwm)-1000);

}else{
car_move('L','F',1000);
car_move('R','F',1000-(-pwm));
}
}else{
if(pwm > 1000){
car_move('L','B',(pwm)-1000);
car_move('R','F',1000);

}else{
car_move('L','F',1000-pwm);
car_move('R','F',1000);
}
}

/*
if(ADCx[0] <= 400 && ADCx[1] <= 400){
car_move('L','F',700);
car_move('R','F',700);
}else{
if(ADCx[1] <= 400){
car_move('L','B',1000-ADCx[1]);
}else{
car_move('L','F',ADCx[1]+ADCx[3]/2);
}
if(ADCx[0] <= 400){
car_move('R','B',1000-ADCx[0]);
}else{
car_move('R','F',ADCx[0]+ADCx[2]/2);
}
}
*/
//-----------------------------------------------------------------------------------------//
// car_move('L','F',ADCx[1]);
// car_move('R','F',ADCx[0]);
if(ADCx[0] < 50 && ADCx[1] < 50 && ADCx[2] < 50&& ADCx[3] < 50&& ADCx[4] < 50&& ADCx[5] < 50){
break;
}
for(i=0;i<6;i++){
printf("%4d = %4d",i,ADCx[i]);
}
printf(" %4d ",pwm);
printf("\r\n");
delay_ms(2);
//-----------------------------------------------------------------------------------------//
}
car_move('L','x',0);
car_move('R','x',0);
while(1);
}
void car_move(char LR, char FB,unsigned int spreed){
switch(LR) {
case 'L' :
if(FB == 'F'){
sbi(PORTD,7); // Output High PD7
cbi(PORTB,0); // Output Low PB0
}else if(FB == 'B'){
cbi(PORTD,7); // Output Low PD7
sbi(PORTB,0); // Output High PB0
}else{
cbi(PORTD,7); // Output Low PD7
cbi(PORTB,0); // Output Low PB0
}
OCR1A = spreed;
break;
case 'R' :
if(FB == 'F'){
sbi(PORTD,6); // Output High PD6
cbi(PORTD,5); // Output Low PD5
}else if(FB == 'B'){
cbi(PORTD,6); // Output Low PD6
sbi(PORTD,5); // Output High PD5
}else{
cbi(PORTD,6); // Output Low PD6
cbi(PORTD,5); // Output Low PD5
}
OCR1B =spreed;
break;
}
}
เขียนโดย ที่

ไม่มีความคิดเห็น:

แสดงความคิดเห็น

สมัครสมาชิก: ส่งความคิดเห็น (Atom)