spathmem.c

利用飞思卡尔单片机实现循线小车的控制

        // 可以高速行驶
        // 预处理
        strMaxSpeed = GetStraightMaxSpeed(length);
        strSpdDwnLength = GetStraightSpdDwnLength(strMaxSpeed, BendSpeed);


        if (length > 2 * StrightErrorLength) {
            // (1) 安全误差点
            SpeedChgPoints[SpeedChgN].dist = StartDist;
            SpeedChgPoints[SpeedChgN].maxSpd = SpeedChgPoints[SpeedChgN - 1].maxSpd;
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN - 1].minSpd;
            SpeedChgN++;
            if (SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
                SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

            if ( length > 2 * StrightErrorLength + strSpdDwnLength) {

                // (2) 加速点
                SpeedChgPoints[SpeedChgN].dist = StartDist + StrightErrorLength;
                SpeedChgPoints[SpeedChgN].maxSpd = strMaxSpeed;
                SpeedChgPoints[SpeedChgN].minSpd = strMaxSpeed;
                SpeedChgN++;
                if (SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
                    SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;


                // (3) 高速结束点
                SpeedChgPoints[SpeedChgN].dist = StartDist + length - strSpdDwnLength;
                SpeedChgPoints[SpeedChgN].maxSpd = strMaxSpeed;
                SpeedChgPoints[SpeedChgN].minSpd = strMaxSpeed;
                SpeedChgN++;
                if (SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
                    SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

                // (4) 减速点 + 安全误差点
                SpeedChgPoints[SpeedChgN].dist = StartDist + length - strSpdDwnLength - StrightErrorLength;
                SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
                SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;
                SpeedChgN++;
                if (SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
                    SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;
            }
        }
        // (5) 弯点
        SpeedChgPoints[SpeedChgN].dist = StartDist + length;
        SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;
        SpeedChgN++;
        if (SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

    }
}
/*

// 计算直道的速度
void CalcStraightSpeed(INT16U StartDist, INT16U length, INT16U BendSpeed) {
    // 小于误差范围
    if(length < EnterLength + OutLength) {
        SpeedChgPoints[SpeedChgN].dist = StartDist;
        SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN - 1].minSpd;
        if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

        SpeedChgN++;

        SpeedChgPoints[SpeedChgN].dist = StartDist + length;
        SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;
        if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

        SpeedChgN++;
    } else {
        SpeedChgPoints[SpeedChgN].dist = StartDist + EnterLength;
        SpeedChgPoints[SpeedChgN].maxSpd = SpeedChgPoints[SpeedChgN - 1].maxSpd;
        SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN - 1].minSpd;
        SpeedChgN++;
        if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

        // 小于加到极速的范围
        if(length < EnterLength + OutLength + SpeedUpLength + SpeedDownLength) {

            SpeedChgPoints[SpeedChgN].dist = StartDist + length / 4;
            SpeedChgPoints[SpeedChgN].maxSpd = XSpeed * (length - (EnterLength + OutLength)) / ( SpeedUpLength + SpeedDownLength);
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN - 1].minSpd;
            SpeedChgN++;
            if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

        }
        // 可加到极速
        else {
            SpeedChgPoints[SpeedChgN].dist = StartDist + EnterLength + SpeedUpLength;
            SpeedChgPoints[SpeedChgN].maxSpd = XSpeed;
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN - 1].minSpd;
            SpeedChgN++;
            if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

            SpeedChgPoints[SpeedChgN].dist = StartDist + length - OutLength - SpeedDownLength;
            SpeedChgPoints[SpeedChgN].maxSpd = XSpeed;
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN - 1].minSpd;
            SpeedChgN++;
            if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;
        }

        SpeedChgPoints[SpeedChgN].dist = StartDist + length - EnterLength;
        SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;
        SpeedChgN++;
        if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

        SpeedChgPoints[SpeedChgN].dist = StartDist + length;
        SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;
        SpeedChgN++;
        if(SpeedChgPoints[SpeedChgN].maxSpd <= SpeedChgPoints[SpeedChgN].minSpd)
            SpeedChgPoints[SpeedChgN].minSpd = SpeedChgPoints[SpeedChgN].maxSpd;

    }
}
*/
// 计算弯道速度
void CalcBendSpeed(INT16U EndDist, INT16U BendSpeed, INT16U NextBendSpeed) {
    SpeedChgPoints[SpeedChgN].dist = EndDist;
    if (NextBendSpeed > BendSpeed) {
        SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;
    } else {
        SpeedChgPoints[SpeedChgN].maxSpd = NextBendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = NextBendSpeed;
    }


    SpeedChgN++;
}

// 计算单弯道速度(有过弯加速)
void CalcSingleBendSpeed(INT16U startDist, INT16U endDist, INT16U BendSpeed) {
    /*   if(endDist - startDist > 10) {
           SpeedChgPoints[SpeedChgN].dist = endDist - 10;
           SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
           SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;;
           SpeedChgN++; 
     
           SpeedChgPoints[SpeedChgN].dist = endDist;
           SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed + 7;
           SpeedChgPoints[SpeedChgN].minSpd = BendSpeed + 7;
           SpeedChgN++;
       } else */{
        SpeedChgPoints[SpeedChgN].dist = endDist;
        SpeedChgPoints[SpeedChgN].maxSpd = BendSpeed;
        SpeedChgPoints[SpeedChgN].minSpd = BendSpeed;
        SpeedChgN++;
    }


}

void PathPlayProcess(INT16U dist) {
    INT16U memDist;
    INT16U straightStartDist, straightEndDist;

    if ( dist < needUpdateDist) {
        return;
    }

    // 和记忆对应的距离
    memDist = dist - secondLoopStartDist + firstLoopStartDist;

    // 找直道
    while (pathPlayN <= pathInfoN) {

        if (pathPlayN == 0) {
            if (pathInfo[pathPlayN].startDist > memDist) {
                // 找到
                break;
            } else {
                pathPlayN++;
            }
        } else {
            if (pathInfo[pathPlayN - 1].endDist <= memDist &&
                    pathInfo[pathPlayN].startDist > memDist) {
                // 找到
                break;
            } else {
                pathPlayN++;
            }
        } // if pathPlayN
    } // while

    // 处理直道
    straightStartDist = dist;
    straightEndDist = pathInfo[pathPlayN].startDist + secondLoopStartDist - firstLoopStartDist;


    // 处理弯道
    if (pathPlayN + 1 <= pathInfoN) {
        /*******************************************************************************************/
        // 下一个还是弯道么?

        // 是直道
        if (pathInfo[pathPlayN].endDist - pathInfo[pathPlayN + 1].startDist > 0) {
            // 处理直道
            CalcStraightSpeed(straightStartDist,
                              straightEndDist - straightStartDist,
                              GetSingleBendMaxSpeed(pathInfo[pathPlayN].extremum,
                                                    pathInfo[pathPlayN].endEdgeDist - pathInfo[pathPlayN].startEdgeDist));

            CalcSingleBendSpeed(pathInfo[pathPlayN].startDist + secondLoopStartDist - firstLoopStartDist,
                                pathInfo[pathPlayN].endDist + secondLoopStartDist - firstLoopStartDist,
                                GetSingleBendMaxSpeed(pathInfo[pathPlayN].extremum,
                                                      pathInfo[pathPlayN].endEdgeDist - pathInfo[pathPlayN].startEdgeDist));
            pathPlayN++;
        } else {
            // 处理直道
            CalcStraightSpeed(straightStartDist,
                              straightEndDist - straightStartDist,
                              GetBendMaxSpeed(pathInfo[pathPlayN].extremum));

            for (;;) {
                if (pathInfo[pathPlayN].endDist - pathInfo[pathPlayN + 1].startDist < 5) {
                    CalcBendSpeed(pathInfo[pathPlayN].endDist + secondLoopStartDist - firstLoopStartDist,
                                  GetBendMaxSpeed(pathInfo[pathPlayN].extremum),
                                  GetBendMaxSpeed(pathInfo[pathPlayN + 1].extremum));
                    pathPlayN++;
                } else {
                    CalcBendSpeed(pathInfo[pathPlayN].endDist + secondLoopStartDist - firstLoopStartDist,
                                  GetBendMaxSpeed(pathInfo[pathPlayN].extremum),
                                  GetBendMaxSpeed(pathInfo[pathPlayN].extremum));
                    pathPlayN++;
                    break;
                }
            }
        }
        /*******************************************************************************************/
    } else {
        // 到最后, 冲刺了?
    }

    needUpdateDist = pathInfo[pathPlayN - 1].endDist - firstLoopStartDist + secondLoopStartDist;
}


void PathPlayer(INT16U dist) {
    INT16U i;
    for (i = nowSpeedChgN;i < SpeedChgN - 1;i++) {
        if ( dist > SpeedChgPoints[i].dist && dist <= SpeedChgPoints[i+1].dist ) {
            /********************************************************************/
            if (SpeedChgPoints[i].maxSpd >= SpeedChgPoints[i + 1].maxSpd) {
                maxSpeed = ( SpeedChgPoints[i+1].dist - dist ) *
                           ( SpeedChgPoints[i].maxSpd - SpeedChgPoints[i+1].maxSpd) / (SpeedChgPoints[i+1].dist - SpeedChgPoints[i].dist) + SpeedChgPoints[i+1].maxSpd;
            } else {
                maxSpeed = ( dist - SpeedChgPoints[i].dist ) *
                           ( SpeedChgPoints[i+1].maxSpd - SpeedChgPoints[i].maxSpd) / (SpeedChgPoints[i+1].dist - SpeedChgPoints[i].dist) + SpeedChgPoints[i].maxSpd;
            }

            if (SpeedChgPoints[i].minSpd >= SpeedChgPoints[i + 1].minSpd) {
                minSpeed = ( SpeedChgPoints[i+1].dist - dist ) *
                           ( SpeedChgPoints[i].minSpd - SpeedChgPoints[i+1].minSpd) / (SpeedChgPoints[i+1].dist - SpeedChgPoints[i].dist) + SpeedChgPoints[i+1].minSpd;
            } else {
                minSpeed = ( dist - SpeedChgPoints[i].dist ) *
                           ( SpeedChgPoints[i+1].minSpd - SpeedChgPoints[i].minSpd) / (SpeedChgPoints[i+1].dist - SpeedChgPoints[i].dist) + SpeedChgPoints[i].minSpd;
            }

            if (maxSpeed < minSpeed ) {
                minSpeed = maxSpeed;
            }

            return;
            /********************************************************************/
        }
    }

}

void StartPathPlay(void) {
    firstLoopStartDist = StartLineDist[0];
    secondLoopStartDist = StartLineDist[1];

    nowSpeedChgN = 0;

    pathPlayN = 0;
    SpeedChgPoints[0].dist = secondLoopStartDist;
    SpeedChgPoints[0].maxSpd = 110;
    SpeedChgPoints[0].minSpd = 80;

    SpeedChgN = 1;

    //ErrorLength = 30;
    //AccLength = 75;

    //XSpeed = 200;

    needUpdateDist = 0;

    PathMemState = PLAY;
}