ccorecontrol.c

飞思卡尔半导体是全球领先的半导体公司,为汽车、消费、工业、网络和无线市场设计并制造嵌入式半导体产品。这家私营企业总部位于美国德克萨斯州奥斯汀,在全球30多个国家和地区拥有设计、研发、制造和销售机构。

        }
    }

#if nIR == 8
    for (j = 6;j <= 9;j++) {
        if (sideMax1 < irValue[j % 8]) {
            sideMax2 = sideMax1;
            sideMax1 = irValue[j % 8];
        }
    }
#elif nIR == 12
    for (j = 6;j <= 9;j++) {
        if (sideMax1 < irValue[j % 8 + 2]) {
            sideMax2 = sideMax1;
            sideMax1 = irValue[j % 8 + 2];
        }
    }
#endif

    if (sideMax1 + sideMax2 + 20 < centerMax1 + centerMax2 && blState > 1) {
        NewSpecialPoint();
        spcPnts[spcPntsN].likeStart++;
    }

    // 判断交叉线
    for (i = 0;i < nIR;i++) {
        if (irValue[i] < 40) {
            nBlack++;
        }
    }
    if (nBlack >= 8 && blState > 1) {
        NewSpecialPoint();
        spcPnts[spcPntsN].allBlack++;
    }
}

void PrintProcSpcResult(void) {
    INT8U i;
    prints("total = %d\n", spcPntsN);
    for (i = 1;i <= spcPntsN;i++) {
        prints("w = %d\tb = %d\ts = %d\tdist = %d\n",
               (INT16U)spcPnts[i].allWhite, (INT16U)spcPnts[i].allBlack, (INT16U)spcPnts[i].likeStart, spcPnts[i].dist);
    }
}




void ProcStartLine(void) {
    nowLoop++;
    switch (nowLoop) {
    case 1:
        PORTB = ~0x22;

        StartLineDist[0] = GetDistance();
        break;
    case 2:
        StartLineDist[1] = GetDistance();

        PORTB = 0xA5;

        // 跑完第二圈后停下
        DistLimit = ( GetDistance() + (StartLineDist[1] - StartLineDist[0]) ) / 25 + 4;
        /** 在这里触发路径播放 **/
        StartPathPlay();

        //maxSpeed = 110;
        //minSpeed = 75;


        break;
    case 3:
        // 到第三圈后停下
        StartLineDist[2] = GetDistance();

        PORTB = 0x81;
        //DistLimit = GetDistance() / 25 + 2;
        break;
    default:
        break;
    }
}
void ProcRamp(void) {
    PORTB = 0x77;

    RampDist[RampDistN] = GetDistance();
    RampDistN++;

}
void ProcCrossLine(void) {
    PORTB = ~PORTB;

    CrossLineDist[CrossLineN] = GetDistance();
    CrossLineN++;
}

// 已经检查过的点
INT8U chkedPntsN = 0;
INT16U lastAllWhiteDist = 0;
void CheckNewPoint(void) {
    // 检查是不是有没有处理的新点
    if (chkedPntsN < spcPntsN && spcPnts[spcPntsN].dist + 4 < GetDistance() ) {
        if (lastAllWhiteDist == 0 || lastAllWhiteDist + 70 < GetDistance()) {
            if (spcPnts[spcPntsN].allWhite > 0) {
                // 进入坡道
                //PORTB = ~(1);
                lastAllWhiteDist = GetDistance();

                ProcRamp();

            }

            if (spcPnts[spcPntsN].allBlack > 0 || spcPnts[spcPntsN].likeStart > 0) {
                // 此处 乘3 因为allBlack 往往比 likeStart 大很多
                if (spcPnts[spcPntsN].allBlack > spcPnts[spcPntsN].likeStart * 3) {
                    // 交叉点
                    //PORTB = ~PORTB;
                    ProcCrossLine();

                } else {
                    // 出发线
                    //PORTB = 0x9;
                    ProcStartLine();


                }
            }
        }
        chkedPntsN++;
    }
}

INT16U xo = 100;
INT16U CenterOffset(INT16U bl) {
    if ( bl >= xo ) {
        return 125 + ( bl - xo ) * 125 / (250 - xo);
    } else {
        return 125 - (xo - bl) * 125 / xo;
    }
}

// 核心进程
void CoreControl(void) {
    INT16U i = 0, j;
    INT8U tpls = 0;
//    INT16U tmp;

    // 串口发送的状态, 和串口字
    //INT8U sciState = 0;//, sciChar;

    StartSpeeder();
    ClearDistanceCounter();

    MotorControlInit();

    crsN = 0;
    nowLoop = 0;


    FOREVER() {
        //PORTB = ~PORTB;

        // 判断小按键是否按下, 是则进入菜单
        if (!PTIP_PTIP0) {
            WaitEnable();
            Wait(20);
            if (!PTIP_PTIP0) {
                StopRun();
                StartMenu();
                WaitSmallButtonPress();
                Wait(1500);
            }
        }


        // 采集红外值
        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]]);
        }

        // 将各值归一化
        for (i = 0;i < nIR;i++) {
            if (whiteAvg[i] < wir[i]) {
                irValue[i] = 100;
            } else if (blackAvg[i] > wir[i]) {
                irValue[i] = 0;
            } else {
                irValue[i] = (100 * (wir[i] - blackAvg[i])) / ( whiteAvg[i] - blackAvg[i]);
            }
        }

        /** 红外值的手工修正 **/
        //irValue[2] = irValue[2] * 100 / 94;
        //irValue[3] = irValue[3] * 100 / 86;
        //irValue[6] = irValue[6] * 100 / 94;


        // 找最低
        minIRv = 100;
        maxIRv = 0;
        ttotal = 0;
        for (i = 0;i < nIR;i++) {
            if ( minIRv > irValue[i] ) {
                minIRn2 = minIRn;
                minIRn = i;
                minIRv2 = minIRv;
                minIRv = irValue[i];
            }

            if ( maxIRv < irValue[i] ) {
                maxIRv = irValue[i];
            }

            ttotal+= irValue[i];
        }
        total = ttotal;



        // 求出精确位置
        if ( minIRn <= (nIR - 1 - 1) && minIRn >= 1) {
            position = ir_position[minIRn - 1] + irValue[minIRn - 1] * IR_SPACE_BETWEEN / (irValue[minIRn - 1] + irValue[minIRn + 1]);
        } else if (minIRn == 0) {
            if (irValue[1] >= 90) {
                position = ir_position[0] - (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]);
            } else {
                position = ir_position[0] + (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]);
            }
        } else {
            if (irValue[nIR - 1 - 1] >= 92/** TODO 这样合适么? **/) {
                position = ir_position[nIR - 1] + (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]);
            } else {
                position = ir_position[nIR - 1] - (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]);
            }
        }

        // 排错, 理论上前后两次之间的值相差不应超过15

        if ( ((position > last_position)?(position - last_position):(last_position - position)) > 60) {
            if (nowLoop == 1) {
                if (GetDistance() - lastAllWhiteDist > 50)
                    position = last_position;
            } else if (nowLoop == 2) {
                if (GetDistance() - (StartLineDist[1] - StartLineDist[0]) - lastAllWhiteDist > 50)
                    position = last_position;
            }

        }


        // 类模糊法判断各种路面情况
        /** TODO 各各变量的权值还可以调节(通过数据), 变量数还可以增加 **/
        //情况                  minIRv              minIRv2             maxIRv              total
        blStateArr[NORMAL]  =   (100 - minIRv) +    (100 - minIRv2) +   maxIRv          +   total / nIR;
        blStateArr[LOST]    =   minIRv         +    minIRv2         +   maxIRv          +   (100 - total / nIR);
        blStateArr[CROSS]   =   (100 - minIRv) +    (100 - minIRv2) +   (100 - maxIRv)  +   (100 - total / nIR);
        blStateArr[START]   =   (100 - minIRv) +    (100 - minIRv2) +   maxIRv          +   (100 - total / nIR);

        // 找最有可能的情况
        blState = NORMAL;
        for (i = 1;i < 4;i++) {
            if (blStateArr[blState] < blStateArr[i]) {
                blState = i;
            }
        }

        tBlState = blState;


        // 根据判断结果作出相应
        if ( blState != NORMAL) {


            ProcessSpecialPoint();
            if (GetDistance() - lastAllWhiteDist < 50) {
                last_position = position;
            } else {
                position = last_position;
            }


        } else {
            last_position = position;
        }

        // 得出舵机转角
#if nIR == 8
        // 与以前的匹配
        if (position > 125) {
            position = 125 + (position - 125) * 220 / 175;
        } else {
            position = 125 - (125 - position) * 220 / 175;
        }
#endif
        /** TODO 使用一个函数发生器来为舵机提供转角 **/
        servoTgtAngle = (INT8U)PosToAgl(position);

        //累加舵机角度
        if (PerDistSrvTotal < 0xFFFFFF && PerDistSrvN < 0xFFF0 && servoTgtAngle < 130 && servoTgtAngle > 50 ) {
            PerDistSrvTotal += servoTgtAngle;
            PerDistSrvN++;
        }


        // 简单转速控制
        tgtSpeed = (INT16S)(minSpeed + (maxSpeed - minSpeed) * (speed_arr[position] ) / 40);


        if (tgtSpeed < 50 || tgtSpeed > XSpeed) {
            tgtSpeed = lastTgtSpeed;
        } else {
            lastTgtSpeed = tgtSpeed;
        }


        // 转动舵机
        ServoControl();

        // 转动马达, 有限制距离的
        if (DistLimit != 0 && DistLimit * 25 < GetDistance() ) {
            StopRun();
        } else {
            MotorControl();
        }

        tdist = GetDistance();

        /*if(SCI0SR1 & 0x80) {
            switch(sciState) {
                case 0:
                    sciChar = 0xFE;
                    break;
                case 1:
                    sciChar = (INT8U)position;
                    break;
                case 2:
                    sciChar = (INT8U)servoTgtAngle;
                    break;
                case 3:
                    sciChar = ((INT8U)((3927 * 4) / GetSpeed())  * _RTI_P )& 0xFF;
                    break;
                case 4:
                    sciChar = (INT8U)(GetDistance());
                default:  
                    break;            
            }
            sciState = (INT16U)(sciState + 1) % 5;
            SCI0DRL = sciChar;
        } */
        ///////////////////////////////////////////////////////////////////////
    }

}

void StartCore(void) {
    PORTB = 0xAA;
    DDRB = 0xFF;

    //  初始化小按键
    DDRP_DDRP0 = 0;
    PPSP_PPSP0 = 0;
    PERP_PERP0 = 1;

    /********************************/
    /* 初始化 */
    InitServo();
    StartServo();
    SetServo(90);

    InitSpeeder();

    irInit();

    InitADC();

    InitSCI0();

    WaitEnable();

    /*******************************/

    GetBlackAndWhite();

    PORTB = 0x55;

    Wait(1500);

    PORTB = 0xAA;

    while (PTIP_PTIP0);

    PORTB = 0x5A;

    Wait(1000);

    PORTB = 0xA5;

    CoreControl();

}