📄 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 + -