【正文】 icStatus^car[5].staticStatus。 for(i=0。 i8。 i++) { if(edgeamp。LASER_BIT(i)) g_staticEdgeNum++。 } if(g_staticStatusamp。LASER_BIT(6)) g_staticShift = 3。 else if(g_staticStatusamp。LASER_BIT(1)) g_staticShift = 3。 else if(g_staticStatusamp。LASER_BIT(7)) g_staticShift = 1。 else if(g_staticStatusamp。LASER_BIT(0)) g_staticShift = 1。 else g_staticShift = 0。 if(g_staticNum==0) g_staticShift = 0。 }void StopDectect(void){ static int32 delta = 0。 static uint8 tFlag = 0。 static uint8 securityCnt = 0。 static uint8 judgeFlag = 0。 static int32 securityDelta = 0。 static int32 maxDelta = 0。 static int8 edgeNum = 0。 static uint8 perhapesFlag = 0。 int8 i = 0。 if(g_staticStatus==0xf3 ||g_staticStatus==0xf7 ||g_staticStatus==0xe7 ||g_staticStatus==0x73 ||g_staticStatus==0x71 ||g_staticStatus==0x31 ||g_staticStatus==0xef ||g_staticStatus==0xcf ||g_staticStatus==0xce ||g_staticStatus==0x8e ||g_staticStatus==0xce ||g_staticStatus==0x8e ||g_staticStatus==0xce) { if(g_startFlag==0) { g_startFlag++。 securityCnt = 1。 } if(g_startFlag==1amp。amp。securityDelta*1000/CNT1000MM=1000) g_startFlag++。 if(g_startFlag==2) { g_startFlag = 0。 g_stopFlag = 1。 } } if(securityCnt) { securityDelta += car[0].speed。 if(securityDelta=1500*CNT1000MM/1000) { securityDelta = 1500*CNT1000MM/1000。 securityCnt = 0。 } }}void ShakeControl(void){ static int32 curveToStright = 0。 int8 laserShift = 0。 float kp1 = 0。 g_shakeControl = (int32) (KP_SHAKE*(1+KI_SHAKE+KD_SHAKE)*car[0].laserShift/2 KP_SHAKE*(1+2*KD_SHAKE)*car[1].laserShift/2 +KP_SHAKE*KD_SHAKE*car[2].laserShift/2)。/***************限幅****************/ if(g_shakeControl=SHAKE_CONTROL_M+4500)//4000 g_shakeControl = SHAKE_CONTROL_M+4500。 else if(g_shakeControl=SHAKE_CONTROL_M4500) g_shakeControl = SHAKE_CONTROL_M4500。 g_shakeShift = (SHAKE_CONTROL_Mg_shakeControl)*。 if(abs(g_shakeShift)1amp。amp。abs(car[1].shakeShift)=1) { g_shakeShift = 0。 g_shakeControl = SHAKE_CONTROL_M。 }// 賦值 SHAKE_CONTROL = g_shakeControl。 g_roadShift = laserShift*+g_shakeShift。 car[0].setShake = g_shakeControl。//存儲數據 car[0].shakeShift = g_shakeShift。 car[0].roadShift = g_roadShift。}void RoadAnalyse(void){ static int32 i,j。 static int32 sum = 0。 static int32 shiftMax = 100。 static int32 shiftMin = 100。 static int32 straightSum = 0。 if(car[0].speed!=0) { j++。 sum += car[0].speed。 if(shiftMaxcar[0].roadShift) shiftMax = car[0].roadShift。 if(shiftMincar[0].roadShift) shiftMin = car[0].roadShift。 shiftSum += car[0].roadShift。 } = 0。 if(sum*1000/CNT1000MM=30) { sum = 0。//防脈沖干擾平均濾波 [0] = (shiftSumshiftMaxshiftMin)/(j2)。 [0] = (shiftSum)/j。 shiftMax = 100。 shiftMin = 100。 shiftSum = 0。 j = 0。 //平均值濾波 [0] = ([0]+[1]+[2])/3。 [1] = ([0]+[1]+[2]+[3])/4。 for(i=2。i17。i++) [i]=([i2]+[i1]+[i]+[i+1]+[i+2])/5。 [18] = ([16]+[17]+[18]+[19])/4。 [19] = ([17]+[18]+[19])/3。 shift1 = [0]。 shift5 = 0。 shift10 = 0。 shift20 = 0。 shiftD6 = 0。 shiftD10 = 0。 shiftD20 = 0。 shiftMax = 100。 shiftMin = 100。// 計算路徑偏差參數 for(i=0。 i20。 i++) { if(i3) { shiftD6 +=([i][i+3])。 //逐差法 } if(i5) { shift5 +=[i]。 shiftD10 += ([i][i+5])。 shiftDD20 += ([i][i+10][i+5]+[i+15])。 } if(i10) { shift10+=[i]。 shiftD20+=([i][i+10])。 } shift20 += [i]。 if([i]shiftMax) shiftMax = [i]。 if([i]shiftMin) shiftMin = [i]。 } shift5 /= 5。 shift10 /= 10。 shift20 /= 20。 shiftD6 /= 9。 shiftD10 /= 25。 shiftD20 /= 100。 shiftDD20 /= 50。 = 1。 } if(!g_leftFlagamp。amp。shift115amp。amp。shift1010amp。amp。!g_rampFlag) { if(g_strightFlagamp。amp。((SWITCH_DATAamp。BREAK_OFF)==0)) g_straightToCurve = 1。 else g_straightToCurve = 0。 g_breakFlag = 0。 g_leftFlag = 1。 g_rightFlag = 0。 g_strightFlag = 0。 g_curveToStright = 0。 LED_DATA(0x01)。 } else if(!g_rightFlagamp。amp。shift115amp。amp。shift1010amp。amp。!g_rampFlag) { if(g_strightFlagamp。amp。((SWITCH_DATAamp。BREAK_OFF)==0)) g_straightToCurve = 1。 else g_straightToCurve = 0。 g_breakFlag = 0。 g_rightFlag = 1。 g_leftFlag = 0。 g_strightFlag = 0。 g_curveToStright = 0。 LED_DATA(0x04)。 } else if(!g_strightFlagamp。amp。!g_rampFlag) { if(abs(shift1)5amp。amp。abs(shift10)5) { g_strightFlag = 1。 g_straightToCurve = 0。 g_curveToStright = 1。 g_breakFlag = 0。 g_shakeShift = 0。 car[0].shakeShift = 0。 g_rightFlag = 0。 g_leftFlag = 0。 LED_DATA(0x02)。 } } if(g_strightFlag) { g_setSpeed = g_straightSpeed。 g_curveMaxSp = 0。 g_curveMinSP = g_straightSpeed。 } else if(g_leftFlag||g_rightFlag) { g_curveSpeed = (g_speedMin+(ROAD_SHIFT_MAX/2abs(shift1)/2)*((g_speedMaxg_speedMin)/(ROAD_SHIFT_MAX/2)))。 g_setSpeed = g_curveSpeed。 if(g_curveMaxSpg_curveSpeed) g_curveMaxSp = g_curveSpeed。 if(g_curveMinSPg_curveSpeed) g_curveMinSP = g_curveSpeed。 } }void SteerControl(void){ if(abs(g_shakeShift)=20) g_steerControl = STEER_CONTROL_M+car[0].roadShift*KP_STEER+(car[0].roadShiftcar[5].roadShift)*KD_STEER+(car[0].roadShiftcar[5].roadShift)*((car[0].roadShift*(car[0].roadShiftcar[5].roadShift))0)*KD_STEER*。 else if(g_shakeShift20) g_steerControl = STEER_CONTROL_M+400+car[0].roadShift*KP_STEER+(car[0].roadShiftcar[5].roadShift)*KD_STEER+(car[0].roadShiftcar[5].roadShift)*((car[0].roadShift*(car[0].roadShiftcar[5].roadShift))0)*KD_STEER*。 else g_steerControl = STEER_CONTROL_M+car[0].roadShift*KP_STEER+(car[0].roadShiftcar[5].roadShift)*KD_STEER+(car[0].roadShiftcar[5].roadShift)*((car[