?? 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];
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -