STM32顏色識別循跡代碼 STM32四驅(qū)車運(yùn)動(dòng)庫函數(shù)版本

ID:640768 · 發(fā)表于 2021-8-6 10:38

/**
* @par Copyright (C): 2010-2019, Shenzhen Yahboom Tech
* @file main.c
* @author liusen
* @version V1.0
* @date 2017.07.17
* @brief 主函數(shù)
* @details
* @par History
*
* version: liusen_20170717
*/
//本實(shí)驗(yàn)對環(huán)境的要求較高,需要用戶自行調(diào)整循跡模塊的靈敏度
//灰度傳感器在不同的光照條件下對不同的顏色有不同的數(shù)值
#include "stm32f10x.h"
#include "app_motor.h"
#include "app_ultrasonic.h"
#include "bsp.h"
#include "sys.h"
#include "delay.h"
#include "app_buzzer.h"
#include "bsp_linewalking.h"
#include "bsp_colorful.h"
#define LOW (0)
#define HIGH (1)
//第二位 1:左90 ; 2: 右90; 3: 180度掉頭 4:直行
//這里可以自定義路徑規(guī)劃, 小車放置在起始位置深圳, 根據(jù)下一個(gè)十字或者丁字路口來寫如上行注釋來左轉(zhuǎn) 、右轉(zhuǎn) 、直行、掉頭。
char mapLocation[38][2] = {
{0, 2}, {1, 1}, {2, 2}, {3, 3},
{4, 2}, {5, 4}, {6, 3},
{7, 2}, {8, 2}, {9, 2}, {10, 3},
{11, 4}, {12, 4}, {13, 3},
{14, 2}, {15, 1}, {16, 1}, {17, 3},
{18, 2}, {19, 1}, {20, 3},
{21, 1}, {22, 1}, {23, 1}, {24, 2}, {25, 3},
{26, 2}, {27, 2}, {28, 3},
{29, 2}, {30, 2}, {31, 3},
{32, 2}, {33, 1}, {34, 3},
{35, 1}, {36, 2}, {37, 3}
};
int position = 0; //位置定義
/**
* Function Color_Dis
* @author liusen
* @date 2017.08.15
* @brief 顏色設(shè)置根據(jù)實(shí)際情況采集數(shù)據(jù)顯示
* @param[in] v_ad : 采集顏色AD
* @param[out] void
* @retval void
* @par History 無
*/
void Color_Dis(int v_ad)
{
int Color = v_ad;
if (Color >= 370 && Color <= 410)
{
bsp_Colorful_RGB_PWM(0, 0, 255);
}
else if (Color >= 300 && Color <= 320)
{
bsp_Colorful_RGB_PWM(0, 255, 0);
}
else if (Color >= 345 && Color <= 365)
{
bsp_Colorful_RGB_PWM(255, 0, 0);
}
else if (Color >= 330 && Color < 345)
{
bsp_Colorful_RGB_PWM(255, 0, 255);
}
else if (Color >= 270 && Color <= 290)
{
bsp_Colorful_RGB_PWM(0, 255, 255);
}
else if (Color >= 250 && Color < 270)
{
bsp_Colorful_RGB_PWM(255, 255, 0);
}
else
{
bsp_Colorful_RGB_PWM(0, 0, 0);
}
}
int main(void)
{
int iGs = 0;
int TrackSensorLeftValue1 = 1, TrackSensorLeftValue2 = 1, TrackSensorRightValue1 = 1, TrackSensorRightValue2 = 1;
/*外設(shè)初始化*/
bsp_init();
/*按鍵啟動(dòng)*/
while(bsp_GetKEY());
Buzzer_GPIO_Init();
while (1)
{
/*下面兩行主要是測試當(dāng)前環(huán)境光不同顏色的AD值，方便修改上面Color_Dis函數(shù)的范圍值*/
//iGs = Get_GS_Value();
//printf("Color:%d\r\n", iGs);
bsp_GetLineWalking(&TrackSensorLeftValue1, &TrackSensorLeftValue2, &TrackSensorRightValue1, &TrackSensorRightValue2);
if ( ((TrackSensorLeftValue1 == LOW || TrackSensorLeftValue2 == LOW)
&& TrackSensorRightValue2 == LOW) || ( TrackSensorLeftValue1 == LOW
&& (TrackSensorRightValue1 == LOW || TrackSensorRightValue2 == LOW)))
{
Car_Run(3400);
delay_ms(60);
Car_Stop();
delay_ms(200);
switch (mapLocation[position][1])
{
case 0: Car_Stop(); break;
case 1: //左轉(zhuǎn)90,這里所說的左轉(zhuǎn)90度其實(shí)是會(huì)讓小車轉(zhuǎn)過一定的角度,然后讓下面的while(1)去判斷
//當(dāng)中間的兩路檢測到時(shí)會(huì)停止轉(zhuǎn)動(dòng),所以會(huì)有一個(gè)轉(zhuǎn)動(dòng)90度的效果.
{
/*延遲旋轉(zhuǎn)轉(zhuǎn)過丁字路口黑線解決探頭開始就在黑線上判斷*/
Car_SpinLeft(4300, 4300);
delay_ms(200);
while (1)
{
Car_SpinLeft(4300, 4300);
bsp_GetLineWalking(&TrackSensorLeftValue1, &TrackSensorLeftValue2, &TrackSensorRightValue1, &TrackSensorRightValue2);
if (TrackSensorLeftValue2 == LOW || TrackSensorRightValue1 == LOW)
{
Car_Stop();
break;
}
}
} break;
case 2://右轉(zhuǎn)90
{
/*延遲旋轉(zhuǎn)轉(zhuǎn)過丁字路口黑線解決探頭開始就在黑線上判斷*/
Car_SpinRight(4300, 4300);
delay_ms(200);
while (1)
{
Car_SpinRight(4300, 4300);
bsp_GetLineWalking(&TrackSensorLeftValue1, &TrackSensorLeftValue2, &TrackSensorRightValue1, &TrackSensorRightValue2);
if (TrackSensorLeftValue2 == LOW || TrackSensorRightValue1 == LOW)
{
Car_Stop();
break;
}
}
} break;
case 3://左旋180掉頭
{
Car_Stop();
delay_ms(200);
BeepOnOffMode();
iGs = Get_GS_Value();
//printf("Color:%d\r\n", iGs); //這里可以打開調(diào)試數(shù)據(jù)
Color_Dis(iGs);
if (position == 37) //走完地圖直接停止
{
ModeBEEP(3);
BeepOnOffMode();
position = 0;
while(1);
}
Car_SpinLeft(4300, 4300);
//下面的延時(shí)相比上面的而言因?yàn)橘惖赖念伾珔^(qū)域前有一段黑線,延時(shí)長就是為了略過這條黑線的檢測
delay_ms(450);
while (1)
{
Car_SpinLeft(4300, 4300);
bsp_GetLineWalking(&TrackSensorLeftValue1, &TrackSensorLeftValue2, &TrackSensorRightValue1, &TrackSensorRightValue2);
if (TrackSensorLeftValue2 == LOW || TrackSensorRightValue1 == LOW)
{
Car_Stop();
break;
}
}
} break;
case 4: Car_Run(5000); delay_ms(50); break;//直行
}
position++;
}
// 0 X X X
//最左邊檢測到
else if ( TrackSensorLeftValue1 == LOW)
{
Car_SpinLeft(3400, 3400);
}
// X X X 0
//最右邊檢測到
else if ( TrackSensorRightValue2 == LOW )
{
Car_SpinRight(3400, 3400);
}
//四路循跡引腳電平狀態(tài)
// X 0 1 X
//處理左小彎
else if ( TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == HIGH)
{
Car_Left(3400);
}
//四路循跡引腳電平狀態(tài)
// X 1 0 X
//處理右小彎
else if (TrackSensorLeftValue2 == HIGH && TrackSensorRightValue1 == LOW)
{
Car_Right(3400);
}
else if (TrackSensorLeftValue2 == LOW && TrackSensorRightValue1 == LOW)
{
Car_Run(3400);
}
}
}