?? autocar4_4.1.c
字號(hào):
#include "Main.h"
void port_init(void)
{
PORTA = 0xFF; //前排8路普通傳感器輸入口，//左 PA7, PA6, PA5, PA4, PA3, PA2, PA1, PA0 右//
DDRA = 0x00;
PORTB = 0xFF;
DDRB = 0xF0;
PORTC = 0xFF; //m103 output only //傳感器輸入口，PC7,PC6,PC5,PC4,PC4,PC3,PC2,PC1,PC0
DDRC = 0x00;
PORTD = 0xFF;//extra傳感器PD4~PD7，鍵盤：S-RXD===PD0，S-TXD====PD1，片選：S-SET-L====PD2，S-SET-R====PD3
DDRD = 0x00;
}
//TIMER2 initialisation - prescale:1024
// WGM: Normal
// desired value: 30mSec
// actual value: 29.952mSec (0.2%)
void uart0_init(void)
{
UCSRB = 0x00; //disable while setting baud rate
UCSRA = 0x00;
UCSRC = 0x86;
UBRRL = 51; //set baud rate lo
UBRRH = 0x00; //set baud rate hi
UCSRB = 0x98;
}
#pragma interrupt_handler uart0_rx_isr:12
void uart0_rx_isr(void)
{
//uart has received a character in UDR
uchar i;
uchar tmp;
uint e2p_addr=0;
CLI();
flag=1;
for( i = 0; i < 20; i ++ )
{
while( !( UCSRA & BIT( RXC ) ) );
receive_buf[i] = UDR;
}
if (receive_buf[0]==1)tmp=8;
if (receive_buf[0]==2)tmp=4;
if (receive_buf[0]==3)tmp=0;
for(i=0;i<tmp;i++)
{
EEPROMwrite(e2p_addr,receive_buf[i]);
e2p_addr++;
}
SEI();
}
void load_para(void)
{
uint e2p_addr=0;
uchar i;
for(i=0;i<20;i++)
{
eep_buf[i]=EEPROMread(e2p_addr);
e2p_addr++;
sel=eep_buf[1];
}
}
//串口傳送數(shù)據(jù)
void send(unsigned char tmp)
{
UDR=tmp;
while(!(UCSRA&BIT(UDRE)));
}
//TIMER2 initialisation - prescale:1024
// WGM: Normal
// desired value: 30mSec
// actual value: 29.952mSec (0.2%)
void timer2_init(void)
{
TCCR2 = 0x00; //stop，初始時(shí)停止，不進(jìn)行尋線PID調(diào)節(jié)
TCNT2 = 0x16; //setup
TCCR2 = 0x07;
}
//每120ms，電機(jī)速度上升一個(gè)等級(jí)，實(shí)現(xiàn)加速。加速曲線分兩段，緩加速段和急加速段
//緩加速段加速度為急加速段加速度的一半，加速的周期為7 X 120ms, 經(jīng)過7個(gè)速度等級(jí)，
//機(jī)器人的速度達(dá)到最大。
#pragma interrupt_handler timer2_ovf_isr:5
void timer2_ovf_isr(void)
{
static unsigned char t_counter = 0; //
static unsigned char T_Counter = 0; //加速周期計(jì)數(shù)量
static signed char now_fettle[2]={8,8};
static signed char CarWheelError[9] = {20, 40, 60, 180, 400, 420, 440,460,500}; //車輪誤差
signed char jiaodu; //
signed char weizhi; //
signed int temp_preU[2];
//static 左右輪行程誤差
static signed int MotorAcceleSpeed = 0; //機(jī)器人前進(jìn)加速度
TCNT2 = 0x16; //reload counter value
SEI();
t_counter++;
if (t_counter >= 4)
{
t_counter = 0;
//產(chǎn)生速度梯形圖。
if (TrapeziaV.SpeedState == ACCESTATE) //機(jī)器人處于加速狀態(tài)
{
if (T_Counter >= TrapeziaV.AcceCyc) //以最大速勻速前進(jìn)
{
T_Counter = TrapeziaV.AcceCyc;
TrapeziaV.SpeedState = REGUSTATE;
MotorAcceleSpeed = 0;
}
else
{
T_Counter ++;
MotorAcceleSpeed = TrapeziaV.AcceSpeed;
}
}
else if (TrapeziaV.SpeedState == REGUSTATE) //機(jī)器人處于勻速狀態(tài)
{
MotorAcceleSpeed = 0;
}
else if (TrapeziaV.SpeedState == DECESTATE) //機(jī)器人處于減速狀態(tài)
{
if ((T_Counter > 0) && (T_Counter <= 8)) //以最低速勻速前進(jìn)
{
T_Counter --;
MotorAcceleSpeed = TrapeziaV.AcceSpeed;
}
else
{
T_Counter = 0;
TrapeziaV.SpeedState = REGUSTATE;
MotorAcceleSpeed = 0;
}
}
TrapeziaV.MotorSpeed += MotorAcceleSpeed; //電機(jī)速度增加
if (TrapeziaV.MotorSpeed <= 300) //速度限定
TrapeziaV.MotorSpeed = 300;
else if (TrapeziaV.MotorSpeed >= 3000)
TrapeziaV.MotorSpeed = 3000;
//PID參數(shù)校準(zhǔn), 不同的速度等級(jí)對應(yīng)不同的參數(shù)
SeekPIDPara(T_Counter);
}
//讀傳感器狀態(tài)，尋線PID調(diào)節(jié)
read_sensor ();
now_fettle[0] = judge_sensor(Sensor_Ahead,now_fettle[0]); //前面的傳感器的情況值
now_fettle[1] = judge_sensor(Sensor_Back, now_fettle[1]); //后排的傳感器的情況值
//由前排8路傳感器和后排8路傳感器計(jì)算車身偏離白線的角度和位置, 進(jìn)行角度和位置的PID調(diào)節(jié)
jiaodu = now_fettle[0] - now_fettle[1]; //計(jì)算偏離角度
weizhi = now_fettle[1] - 8;
skPID.angle_FeedBack = value_change_Jiaodu ( jiaodu );
skPID.loca_FeedBack = value_change_Weizi(weizhi);
temp_preU[0] = angle_PIDCalc( &skPID ); //進(jìn)行角度PID調(diào)節(jié)
temp_preU[1] = loca_PIDCalc( &skPID ); //進(jìn)行位置PID調(diào)節(jié)
//調(diào)節(jié)量輸出，使得誤差減少。
set_speed ( TrapeziaV.MotorSpeed - temp_preU[0] - temp_preU[1], 1); //Right
set_speed ( TrapeziaV.MotorSpeed - CarWheelError[T_Counter]+ temp_preU[0] + temp_preU[1], 0); //Left
}
/*********************************************************************************/
//只用左邊傳感器數(shù)線，并向右轉(zhuǎn)90度。
//Lines : 預(yù)備走的線數(shù)
//motorinitspeed: 開始走時(shí)速度
//accespeed： 加速過程中的加速度
//decespeedline： 開始減速時(shí)離終點(diǎn)線的距離對應(yīng)的白線數(shù)。必須大于或等于2，才能穩(wěn)停
#define LSensorON !(PINB & BIT(1)) //左邊數(shù)線傳感器在白線狀態(tài)
unsigned char Count2TurnR(unsigned char Lines, signed int motorinitspeed, signed int accespeed, unsigned char decespeedline)
{
unsigned char flag = 1, CrossedLines = 0; //小車已經(jīng)通過的橫向白線數(shù)目
TCCR2 = 0x07; //尋線
TrapeziaV.MotorSpeed = motorinitspeed;
if (accespeed != 0) //加速度不為零時(shí)，梯形加速
{
if (Lines >= 3)
TrapeziaV.SpeedState = ACCESTATE; //梯形速度曲線設(shè)置
else
TrapeziaV.SpeedState = REGUSTATE; //如果走的線數(shù)太少時(shí)，不走梯形曲線
}
else
{
TrapeziaV.SpeedState = REGUSTATE; //勻速
}
TrapeziaV.AcceSpeed = accespeed;
while (CrossedLines < Lines)
{
if (LSensorON) //判斷在線
{
delay_100us(1);
if (LSensorON) //二次判斷在線,確認(rèn)信號(hào)
{
while (1) //在白線，等待出線。
{
if (!LSensorON)
{
delay_100us(1);
if (!LSensorON)
{
CrossedLines++;
if (Lines >= 3) //如果走的線數(shù)太少時(shí)，不走梯形曲線, 保持勻速過程
{
/************************************數(shù)線減速************************/
if (CrossedLines == (Lines - decespeedline))
{
TrapeziaV.SpeedState = DECESTATE;
TrapeziaV.AcceSpeed = -accespeed;
}
}
break;
}
}
}
}
}
}////////
TCCR2 = 0x00;
//set_distance(100, 1); //保證將數(shù)線傳感器移出白線
//set_distance(100, 0);
//delay_ms(200);
//右轉(zhuǎn)
set_distance(0, 1); //右輪不動(dòng)
set_speed(800, 0); //左輪旋轉(zhuǎn)，實(shí)現(xiàn)右轉(zhuǎn)
while (flag)
{
if (LSensorON)
{
delay_100us(1);
if (LSensorON)
{
while (1)
{
if (!LSensorON) //兩次判斷左邊數(shù)線從傳感器出線。
{
delay_100us(1);
if (!LSensorON)
{
while (PINA != 0x3F); //當(dāng)前排傳感器沒有達(dá)到特定的狀態(tài)時(shí)，持續(xù)轉(zhuǎn)動(dòng)
flag = 0;
break;
}
}
}
}
}
}
set_speed(0, 1);
set_speed(0, 0);
return 1;
}
///////////////////////////////////////////////////////
//用左邊傳感器數(shù)線，直走，到達(dá)終點(diǎn)，停車
unsigned char GoSraight_L(unsigned char Lines, signed int motorinitspeed, signed int accespeed, unsigned char decespeedline)
{
unsigned char flag = 1, CrossedLines = 0; //小車已經(jīng)通過的橫向白線數(shù)目
TCCR2 = 0x07; //尋線
TrapeziaV.MotorSpeed = motorinitspeed;
if (accespeed != 0) //加速度不為零時(shí)，梯形加速
{
if (Lines >= 3)
TrapeziaV.SpeedState = ACCESTATE; //梯形速度曲線設(shè)置
else
TrapeziaV.SpeedState = REGUSTATE; //如果走的線數(shù)太少時(shí)，不走梯形曲線
}
else
{
TrapeziaV.SpeedState = REGUSTATE; //勻速
}
TrapeziaV.AcceSpeed = accespeed;
while (CrossedLines < Lines)
{
if (LSensorON) //判斷在線
{
delay_100us(1);
if (LSensorON) //二次判斷在線,確認(rèn)信號(hào)
{
while (1) //在白線，等待出線。
{
if (!LSensorON)
{
delay_100us(1);
if (!LSensorON)
{
CrossedLines++;
if (Lines >= 3) //如果走的線數(shù)太少時(shí)，不走梯形曲線, 保持勻速過程
{
/************************************數(shù)線減速************************/
if (CrossedLines == (Lines - decespeedline))
{
TrapeziaV.SpeedState = DECESTATE;
TrapeziaV.AcceSpeed = -accespeed;
}
}
break;
}
}
}
}
}
}////////
TCCR2 = 0x00; //到達(dá)終點(diǎn)線，停車
set_speed(0, 0);
set_speed(0, 1);
return 1;
}
/*********************************************************************************/
//只用右邊傳感器數(shù)線，并向左轉(zhuǎn)90度。
#define RSensorON !(PINB & BIT(2)) //右邊數(shù)線傳感器在白線狀態(tài)
unsigned char Count2TurnL(unsigned char Lines, signed int motorinitspeed, signed int accespeed, unsigned char decespeedline)
{
unsigned char flag = 1, CrossedLines = 0; //小車已經(jīng)通過的橫向白線數(shù)目
TCCR2 = 0x07; //尋線
TrapeziaV.MotorSpeed = motorinitspeed;
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -