ccorecontrol.c

第二届飞思卡尔杯智能车大赛

/*
 * Copyright (c) 1998, Xiongfei Guo, Shanghai Second Polytechnic University 
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Shanghai Second Polytechnic University nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

// 黑线识别

#include "includes.h"

// 有关速度的参数
INT16S lastTgtSpeed = 100;
INT16S tgtSpeed = 100;
INT16U maxSpeed = 110;
INT16U minSpeed = 80;

// 限制距离为 多少米
INT16U DistLimit = 0;

// 绝对和相对的红外检测值
INT16U wir[nIR];
INT16U irValue[nIR];

// 黑白的参考值
INT16U whiteAvg[nIR], blackAvg[nIR];

// 中间变量
INT16U minTmp[nIR][10], maxTmp[nIR][10];

// 每次检测到的IR位置
INT16U position;

// 舵机目标位置
INT8U servoTgtAngle;

INT16U tmir;

// 十字和交叉的记录
INT8U crsN,crsType[10];
INT16U crsDist[10];

INT16U StartLineDist[4];
INT8U CrossLineN = 0, RampDistN = 0;
INT16U CrossLineDist[20];
INT16U RampDist[10];
// 当前所在的圈
INT8U nowLoop;

// 用于计算单位角度舵机拐角的平均值
INT32U PerDistSrvTotal;
INT16U PerDistSrvN;

#define swap(x, y) {(x) = (x) ^ (y);(y) = (x) ^ (y);(x) = (x) ^ (y);}


// IR 发射序列 分别向PTH, PORTE & PORTA 赋值
const INT8U irSendArr[nIR][3] = {
#if nIR == 12
                                    {0, 0, PORTA_BIT6_MASK},
                                    {0, 0, PORTA_BIT7_MASK},
                                    {PTH_PTH3_MASK, 0, 0},
                                    {PTH_PTH2_MASK, 0, 0},
                                    {PTH_PTH1_MASK, 0, 0},
                                    {PTH_PTH0_MASK, 0, 0},
                                    {0, PORTE_BIT3_MASK, 0},
                                    {0, PORTE_BIT2_MASK, 0},
                                    {0, 0, PORTA_BIT2_MASK},
                                    {0, 0, PORTA_BIT3_MASK},
                                    {0, 0, PORTA_BIT5_MASK},
                                    {0, 0, PORTA_BIT4_MASK}
#elif nIR == 8
                                    {PTH_PTH3_MASK, 0, 0},
                                    {PTH_PTH2_MASK, 0, 0},
                                    {PTH_PTH1_MASK, 0, 0},
                                    {PTH_PTH0_MASK, 0, 0},
                                    {0, PORTE_BIT3_MASK, 0},
                                    {0, PORTE_BIT2_MASK, 0},
                                    {0, 0, PORTA_BIT2_MASK},
                                    {0, 0, PORTA_BIT3_MASK},

#endif
                                };

// IR 接收管排列方式
const INT8U irRecvArr[nIR] = {
#if nIR == 12
                                 11,//0
                                 10,
                                 12,
                                 4,
                                 15,
                                 7,
                                 14,//6
                                 6,
                                 13,
                                 5,
                                 3,
                                 2
#elif nIR == 8
                                 12,
                                 14,//64,
                                 15,
                                 13,
                                 4,
                                 6,
                                 7,
                                 5
#endif
                             };

const INT8U irRevPair[nIR / 2][4] = {
#if nIR == 12
                                        {5, 0, 7, 11},
                                        {10, 1, 3, 10},
                                        {7 ,2, 6, 12},
                                        {3, 8, 4, 13},
                                        {9, 4, 5, 15},
                                        {11, 6, 2, 14}
#elif nIR == 8
                                        {7,3,5,13},
                                        {4,0,4,12},
                                        {5,1,6,14},
                                        {6,2,7,15}

#endif
                                    };


const INT8U ir_position[nIR] = {
#if nIR == 8
                                   38,
                                   63,
                                   88,
                                   113,
                                   138,
                                   163,
                                   188,
                                   213
#elif nIR == 12
                                   15,
                                   35,
                                   55,
                                   75,
                                   95,
                                   115,
                                   135,
                                   155,
                                   175,
                                   195,
                                   215,
                                   235
#endif
                               };

const INT8U speed_arr[251] = {
                                 0, 0, 0, 0, 0, 0, 0, 0,
                                 0, 0, 0, 0, 1, 1, 1, 1,
                                 1, 2, 2, 2, 2, 2, 3, 3,
                                 3, 4, 4, 4, 5, 5, 5, 6,
                                 6, 6, 7, 7, 8, 8, 8, 9,
                                 9, 10, 10, 11, 11, 11, 12, 12,
                                 13, 13, 14, 14, 15, 15, 16, 16,
                                 17, 17, 18, 18, 19, 19, 20, 20,
                                 21, 21, 22, 22, 23, 23, 24, 24,
                                 25, 25, 26, 26, 27, 27, 28, 28,
                                 28, 29, 29, 30, 30, 31, 31, 31,
                                 32, 32, 33, 33, 33, 34, 34, 34,
                                 35, 35, 35, 36, 36, 36, 37, 37,
                                 37, 37, 37, 38, 38, 38, 38, 38,
                                 39, 39, 39, 39, 39, 39, 39, 39,
                                 39, 39, 39, 39, 39, 39, 39, 39,
                                 39, 39, 39, 39, 39, 39, 39, 39,
                                 39, 38, 38, 38, 38, 38, 37, 37,
                                 37, 37, 37, 36, 36, 36, 35, 35,
                                 35, 34, 34, 34, 33, 33, 33, 32,
                                 32, 31, 31, 31, 30, 30, 29, 29,
                                 28, 28, 28, 27, 27, 26, 26, 25,
                                 25, 24, 24, 23, 23, 22, 22, 21,
                                 21, 20, 20, 19, 19, 18, 18, 17,
                                 17, 16, 16, 15, 15, 14, 14, 13,
                                 13, 12, 12, 11, 11, 11, 10, 10,
                                 9, 9, 8, 8, 8, 7, 7, 6,
                                 6, 6, 5, 5, 5, 4, 4, 4,
                                 3, 3, 3, 2, 2, 2, 2, 2,
                                 1, 1, 1, 1, 1, 0, 0, 0,
                                 0, 0, 0, 0, 0, 0, 0, 0,
                                 0, 0, 0
                             };


const INT8U servo_turn_arr[251] = {
                                      51, 51, 51, 51, 51, 51, 51, 51,
                                      51, 51, 51, 51, 51, 51, 51, 51,
                                      51, 51, 51, 51, 51, 51, 51, 51,
                                      51, 52, 52, 52, 52, 52, 52, 52,
                                      52, 52, 52, 52, 53, 53, 53, 53,
                                      53, 53, 53, 53, 54, 54, 54, 54,
                                      54, 54, 54, 55, 55, 55, 55, 55,
                                      56, 56, 56, 56, 56, 57, 57, 57,
                                      57, 58, 58, 58, 58, 59, 59, 59,
                                      60, 60, 60, 61, 61, 61, 62, 62,
                                      63, 63, 63, 64, 64, 65, 65, 66,
                                      66, 67, 67, 68, 68, 69, 69, 70,
                                      71, 71, 72, 72, 73, 74, 74, 75,
                                      76, 77, 77, 78, 79, 80, 80, 81,
                                      82, 83, 84, 84, 85, 86, 87, 87,
                                      88, 89, 89, 89, 89, 89, 90, 90,
                                      91, 92, 92, 93, 94, 95, 96, 97,
                                      98, 99, 100, 101, 101, 102, 103, 104,
                                      105, 106, 106, 107, 108, 109, 109, 110,
                                      111, 111, 112, 113, 113, 114, 114, 115,
                                      115, 116, 116, 117, 117, 118, 118, 119,
                                      119, 119, 120, 120, 121, 121, 121, 122,
                                      122, 122, 122, 123, 123, 123, 124, 124,
                                      124, 124, 125, 125, 125, 125, 125, 126,
                                      126, 126, 126, 126, 127, 127, 127, 127,
                                      127, 127, 127, 127, 128, 128, 128, 128,
                                      128, 128, 128, 128, 128, 128, 129, 129,
                                      129, 129, 129, 129, 129, 129, 129, 129,
                                      129, 129, 129, 129, 129, 129, 129, 129,
                                      129, 129, 129, 129, 129, 129, 129, 129,
                                      129, 129, 129, 129, 129, 129, 129, 129,
                                      129, 129, 129
                                  };

// 识别黑线所需的一些变量
INT16U tBlState;
INT16U tv;
INT16U ttotal,total;
INT16U minIRv, minIRn;
INT16U minIRv2, minIRn2; /* 第二小 */
INT16U maxIRv;
INT16U last_position = 90;

INT32U tdist;

// 黑线的状态
#define NORMAL 0        /* 普通黑线 */
#define LOST 1          /* 找不到 */
#define START 2         /* 起始线 */
#define CROSS 3         /* 十字交叉 */

INT16U blStateArr[4];   /* 状态数组 */
INT16U blState;         /* 状态分析结果 */
INT16U tw, tb;
void GetBlackAndWhite(void) {
    INT16U i, j;


    for (i = 0;i < nIR;i++) {
        whiteAvg[i] = 0;
        blackAvg[i] = 0;
        for (j = 0;j < 10;j++) {
            maxTmp[i][j] = 0;
            minTmp[i][j] = 1023;
        }
    }

    for (;;) {


        PORTB = 0x55;


        if (!PTIP_PTIP0) {
            // 处理
            for (i = 0;i < nIR;i++) {
                tw = 0;
                tb = 0;
                for (j = 0;j < 10;j++) {
                    if (maxTmp[i][j] != 0) {
                        tw++;
                        whiteAvg[i] += maxTmp[i][j];
                    }
                }
                whiteAvg[i] /= tw;
                for (j = 0;j < 10;j++) {
                    if (minTmp[i][j] != 1023) {
                        tb++;
                        blackAvg[i] += minTmp[i][j];
                    }
                }
                blackAvg[i] /= tb;

            }

            /** 红外值的手工修正 **/

            //whiteAvg[3] -= 36;
            //whiteAvg[2] -= 10;
            //whiteAvg[6] -= 10;

            return;
        }


        // 采集红外值
        for (i = 0;i < nIR / 2;i++) {
            irSendDouble(irRevPair[i][0], irRevPair[i][1]);

            for (j = 0;j < 100;j++);

            ReadADCDouble(irRevPair[i][2],irRevPair[i][3], &wir[irRevPair[i][0]], &wir[irRevPair[i][1]]);

        }

        irSendAllStop();

        // 找最大最小
        for (i = 0;i < nIR;i++) {
            for (j = 0;j < 10;j++) {
                if (wir[i] > maxTmp[i][j]) {
                    maxTmp[i][j] = wir[i];
                    break;
                }
            }
            for (j = 0;j < 10;j++) {
                if (wir[i] < minTmp[i][j]) {
                    minTmp[i][j] = wir[i];
                    break;
                }
            }

        }
    }
}


