?? autocar4_4.1.c
字號:
#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];
}
}
//串口傳送數據
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，初始時停止，不進行尋線PID調節
TCNT2 = 0x16; //setup
TCCR2 = 0x07;
}
//每120ms，電機速度上升一個等級，實現加速。加速曲線分兩段，緩加速段和急加速段
//緩加速段加速度為急加速段加速度的一半，加速的周期為7 X 120ms, 經過7個速度等級，
//機器人的速度達到最大。
#pragma interrupt_handler timer2_ovf_isr:5
void timer2_ovf_isr(void)
{
static unsigned char t_counter = 0; //
static unsigned char T_Counter = 0; //加速周期計數量
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; //機器人前進加速度
TCNT2 = 0x16; //reload counter value
SEI();
t_counter++;
if (t_counter >= 4)
{
t_counter = 0;
//產生速度梯形圖。
if (TrapeziaV.SpeedState == ACCESTATE) //機器人處于加速狀態
{
if (T_Counter >= TrapeziaV.AcceCyc) //以最大速勻速前進
{
T_Counter = TrapeziaV.AcceCyc;
TrapeziaV.SpeedState = REGUSTATE;
MotorAcceleSpeed = 0;
}
else
{
T_Counter ++;
MotorAcceleSpeed = TrapeziaV.AcceSpeed;
}
}
else if (TrapeziaV.SpeedState == REGUSTATE) //機器人處于勻速狀態
{
MotorAcceleSpeed = 0;
}
else if (TrapeziaV.SpeedState == DECESTATE) //機器人處于減速狀態
{
if ((T_Counter > 0) && (T_Counter <= 8)) //以最低速勻速前進
{
T_Counter --;
MotorAcceleSpeed = TrapeziaV.AcceSpeed;
}
else
{
T_Counter = 0;
TrapeziaV.SpeedState = REGUSTATE;
MotorAcceleSpeed = 0;
}
}
TrapeziaV.MotorSpeed += MotorAcceleSpeed; //電機速度增加
if (TrapeziaV.MotorSpeed <= 300) //速度限定
TrapeziaV.MotorSpeed = 300;
else if (TrapeziaV.MotorSpeed >= 3000)
TrapeziaV.MotorSpeed = 3000;
//PID參數校準, 不同的速度等級對應不同的參數
SeekPIDPara(T_Counter);
}
//讀傳感器狀態，尋線PID調節
read_sensor ();
now_fettle[0] = judge_sensor(Sensor_Ahead,now_fettle[0]); //前面的傳感器的情況值
now_fettle[1] = judge_sensor(Sensor_Back, now_fettle[1]); //后排的傳感器的情況值
//由前排8路傳感器和后排8路傳感器計算車身偏離白線的角度和位置, 進行角度和位置的PID調節
jiaodu = now_fettle[0] - now_fettle[1]; //計算偏離角度
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 ); //進行角度PID調節
temp_preU[1] = loca_PIDCalc( &skPID ); //進行位置PID調節
//調節量輸出，使得誤差減少。
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
}
/*********************************************************************************/
//只用左邊傳感器數線，并向右轉90度。
//Lines : 預備走的線數
//motorinitspeed: 開始走時速度
//accespeed： 加速過程中的加速度
//decespeedline： 開始減速時離終點線的距離對應的白線數。必須大于或等于2，才能穩停
#define LSensorON !(PINB & BIT(1)) //左邊數線傳感器在白線狀態
unsigned char Count2TurnR(unsigned char Lines, signed int motorinitspeed, signed int accespeed, unsigned char decespeedline)
{
unsigned char flag = 1, CrossedLines = 0; //小車已經通過的橫向白線數目
TCCR2 = 0x07; //尋線
TrapeziaV.MotorSpeed = motorinitspeed;
if (accespeed != 0) //加速度不為零時，梯形加速
{
if (Lines >= 3)
TrapeziaV.SpeedState = ACCESTATE; //梯形速度曲線設置
else
TrapeziaV.SpeedState = REGUSTATE; //如果走的線數太少時，不走梯形曲線
}
else
{
TrapeziaV.SpeedState = REGUSTATE; //勻速
}
TrapeziaV.AcceSpeed = accespeed;
while (CrossedLines < Lines)
{
if (LSensorON) //判斷在線
{
delay_100us(1);
if (LSensorON) //二次判斷在線,確認信號
{
while (1) //在白線，等待出線。
{
if (!LSensorON)
{
delay_100us(1);
if (!LSensorON)
{
CrossedLines++;
if (Lines >= 3) //如果走的線數太少時，不走梯形曲線, 保持勻速過程
{
/************************************數線減速************************/
if (CrossedLines == (Lines - decespeedline))
{
TrapeziaV.SpeedState = DECESTATE;
TrapeziaV.AcceSpeed = -accespeed;
}
}
break;
}
}
}
}
}
}////////
TCCR2 = 0x00;
//set_distance(100, 1); //保證將數線傳感器移出白線
//set_distance(100, 0);
//delay_ms(200);
//右轉
set_distance(0, 1); //右輪不動
set_speed(800, 0); //左輪旋轉，實現右轉
while (flag)
{
if (LSensorON)
{
delay_100us(1);
if (LSensorON)
{
while (1)
{
if (!LSensorON) //兩次判斷左邊數線從傳感器出線。
{
delay_100us(1);
if (!LSensorON)
{
while (PINA != 0x3F); //當前排傳感器沒有達到特定的狀態時，持續轉動
flag = 0;
break;
}
}
}
}
}
}
set_speed(0, 1);
set_speed(0, 0);
return 1;
}
///////////////////////////////////////////////////////
//用左邊傳感器數線，直走，到達終點，停車
unsigned char GoSraight_L(unsigned char Lines, signed int motorinitspeed, signed int accespeed, unsigned char decespeedline)
{
unsigned char flag = 1, CrossedLines = 0; //小車已經通過的橫向白線數目
TCCR2 = 0x07; //尋線
TrapeziaV.MotorSpeed = motorinitspeed;
if (accespeed != 0) //加速度不為零時，梯形加速
{
if (Lines >= 3)
TrapeziaV.SpeedState = ACCESTATE; //梯形速度曲線設置
else
TrapeziaV.SpeedState = REGUSTATE; //如果走的線數太少時，不走梯形曲線
}
else
{
TrapeziaV.SpeedState = REGUSTATE; //勻速
}
TrapeziaV.AcceSpeed = accespeed;
while (CrossedLines < Lines)
{
if (LSensorON) //判斷在線
{
delay_100us(1);
if (LSensorON) //二次判斷在線,確認信號
{
while (1) //在白線，等待出線。
{
if (!LSensorON)
{
delay_100us(1);
if (!LSensorON)
{
CrossedLines++;
if (Lines >= 3) //如果走的線數太少時，不走梯形曲線, 保持勻速過程
{
/************************************數線減速************************/
if (CrossedLines == (Lines - decespeedline))
{
TrapeziaV.SpeedState = DECESTATE;
TrapeziaV.AcceSpeed = -accespeed;
}
}
break;
}
}
}
}
}
}////////
TCCR2 = 0x00; //到達終點線，停車
set_speed(0, 0);
set_speed(0, 1);
return 1;
}
/*********************************************************************************/
//只用右邊傳感器數線，并向左轉90度。
#define RSensorON !(PINB & BIT(2)) //右邊數線傳感器在白線狀態
unsigned char Count2TurnL(unsigned char Lines, signed int motorinitspeed, signed int accespeed, unsigned char decespeedline)
{
unsigned char flag = 1, CrossedLines = 0; //小車已經通過的橫向白線數目
TCCR2 = 0x07; //尋線
TrapeziaV.MotorSpeed = motorinitspeed;
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -