spathmem.c

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

/*
 * 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"


INT8U PathMemState = RECORD;

#define UP 2
#define DOWN 0
#define ZERO 1

#define POSI 0
#define NEGA 1

INT16U lastDir = UP, nowDir;
INT8U lastValue = 90;
INT8U nowSigned, lastSigned = POSI;
INT8U valve = 10; /* 阀值 */

INT16U newXPointDist;
INT8U newXPointValue;
INT8U newXPointType;
INT16U lastXPointDist;

INT16U edgeStartPointDist, edgeEndPointDist;

#define MAX 1
#define MIN 2

#define WAIT 1
#define GET_X 2

INT8U nowState = WAIT;
INT8U lastState = WAIT;

#define LEFT 1
#define RIGHT 2

struct _PATHINFO {
    INT16U startDist;
    INT16U endDist;

    INT16U startEdgeDist;
    INT16U endEdgeDist;

    INT8U type;

    INT8U extremum;
}
pathInfo[200];


INT8U pathInfoN = 0;

void PrintPath() {
    INT16U i;
    for (i = 0;i < pathInfoN;i++) {
        prints("%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
               i, pathInfo[i].startDist, pathInfo[i].endDist,
               pathInfo[i].startEdgeDist, pathInfo[i].endEdgeDist,
               pathInfo[i].type, pathInfo[i].extremum
              );
    }
}

void Process(INT16U i, INT8U value) {
    INT8U X_POINT = FALSE;

    if (lastValue > value) {
        nowDir = UP;
    } else if ( lastValue < value) {
        nowDir = DOWN;
    } else {
        nowDir = lastDir;
    }

    if (value >= 90) {
        nowSigned = POSI;
    } else {
        nowSigned = NEGA;
    }
    // 判断符号的变化

    if (nowSigned != lastSigned) {
        X_POINT = TRUE;
    }

    // 判断方向的变化
    if ( (lastDir >= 1) ^ (nowDir > 1) ) {
        X_POINT = TRUE;
    }

    // 找阀值边缘的点
    if (lastState == WAIT) {
        if ( (lastValue < 90 + valve && value >= 90 + valve) ||
                (lastValue > 90 - valve && value <= 90 - valve) ) {
            edgeStartPointDist = i;
        }
    } else {
        if ( (lastValue > 90 + valve && value <= 90 + valve) ||
                (lastValue < 90 - valve && value >= 90 - valve) ) {
            edgeEndPointDist = i;
        }
    }


    // 如果找到极值点
    if (X_POINT) {
        newXPointDist = i;
        newXPointValue = value;

        // 若上次在等待极值
        if (lastState == WAIT) {

            // 如果此次大于了阀值 (右)
            if (newXPointValue > 90 + valve) {
                lastState = GET_X;
                pathInfo[pathInfoN].startEdgeDist = edgeStartPointDist;
                pathInfo[pathInfoN].type = RIGHT;
                pathInfo[pathInfoN].extremum = newXPointValue;
                pathInfo[pathInfoN].startDist = lastXPointDist;
            }
            // 如果此次大于了阀值 (左)
            else if (newXPointValue < 90 - valve) {
                lastState = GET_X;
                pathInfo[pathInfoN].startEdgeDist = edgeStartPointDist;
                pathInfo[pathInfoN].type = LEFT;
                pathInfo[pathInfoN].extremum = newXPointValue;
                pathInfo[pathInfoN].startDist = lastXPointDist;
            }
        }
        // 如果上次已经是在极值中
        else if (lastState == GET_X) {
            // 如果小于了阀值, 则算完成一次判断
            if (newXPointValue <= 90 + valve && newXPointValue >= 90 - valve) {
                pathInfo[pathInfoN].endEdgeDist = edgeEndPointDist;
                pathInfo[pathInfoN].endDist = newXPointDist;
                lastState = WAIT;
                pathInfoN++;
            }
            // 如果大于阀值, 则刷最高点 (右)
            else if (newXPointValue > 90 + valve) {
                if (pathInfo[pathInfoN].extremum < newXPointValue) {
                    pathInfo[pathInfoN].extremum = newXPointValue;
                }
            }
            // 如果大于阀值, 则刷最低点 (左)
            else if (newXPointValue < 90 - valve) {
                if (pathInfo[pathInfoN].extremum > newXPointValue) {
                    pathInfo[pathInfoN].extremum = newXPointValue;
                }
            }
        }

        // 记下上次值
        lastXPointDist = newXPointDist;
    }

    lastDir = nowDir;
    lastValue = value;
    lastSigned = nowSigned;

}


/******************************************************************************************/
// play

INT16U firstLoopStartDist;
INT16U secondLoopStartDist;

INT8U nowSpeedChgN = 0;

INT8U pathPlayN = 0;
struct _SPEED_CHG_POINTS {
    INT16U dist;
    INT16U maxSpd;
    INT16U minSpd;
}
SpeedChgPoints[200];

INT8U SpeedChgN = 1;



INT16U XSpeed = 300;

INT16U BendMaxSpeedBase = 110;
INT16U SingleBendMaxSpeedBase = 125;

INT16U needUpdateDist = 0;
// 计算弯道最大速度
INT16U GetBendMaxSpeed(INT8U x) {
    INT16U tspd;
    if ( x > 90 ) {
        tspd = (130 - x)  * 30 / 40 + BendMaxSpeedBase;
    } else {
        tspd = (x - 30) * 30 / 40 + BendMaxSpeedBase;
    }
    return (tspd > XSpeed)?(XSpeed):tspd;
}

INT16U GetSingleBendMaxSpeed(INT8U x, INT16U arcLength) {
    INT16U tspd;

    if (arcLength > 45) return GetBendMaxSpeed(x);

    if ( x > 90 ) {
        tspd = (130 - x)  * 33 / 40 + SingleBendMaxSpeedBase;
    } else {
        tspd = (x - 30) * 33 / 40 + SingleBendMaxSpeedBase;
    }


    return (tspd > XSpeed)?(XSpeed):tspd;
}

INT16U ErrorLength = 30;
INT16U AccLength = 15;

INT16U EnterLength = 5;
INT16U OutLength = 57;
INT16U SpeedUpLength = 5;
INT16U SpeedDownLength = 2;

// 在一定长的直道上的最小速度
INT16U MSpeed = 130;

// 根据长度得到直道得最大速度(length 应大于1m)
INT16U GetStraightMaxSpeed(INT16U length) {
    if (length > 90 /* 4m */) {
        return XSpeed;
    } else if ( length < 10) {
        return MSpeed;
    } else {
        return MSpeed + (XSpeed - MSpeed) * (length - 5) / 75;
    }
}
// 根据最大速度计算合适的减速距离
INT16U MaxStraightSpdDwnLength = 40;
INT16U MinStraightSpdDwnLength = 18;
INT16U GetStraightSpdDwnLength(INT16U maxspeed, INT16U bendspeed) {
    INT16U dspeed = (maxspeed > bendspeed)?(maxspeed - bendspeed):(1);
    return MinStraightSpdDwnLength +
           (MaxStraightSpdDwnLength - MinStraightSpdDwnLength) * dspeed / 170;

}


// 直道误差距离
INT16U StrightErrorLength = 2;

// 最短高速行驶距离
INT16U ShortestHighSpeedLength = 14;


void CalcStraightSpeed(INT16U StartDist, INT16U length, INT16U BendSpeed) {
    INT16U strMaxSpeed, strSpdDwnLength;
    // 直道太短不宜高速行驶
    if (length < ShortestHighSpeedLength) {
        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 {