// 多少黑 多少白
struct _SPC_PNT {
    INT32U allWhite;
    INT32U allBlack;
    INT32U likeStart;
    INT16U dist;
}
spcPnts[30] = {{0,0,0,0}};
INT8U spcPntsN = 0;

INT16U nBlack, nWhite;

// 检测是不是同一个点, 不是则新开一个
void NewSpecialPoint(void) {
    if (GetDistance() - spcPnts[spcPntsN].dist > 8) {
        if (spcPntsN > 28) return;
        spcPntsN++;
        spcPnts[spcPntsN].allWhite = 0;
        spcPnts[spcPntsN].allBlack = 0;
        spcPnts[spcPntsN].likeStart = 0;
        spcPnts[spcPntsN].dist = GetDistance();
    }
}

void ProcessSpecialPoint(void) {
    INT16U centerMax1,centerMax2, sideMax1,sideMax2;
    INT16U i,j;
    nBlack = nWhite = 0;

    // 在路中间么
    if (LastSrvAvg > 115 || LastSrvAvg < 75)
        return;

    // 判断坡
    for (i = 0;i < nIR;i++) {
        if (wir[i] > whiteAvg[i] + (1023 - whiteAvg[i]) / 3) {
            nWhite++;
        }
    }// for

    if ( nWhite > 5 && blState == 1) {
        NewSpecialPoint();
        spcPnts[spcPntsN].allWhite++;
    }


    // 判断出发点
    centerMax1 = centerMax2 = sideMax1 = sideMax2 = 0;
#if nIR == 8
    for (j = 2;j <= 5;j++) {
#elif nIR == 12
    for (j = 4;j <= 7;j++) {
#endif
        if (centerMax1 < irValue[j]) {
            centerMax2 = centerMax1;
            centerMax1 = irValue[j];