单片机四驱车遥控避障寻线演示程序(GP41QS) -

#include <stdio.h>
#include <intrins.h>
#include "STC12C5202AD.H"
#include "sio.h"
typedef struct PID
{
int p; // 比例常数 Proportional Const
int i; // 积分常数 Integral Const
int d; // 微分常数 Derivative Const
int position;
int hisPosition;
int lastPosition[3];
} PID;
static PID idata pid;
static int GAIN;
#define MIN9MS (XTAL/2L/256L*9L/1000L*9L/10L) //9ms脉宽*90%
#define MAX9MS (XTAL/2L/256L*9L/1000L*11L/10L) //9ms脉宽*110%
#define MIN45MS (XTAL/2L/256L*45L/10000L*9L/10L) //4.5ms脉宽
#define MAX45MS (XTAL/2L/256L*45L/10000L*11L/10L)
#define MIN225MS (XTAL/2L/256L*225L/100000L*9L/10L) //2.25ms脉宽
#define MAX225MS (XTAL/2L/256L*225L/100000L*11L/10L)
#define MIN056MS (XTAL/2L/256L*56L/100000L*6L/10L) //0.56ms脉宽*60%,下限
#define MAX056MS (XTAL/2L/256L*56L/100000L*14L/10L)
#define MIN168MS (XTAL/2L/256L*168L/100000L*6L/10L) //1.68ms脉宽*60%
#define MAX168MS (XTAL/2L/256L*168L/100000L*14L/10L)
sfr ISP_CUNTR = 0xE7;
sbit LED1 = P3^0;
sbit LED2 = P3^1;
sbit AD_LED1 = P2^0;
sbit AD_LED2 = P2^1;
sbit AD_LED3 = P2^7;
sbit IR_FRONT = P3^2;
sbit IR_LEFT = P3^3;
sbit IR_RIGHT = P2^6;
sbit IR_BACK = P3^7;
sbit IR_OUT = P3^5;
sbit MOTO_IN_B1 = P2^5;
sbit MOTO_IN_B2 = P2^4;
sbit MOTO_IN_A1 = P2^3;
sbit MOTO_IN_A2 = P2^2;
static unsigned char idata ad_datas[8]; //8路光电管采样电压
static unsigned char idata ad_datas_check[8]; //8路光电管采样电压校验值,轨道的白色背景的采样值
bit power_stat;
static unsigned char car_stat; //小车状态:0,停止;1,前进;2,后退;3,左转;4,右转;5,寻线模式;ff,自控避障模式
static unsigned int now;
static unsigned char code led_mod_table[3][20] = {
{1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
{1,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0},
{1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0}
};
unsigned char idata led_mod = 0;
static unsigned char idata led_tick = 0;
static unsigned char idata led_ptr = 0;
static bit test;
static unsigned char tick = 0;
static unsigned int pwm38k = 0;
static unsigned char check;
static unsigned char pwm_moto = 0;
static unsigned char pwm_moto_left = 0;
static unsigned char pwm_moto_right = 0;
static bit moto_left_forward = 1;
static bit moto_right_forward = 1;
#define IR_SINGAL_DELAY 11 //接收管输出延迟载波数量
#define TEST_PERIOD 1620 //评估周期,这个不同的接收管差别很大.
#define IR_SINGAL_PERIOD 76 //持续发射红外线载波数量
#define IR_VALID_THROLD 70 //判断是否前方有障碍的阀值
/*
#define IR_SINGAL_DELAY 1 //接收管输出延迟载波数量
#define TEST_PERIOD 200 //评估周期,这个不同的接收管差别很大.
#define IR_SINGAL_PERIOD 10 //持续发射红外线载波数量
#define IR_VALID_THROLD 8 //判断是否前方有障碍的阀值
*/
static unsigned char idata front_signal = 0;
static unsigned char idata back_signal = 0;
static unsigned char idata left_signal = 0;
static unsigned char idata right_signal = 0;
static bit front_obj = 0, back_obj = 0, left_obj = 0, right_obj = 0;
/*
* PCA中断计数,根据位置判断信号区域和定义,位置0表示初始,1代表引导码信号,2表示引导码间隔,
* 3表示第一个bit的信号,4表示第一个bit的间隔,以次类推...
* 更具体见对应的红外线协议.
*/
static unsigned int idata pca_tick;
static unsigned char idata pca_int_count;
static unsigned char data pca_int_total; /* 根据引导头确定总长度 */
static unsigned int idata period; /* 红外信号占或空周期计数 */
static unsigned char idata data_buf[6]; /* 红外线协议数据缓冲区 */
static unsigned int idata ccap1; //PCA0上一次的的计数
static unsigned char idata frame_dog; //红外帧看门狗,限定时间未接收完成清除工作
static void delay_ms(unsigned int v) {
unsigned int wait = pca_tick + v / 7 + 1;
while (wait != pca_tick) {
PCON |= 0x01;
}
}
static void delay_100us(unsigned char v) {
unsigned char wait = tick + v * 2; //每tick约50us
while (wait != tick) {
;//PCON |= 0x01;
}
}
void pid_init(PID *pid)
{
pid->position = 0;
pid->hisPosition = 0;
pid->lastPosition[0] = 0;
pid->lastPosition[1] = 0;
pid->lastPosition[2] = 0;
pid->p = 2;
pid->i = 0;
pid->d = 8;
}
void read_sensors(unsigned char sensors[]) { //读光电传感器的采样值
unsigned char i;
for (i = 0; i < 8; i++) { //LED熄灭状态测量一次电压
ADC_CONTR = 0xE0 | i; //设置AD通道, 快速AD采样.
delay_100us(5); //延迟等待电压稳定
ADC_RES = 0;
ADC_CONTR = 0xE8 | i;; //启动AD
while (!(ADC_CONTR & 0x10)) {
}
ADC_CONTR &= 0xE7;
sensors[i] = ADC_RES;
}
AD_LED1 = 0;
AD_LED2 = 0;
AD_LED3 = 0;
for (i = 0; i < 8; i++) { //LED打开再测量一次电压,用于消除环境光的差异.
ADC_CONTR = 0xE0 | i; //设置AD通道, 快速AD采样.
delay_100us(5); //延迟等待电压稳定
ADC_RES = 0;
ADC_CONTR = 0xE8 | i;; //启动AD
while (!(ADC_CONTR & 0x10)) {
}
ADC_CONTR &= 0xE7;
if (ADC_RES > sensors[i])
sensors[i] = ADC_RES - sensors[i];
else
sensors[i] = ADC_RES;
}
AD_LED1 = 1;
AD_LED2 = 1;
AD_LED3 = 1;
}
static void read_sensors_check() { //从flash中读取光电传感器的校验值,结构是8byte的值+1字节的crc
unsigned char i, crc = 0;
IAP_ADDRH = 0;
IAP_CONTR = 0x80; //允许ISP/IAP操作
IAP_CMD = 0x01; //读flash
for (i = 0; i < 8; i++) {
IAP_ADDRL = i;
IAP_TRIG = 0x5A;
IAP_TRIG = 0xA5; //启动IAP操作
_nop_();
ad_datas_check[i] = IAP_DATA;
crc += IAP_DATA;
//com_putchar(IAP_DATA);
}
IAP_ADDRL = 8;
IAP_TRIG = 0x5A;
IAP_TRIG = 0xA5; //启动IAP操作
_nop_();
if (crc != IAP_DATA) { //crc验证错误,flash数据失效
for (i = 0; i < 8; i++) {
ad_datas_check[i] = 0xff;
}
}
IAP_CONTR = 0; //禁止IAP,防止误操作
IAP_CMD = 0;
IAP_TRIG = 0;
IAP_ADDRH = 0xff;
IAP_ADDRL = 0xff;
}
static void write_sensors_check() { //将光电传感器的校验值写入flash,结构是8byte的值+1字节的crc
unsigned char i, crc = 0;
IAP_ADDRH = 0;
IAP_CONTR = 0x80; //允许ISP/IAP操作
read_sensors(ad_datas_check);
IAP_ADDRL = 0;
IAP_CMD = 0x03; //擦除flash
IAP_TRIG = 0x5A;
IAP_TRIG = 0xA5; //启动IAP操作
_nop_();
IAP_CMD = 0x10; //写flash
for (i = 0; i < 8; i++) {
IAP_ADDRL = i;
IAP_DATA = ad_datas_check[i];
//com_putchar(IAP_DATA);
crc += ad_datas_check[i];
IAP_CMD = 0x02; //写flash
IAP_TRIG = 0x5A;
IAP_TRIG = 0xA5; //启动IAP操作
_nop_();
}
IAP_ADDRL = 8;
IAP_DATA = crc;
IAP_TRIG = 0x5A;
IAP_TRIG = 0xA5; //启动IAP操作
_nop_();
IAP_CONTR = 0; //禁止IAP,防止误操作
IAP_CMD = 0;
IAP_TRIG = 0;
IAP_ADDRH = 0xff;
IAP_ADDRL = 0xff;
}
void time0_isr() interrupt 1
{
tick++;
pwm38k++;
if (pwm38k == 0)
{
front_signal = 0;
back_signal = 0;
left_signal = 0;
right_signal = 0;
} else if (pwm38k == TEST_PERIOD)
{
//com_putchar(check);
if (front_signal >= IR_VALID_THROLD)
{
front_obj = 1;
} else
front_obj = 0;
if (back_signal >= IR_VALID_THROLD)
{
back_obj = 1;
} else
back_obj = 0;
if (left_signal >= IR_VALID_THROLD)
{
left_obj = 1;
} else
left_obj = 0;
if (right_signal >= IR_VALID_THROLD)
{
right_obj = 1;
} else
right_obj = 0;
pwm38k = 0xFFFF;
return;
}
if (pwm38k == 0)
{
IR_OUT = 1;
test = 0;
} else if (pwm38k == IR_SINGAL_PERIOD)
{
IR_OUT = 0;
}
// if (!test && !IR_FRONT) //调试接收管的延迟
// {
// test = 1;
// com_putchar(tick);
// }
if (pwm38k >= IR_SINGAL_DELAY && pwm38k < IR_SINGAL_DELAY + IR_SINGAL_PERIOD)
{
if (!IR_FRONT)
front_signal++;
else
{
if (front_signal)
front_signal--;
}
if (!IR_BACK)
back_signal++;
else
{
if (back_signal)
back_signal--;
}
if (!IR_LEFT)
left_signal++;
else
{
if (left_signal)
left_signal--;
}
if (!IR_RIGHT)
right_signal++;
else
{
if (right_signal)
right_signal--;
}
}
if (pwm_moto == 0)
{
if (pwm_moto_left > 0)
{
if (moto_left_forward)
MOTO_IN_A2 = 1;
else
MOTO_IN_A1 = 1;
} else
{
MOTO_IN_A1 = 0;
MOTO_IN_A2 = 0;
}
if (pwm_moto_right > 0)
{
if (moto_right_forward)
MOTO_IN_B2 = 1;
else
MOTO_IN_B1 = 1;
} else
{
MOTO_IN_B1 = 0;
MOTO_IN_B2 = 0;
}
} else
{
if (pwm_moto == pwm_moto_left)
{
MOTO_IN_A1 = 0;
MOTO_IN_A2 = 0;
}
if (pwm_moto == pwm_moto_right)
{
MOTO_IN_B1 = 0;
MOTO_IN_B2 = 0;
}
}
pwm_moto++;
}
void time0_initialize(void)
{
TMOD &= ~0x0F; /* clear timer 0 mode bits */
TMOD |= 0x02; /* put timer 0 into MODE 2 */
AUXR |= 0x80; // timer0工作在1T模式
TH0 = 256 - XTAL / 2L / 38400L; // 256 - XTAL/T1_12/f, f=输出时钟频率
TL0 = 0x0;
WAKE_CLKO = 0x01; // T0在P3.4上输出时钟.
PT0 = 1; /* 时钟0中断高优先级 */
TR0 = 1; //
ET0 = 1;
}
static void wakeup (void) interrupt 2
{
}
static void pca_isr (void) interrupt 6
{
unsigned char i, j;
if (CCF0) {
CCF0 = 0; //清PCA1中断标志
LED1 = IR_BACK;
if (!pca_int_count) { //第一次收到信号
if (!IR_BACK) {
ccap1 = pca_tick * 256 + CCAP0H;
pca_int_count++;
}
} else { //已经收到一些信号
period = pca_tick * 256 + CCAP0H - ccap1;
ccap1 = pca_tick * 256 + CCAP0H;
//com_putchar(period / 256);
//com_putchar(period % 256);
if (pca_int_count == 1) {
if (period < MIN9MS || period > MAX9MS) { //9ms
pca_int_count = 0;
frame_dog = 0;
} else
pca_int_count++;
} else if (pca_int_count == 2) {
if (period > MIN225MS && period < MAX225MS) { //2.25ms
pca_int_total = 3;
pca_int_count++;
} else if (period > MIN45MS && period < MAX45MS) { //4.5ms
pca_int_total = 67;
pca_int_count++;
} else {
pca_int_count = 0;
frame_dog = 0;
}
} else {
if (IR_BACK) {
if (period > MIN056MS && period < MAX056MS) { //0.56ms
if (pca_int_count >= pca_int_total) { //帧接收完毕,下面进行有效性分析.
if (pca_int_total == 67) { //完整信号,含有引导信号,设备码8bit,设备反码8bit,命令字8bit,命令字反码8bit
if ((data_buf[0] ^ data_buf[1] == 0xff) && (data_buf[2] ^ data_buf[3] == 0xff)) {
com_putchar(data_buf[0]);
com_putchar(data_buf[2]);
if (data_buf[0] == 0x40) {
switch (data_buf[2]) {
case 0x5F: //左
car_stat = 3;
break;
case 0x5B: //右
car_stat = 4;
break;
case 0x5A: //上
car_stat = 1;
break;
case 0x5E: //下
car_stat = 2;
break;
case 0x56: //菜单
car_stat = 0;
break;
case 0x0: //数字0
car_stat = 0xff;
now = pca_tick;
break;
case 0x1: //数字1
car_stat = 5;
pid_init(&pid);
GAIN = 150;
now = pca_tick;
break;
case 0x3: //数字3
write_sensors_check(); //校验光电传感器
break;
case 0x1A: //+号
pid.p++;
break;
case 0x1E: //-号
if (pid.p)
pid.p--;
break;
case 0x10: //静音键
GAIN++;
break;
case 0x1C: //屏显
if (GAIN)
GAIN--;
break;
case 0x1B: //^
pid.d++;
break;
case 0x1F: //向下
if (pid.d)
pid.d--;
break;
case 0x12: //POWER
// power_stat = ~power_stat;
break;
default:
break;
}
}
}
} else { //重复信号,仅含有引导信号
}
pca_int_count = 0;
frame_dog = 0;
} else {
pca_int_count++;
}
} else {
pca_int_count = 0;
frame_dog = 0;
}
} else {
j = (pca_int_count - 3) / 2;
i = j / 8;
j = j % 8;
if (period > MIN168MS && period < MAX168MS) { //1.68ms
// com_putchar(0x01);
data_buf[i] |= (0x01 << j);
pca_int_count++;
} else if (period > MIN056MS && period < MAX056MS) { //0.56ms
// com_putchar(0x00);
data_buf[i] &= ~(0x01 << j);
pca_int_count++;
} else {
pca_int_count = 0;
frame_dog = 0;
}
}
}
}
}
if (CF) { //PCA计数溢出中断,19.6608MHZ晶体大约6.7ms溢出
CF = 0;
pca_tick++;
if (led_tick++ >= 10) {
led_tick = 0;
if (led_mod_table[led_mod][led_ptr++]) {
LED1 = 0;
LED2 = 0;
} else {
if (left_obj || right_obj || front_obj || back_obj) {
LED1 = 0;
LED2 = 0;
} else {
LED1 = 1;
LED2 = 1;
}
}
led_ptr %= 20;
}
if (pca_int_count) {
frame_dog++;
if (frame_dog >= 15) { //100ms后重新开始分析新的红外线数据包
pca_int_count = 0;
frame_dog = 0;
}
}
}
}
int read_black_line() { //读黑线位置.
unsigned char i;
int grayscale_min; //最黑的线
char line_begin, line_end; //黑线对应的传感器的开始和结束位置.
int tmp;
read_sensors(ad_datas);
/*
line_begin = -1; line_end= -1;
for (i = 0; i < 8; i++) {
//com_putchar(ad_datas[i]);
//com_putchar(ad_datas_check[i]);
tmp = (ad_datas[i] * 100);
tmp = tmp / ad_datas_check[i];
ad_datas[i] = tmp;
if (ad_datas[i] <= 40) //灰度<=40%认为下面有部分黑线
{
if (line_begin == -1)
line_begin = i;
else
line_end = i;
}
//com_putchar(ad_datas[i]);
}
if (line_begin == -1)
return -1;
if (line_end == -1)
return line_begin * 10;
return 10*line_begin + (line_end - line_begin) * 10 * ad_datas[line_begin] / (ad_datas[line_begin] + ad_datas[line_end]);
*/
line_begin = 0;
// ad_datas[0] = ad_datas[0] * 100 / ad_datas_check[0];
grayscale_min = 500;
for (i = 1; i < 8; i++) {
//com_putchar(ad_datas[i]);
//com_putchar(ad_datas_check[i]);
tmp = (ad_datas[i] * 100);
tmp = tmp / ad_datas_check[i];
ad_datas[i] = tmp;
if (ad_datas[i] <= grayscale_min)
{
grayscale_min = ad_datas[i];
line_begin = i;
}
//com_putchar(ad_datas[i]);
}
if (grayscale_min < 50) //灰度<50%认为是有效黑线
{
if (line_begin == 0)
line_end = 1;
else if (line_begin == 7)
{
line_begin = 6;
line_end = 7;
} else if (ad_datas[line_begin - 1] < ad_datas[line_begin + 1])
{
line_end = line_begin;
line_begin = line_begin - 1;
} else
line_end = line_begin + 1;
if (ad_datas[line_begin] == 0)
ad_datas[line_begin] = 1;
if (ad_datas[line_end] == 0)
ad_datas[line_end] = 1;
return 10*line_begin + (line_end - line_begin) * 10 * ad_datas[line_begin] / (ad_datas[line_begin] + ad_datas[line_end]);
} else
return -1;
}
void track_line(PID *pid) { //未使用PID算法的版本,四驱车用PID算法效果差些,需要改进,三轮车用PID效果还可以.
int line_pos;
line_pos = read_black_line();
if (line_pos == -1)
{
return;
}
com_putchar(line_pos);
if (line_pos > 45)
{
pwm_moto_left = 255;
pwm_moto_right = 255;
moto_left_forward = 0;
moto_right_forward = 1;
} else if (line_pos < 25) {
pwm_moto_left = 255;
pwm_moto_right = 255;
moto_left_forward = 1;
moto_right_forward = 0;
} else
{
pwm_moto_left = GAIN;
pwm_moto_right = GAIN;
moto_left_forward = 1;
moto_right_forward = 1;
}
}
/*
void track_line(PID *pid) { //使用PID算法的版本,三轮小车效果比较不错.
int p; //P增益
int i; //I增益
int d; //D增益
int gain; //控制量
pid->position = read_black_line();
if (pid->position == -1)
{
// pwm_moto_left = 0;
// pwm_moto_right = 0;
// pid_init(pid);
return;
}
com_putchar(pid->position);
pid->position -= 30;
pid->hisPosition += pid->position; //记录偏差之和
pid->lastPosition[2] = pid->lastPosition[1]; //
pid->lastPosition[1] = pid->lastPosition[0]; //
pid->lastPosition[0] = pid->position; //
p = (pid->p) * (pid->position); //计算比例分量(P)=比例系数*本次位置差
i = (pid->i) * (pid->hisPosition); //计算积分分量(I)=积分系数*偏差之和
d = (pid->d) * ((pid->position) - (pid->lastPosition[2])); //计算微分分量(D)=微分系数*(本次位置差-前3次的位置差)
// 由于采样比较快，用本次位置-前3次位置才有足够大的控制
gain = p + i + d; //P分量和D分量相加，得到控制量
// if (gain > GAIN)
// gain = GAIN;
// if (gain < -GAIN)
// gain = -GAIN;
if (gain > 30)
{
pwm_moto_left = GAIN + gain;
pwm_moto_right = GAIN + gain;
moto_left_forward = 0;
moto_right_forward = 1;
} else if (gain < -30) {
pwm_moto_left = GAIN - gain;
pwm_moto_right = GAIN - gain;
moto_left_forward = 1;
moto_right_forward = 0;
} else
{
pwm_moto_left = GAIN - gain;
pwm_moto_right = GAIN -gain;
moto_left_forward = 1;
moto_right_forward = 1;
}
}
*/
void main (void)
{
unsigned int i;
P2M0 = 0x3C; //P2.2~P2.5 强推挽输出
P3M0 = 0x30; //P3.4,P3.5强输出.
P1ASF = 0xff; //P1.7~P1.0用做AD
i = 0;
MOTO_IN_A1 = 0;
MOTO_IN_A2 = 0;
MOTO_IN_B1 = 0;
MOTO_IN_B2 = 0;
EA = 0;
power_stat = 0;
time0_initialize();
// com_initialize (); /* initialize interrupt driven serial I/O */
// com_baudrate (14400); /* setup for 14400 baud */
/*
CMOD = 0x01; // #00000001B,PCA空闲计数,PCA计数源=Fosc/12,PCA溢出中断(做一个定时器使用)
CCON = 0x00; //PCA中断标志清0,PCA停止计数
CL = 0x0;
CH = 0x0;
CCAPM1 = 0x31; //PCA1上升下降沿捕获
*/
CMOD = 0x03; // #00000011B,PCA空闲计数,PCA计数源=fosc/2,PCA溢出中断
CCON = 0x00; //PCA中断标志清0,PCA停止计数
CL = 0x0;
CH = 0x0;
CCAPM0 = 0x31; //PCA0上升下降沿捕获
/*
CCAPM0 = 0x42; //PCA0工作模式:8位pwm
PCA_PWM0 = 0x00;
CCAP0L = 0x40; //25%占空比,可调节占空比来调节发射功率.低电平驱动.
CCAP0H = 0x40;
*/
// EPCA_LVD = 1; //允许PCA和低压检测中断
ELVD = 1;
car_stat = 0x00;
pca_tick = 0;
pca_int_count = 0;
frame_dog = 0;
EA = 1; /* Enable Interrupts */
CR = 1; //启动PCA计数
now = pca_tick;
delay_ms(200);
read_sensors_check();
while (1)
{
if (power_stat) {
// auto_power_down(); //自动关机
}
switch (car_stat) {
case 0:
/*
MOTO_IN_A1 = 0;
MOTO_IN_A2 = 0;
MOTO_IN_B1 = 0;
MOTO_IN_B2 = 0;
*/
pwm_moto_left = 0;
pwm_moto_right = 0;
break;
case 1:
if (!front_obj) {
/*
MOTO_IN_A1 = 0;
MOTO_IN_A2 = 1;
MOTO_IN_B1 = 0;
MOTO_IN_B2 = 1;
*/
moto_left_forward = 1;
moto_right_forward = 1;
pwm_moto_left = 255;
pwm_moto_right = 255;
} else {
/*
MOTO_IN_A1 = 1;
MOTO_IN_A2 = 0;
MOTO_IN_B1 = 1;
MOTO_IN_B2 = 0;
delay(1000);
*/
car_stat = 0;
}
break;
case 2:
if (!back_obj) {
moto_left_forward = 0;
moto_right_forward = 0;
pwm_moto_left = 255;
pwm_moto_right = 255;
/*
MOTO_IN_A1 = 1;
MOTO_IN_A2 = 0;
MOTO_IN_B1 = 1;
MOTO_IN_B2 = 0;
*/
} else {
/*
MOTO_IN_A1 = 0;
MOTO_IN_A2 = 1;
MOTO_IN_B1 = 0;
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码