main.c

遥控避障寻线开发板

#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] == 0x80) {
                                        switch (data_buf[2]) {
                                        case 0x07: //左
                                            car_stat = 3;
                                            break;
                                        case 0x09: //右
                                            car_stat = 4;
                                            break;
                                        case 0x05: //上
                                            car_stat = 1;
                                            break;
                                        case 0x1B: //下
                                            car_stat = 2;
                                            break;
                                        case 0x08: //确认
                                            car_stat = 0;
                                            break;
                                        case 0x01:  //图片
                                            car_stat = 0xff;
                                            now = pca_tick;
                                            break;
                                        case 0x02:  //音乐
                                            car_stat = 5;
                                            pid_init(&pid);
                                            GAIN = 75;
                                            now = pca_tick;
                                            break;
                                        case 0x03:  //电影
                                            write_sensors_check();  //校验光电传感器
                                            break;
                                        case 0x0C:  //音量+
                                            if (car_stat == 0x05)
                                                pid.p++;