目录
一、注释：
参数：pwm：最大速度值，P：比例系数
根据位置式离散PID公式
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2···k;
pid巡线函数：track_PID
float Proportion; // 比例常数 Proportional Const
float Integral; // 积分常数 Integral Const
float Derivative; // 微分常数 Derivative Const
int Error1; // Error[n-1]
int Error2; // Error[n-2]
int iError; // Error[n]
PID position 位置式
PID int MatOut 设定输出最大值
int Setpoint PID控制器设定值
position.iError=Setpoint-CurrentPoint; // 计算当前误差
position.Error_sum+=position.iError;
iIncpid=position.Proportion * position.iError // P
+position.Integral * position.Error_sum // I
+position.Derivative * (position.iError-position.Error1); // D
position.Error1=position.iError; // 存储误差，用于下次计算
sensor 传感器
analog 模拟值
左偏设置值为正，右偏设置值为负
二、接线
五路灰度传感器----stm32
DND--------------GND
VCC--------------5V
L2---------------C13
L1---------------C4
M----------------E6
R1---------------E5
R2---------------A4(可自行更改）
通讯协议
要想得到偏移量就必须使用串口传输的形式
三、代码
灰度传感器bsp_GraySensor.c
#include "bsp_GraySensor.h"
#include "main.h"
#include "adc.h"
#include "control.h"
//ÓÃÀ´Ñ²ÏßµÄ '»Ò¶È´«¸ÐÆ÷' ºÍ ÓÃÀ´¼ì²âÊÇ·ñ·ÅÉÏÒ©Æ·µÄ 'ºìÍâ´«¸ÐÆ÷' µÄ´úÂ붼·ÅÔÚÕâ¸ö.cÎļþÀïÁË
u8 L2_Val, L1_Val, M_Val, R1_Val, R2_Val;
int Line_Num;
u16 ADC_Value;
float ADC_Volt;
u8 Load_flag =0; // 0±íʾ»¹Ã»×ªÔØÒ©Æ·£¬±íʾתÔØÍê±Ï£¬-1±íʾҩƷËÍ
u8 LoadCount,NotLoadCount;
/*******************»Ò¶È´«¸ÐÆ÷ѲÏß*************/
//Ö»Òª°ÑËٶȻ·¿ØÖƺ㬲¹³¥Öµ¼´¿ÉÈ·¶¨¡£ ²»Í¬ËÙ¶ÈÖµÐèÒª²»Í¬µÄ²¹³¥Öµ¡£ ²âÊÔºÃÒ»¸ö×îºÃµÄ¡£ ²»Í¬µÄתËÙÒª¶ÔÓ¦²»Í¬µÄ²¹³¥ÏµÊý»òÊýÖµ
void Light_GoStraight_control(void) //»Ò¶ÈѲÏßÖ±ÐÐ, ÐèÒªÓиöÅжÏÐèÒªÖ±Ðж೤¾àÀëµÄº¯Êý //²¹³¥Òªµ½ÖмäµÄ´«¸ÐÆ÷²âµ½»Øµ½ÏßÉÏΪֹ
{
Get_Light_TTL();
//Èç¹ûÒª¸ü¼Ó¾«×¼µÄ¿ØÖÆ£¬Ö»ÒªÓÐÁËѲÏß»·£¬¶¼ÐèÒª¶ÔλÖýøÐв¹³¥¡£ ¿É·ñÖ±½Ó½« g_lMotorPulseSigma ºÍg_lMotor2PulseSigma ½øÐÐÊýÖµ¼Ó¼õ£¿£¿
//Èç¹ûÑ°Ö±ÏßÓõĴ«¸ÐÆ÷¿ÉÒÔ¶ÁÈ¡Ä£ÄâÁ¿µÄ»°£¬Èý¸ö´«¸ÐÆ÷¶¼¿ÉÒÔѲֱÏßѰͦÎȵÄ
if(L1_Val == 0 && M_Val == 1 && R1_Val == 0 ) Line_Num = 0;
if(L1_Val == 0 && M_Val == 1 && R1_Val == 1) Line_Num = 400;
if(L1_Val == 0 && M_Val == 0 && R1_Val == 1 ) Line_Num = 500;
if(L1_Val == 1 && M_Val == 0 && R1_Val == 0) Line_Num = -500;
if(L1_Val == 1 && M_Val == 1 && R1_Val == 0) Line_Num = -400;
// if(LineNumToggleFlag == 1)
// {
// Line_Num = - Line_Num;
// }
}
//°Ñ»Ò¶È´«¸ÐÆ÷µ±×÷Ö»Êä³ö¸ßµÍµçƽ¡£ ¸ßµçƽÊÇʶ±ðµ½ºìÏßÁË¡£
void Get_Light_TTL(void)
{
if(HAL_GPIO_ReadPin(L2_GPIO_Port, L2_Pin) == GPIO_PIN_SET){L2_Val = 1;} else {L2_Val = 0;}
if(HAL_GPIO_ReadPin(L1_GPIO_Port, L1_Pin) == GPIO_PIN_SET){L1_Val = 1;} else {L1_Val = 0;}
if(HAL_GPIO_ReadPin(M_GPIO_Port, M_Pin) == GPIO_PIN_SET) {M_Val = 1;} else {M_Val = 0;}
if(HAL_GPIO_ReadPin(R1_GPIO_Port, R1_Pin) == GPIO_PIN_SET){R1_Val = 1;} else {R1_Val = 0;}
if(HAL_GPIO_ReadPin(R2_GPIO_Port, R2_Pin) == GPIO_PIN_SET){R2_Val = 1;} else {R2_Val = 0;}
}
/********¼ì²âÊÇ·ñתÔØÒ©Æ·*************/
void LoadOrNot()
{
HAL_ADC_Start(&hadc1);
if(HAL_ADC_PollForConversion(&hadc1, 10) == HAL_OK)
{
ADC_Value = HAL_ADC_GetValue(&hadc1);
ADC_Volt = (float)ADC_Value*3.3/4096.0;
}
if(ADC_Volt <= 1.6)
{
NotLoadCount = 0;
LoadCount++;
if(LoadCount > 3) //3´Î³ÌÐòÖ´Ðе½ÕâÖ®ºóÔÙ´ÎÅжÏ
{
Load_flag = 1;
}
}
else if(ADC_Volt > 1.6)
{
LoadCount = 0;
if(Load_flag == 1) //±ØÐëÏÈ×°ÔعýÒ©Æ·µÄÇé¿öϲÅÄÜÅжÏÊÇ·ñÄÃ×ßÒ©Æ·
{
NotLoadCount++;
if(NotLoadCount > 3 ) //3´Î³ÌÐòÖ´Ðе½ÕâÖ®ºóÔÙ´ÎÅжÏ
{
Load_flag = 2;
}
}
}
}
灰度传感器部bsp_GraySensor.h
#ifndef __GRAYSENSOR_H
#define __GRAYSENSOR_H
#include "bsp_sys.h"
extern u8 L2_Val, L1_Val, M_Val, R1_Val, R2_Val;
extern u16 ADC_Value;
extern float ADC_Volt;
extern u8 Load_flag;
extern int Line_Num;
void LoadOrNot(void);
void Get_Light_TTL(void);
void Light_GoStraight_control(void);
#endif
主函数main.h
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.h
* @brief : Header for main.c file.
* This file contains the common defines of the application.
******************************************************************************
* @attention
*
* <h2><center>© Copyright (c) 2021 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __MAIN_H
#define __MAIN_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f1xx_hal.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants --------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro ------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
/* Exported functions prototypes ---------------------------------------------*/
void Error_Handler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
/* Private defines -----------------------------------------------------------*/
#define L2_Pin GPIO_PIN_13
#define L2_GPIO_Port GPIOC
#define LOAD_Pin GPIO_PIN_1
#define LOAD_GPIO_Port GPIOA
#define L1_Pin GPIO_PIN_4
#define L1_GPIO_Port GPIOC
#define M_Pin GPIO_PIN_6
#define M_GPIO_Port GPIOE
#define BIN1_Pin GPIO_PIN_10
#define BIN1_GPIO_Port GPIOB
#define BIN2_Pin GPIO_PIN_11
#define BIN2_GPIO_Port GPIOB
#define red_sig_Pin GPIO_PIN_15
#define red_sig_GPIO_Port GPIOB
#define R1_Pin GPIO_PIN_5
#define R1_GPIO_Port GPIOE
#define R2_Pin GPIO_PIN_4
#define R2_GPIO_Port GPIOA
#define blue_sig_Pin GPIO_PIN_11
#define blue_sig_GPIO_Port GPIOA
#define yellow_sig_Pin GPIO_PIN_12
#define yellow_sig_GPIO_Port GPIOA
#define AIN1_Pin GPIO_PIN_8
#define AIN1_GPIO_Port GPIOB
#define AIN2_Pin GPIO_PIN_9
#define AIN2_GPIO_Port GPIOB
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
#ifdef __cplusplus
}
#endif
#endif /* __MAIN_H */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
主函数mian.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* <h2><center>© Copyright (c) 2021 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oled.h"
#include "bsp_key.h"
#include "bsp_sys.h"
#include "bsp_delay.h"
#include "control.h"
#include "bsp_led.h"
#include "menu.h"
#include "Fire_protocol.h"
#include "bsp_pid.h"
#include "openmv.h"
#include "bsp_GraySensor.h"
#include "bsp_BlueTooth.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
u8 Do_count = 0, Do2_count = 0;
u16 showOLEDTime = 0;
u16 SendTime = 0;
extern unsigned char BMP3[];
extern unsigned char BMP4[];
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/*ÊÓƵ½Ì³ÌÁ´½Ó https://www.bilibili.com/video/BV1UL411V7XK?p=2&share_source=copy_web êdzƣºº£Î¹Î¹Î¹*/
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
LED_Init();
KEY_GPIO_Init();
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
delay_init(72);
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_ADC1_Init();
MX_TIM3_Init();
MX_TIM4_Init();
MX_TIM6_Init();
MX_USART2_UART_Init();
MX_USART3_UART_Init();
MX_TIM7_Init();
MX_TIM8_Init();
MX_TIM2_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); //¿ªÆôTIM3µÄ±àÂëÆ÷½Ó¿Úģʽ
HAL_TIM_Encoder_Start(&htim8, TIM_CHANNEL_ALL); //¿ªÆôTIM8µÄ±àÂëÆ÷½Ó¿Úģʽ
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3); //Motor 2
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4);
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3); //Motor1
HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
delay_ms(100); //ÑÓʱһÏ£¬ÈÃOLEDÕý³£Êä³ö
OLED_Init();
delay_ms(100); //ÑÓʱһÏ£¬ÈÃOLEDÕý³£Êä³ö
OLED_Clear(); //ÒòΪÊÇÖ±½Ó½øÀ´µÄ£¬ËùÒÔÇå³ýһϱȽϺÃ
PID_param_init();
set_motor2_enable(); //ÉÏλ»úµ÷ÊÔpidʱ²ÅÐèÒªÊÖ¶¯Ê¹Äܵç»úµÄPWMÊä³ö
set_motor_enable();
HAL_TIM_Base_Start_IT(&htim7);
HAL_TIM_Base_Start_IT(&htim6);
HAL_UART_Receive_IT(&huart3,&uart3_rxbuff,1); //openmvͨÐÅ´®¿Ú
HAL_UART_Receive_IT(&huart2,&uart2_rxbuff,1); //À¶ÑÀͨÐÅ´®¿Ú
__HAL_UART_ENABLE_IT(&huart1,UART_IT_RXNE); //´®¿Ú1½ÓÊÕÖжϣ¬Ö»¿ªÆô½ÓÊÕÖжϣ¬ÓëÉÏλ»úͨÐÅ
protocol_init(); //Õâ¸ö³õʼ»¯Á˲ÅÄÜͨ¹ýÉÏλ»ú¿ØÖÆѽ, ÔÙ¿´Õâ¸ö×¢ÒâÊÂÏî
__HAL_GPIO_EXTI_CLEAR_IT(GPIO_PIN_0);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
receiving_process(); //½âÎöÒ°»ðÉÏλ»úͨÐÅÐÒé,µ÷ÊÔµç»úλÖÃËٶȴ®¼¶PIDʱÓÃ
if(SendTime >= 21)
{
SendTime = 0;
SendDataToOpenmv(); //²»ÄÜÌ«¿ì£¬·ñÔò»á³¬¹ýopenmvµÄ½ÓÊÕÊý¾Ý»º³åÇø
}
if(showOLEDTime >= 200)
{
showOLEDTime = 0;
OLED_Display(Menu_Item);
// if(OneLoadFlag == 1)
// {
// BlueSignal_Toggle;
// }
//printf("123");
/****ÅжÏÊÇ·ñÓзÅÒ©Æ·******/
LoadOrNot();
}
/**************µÈ´ýʶ±ð²¡·¿ºÅ**************/
if(TASK == 1)
{
SetTargetRoom();
}
else if(TASK == 2)
{
//¼ÓÉÏ·¢Ë͸øopenmv µÄÊý¾Ý
if(Load_flag == 1) //ÔËËÍÒ©Îï
{
//½ü¶Ë²¡·¿£¬¿¿openmv¹æ»® £¬ ÓÃÀ´²âÊÔ¶þ³µÐÔÄÜ
if(TargetRoom == 'A')
{
switch(Do_count)
{
case 0:
Do_count++;
Car_go(60-5+15+HeadToWheelCM); //´Ëʱ Stop_Flag ==0 Íê³Éºó×Ô¶¯ÖÃ1 //½ü¶Ë²¡·¿²»Ô¤Áôopenmvʶ±ðµÄλÖÃ
break;
case 1:
if(Stop_Flag ==1) //Ö±ÐС¢×ªÍ亯Êý½»ÌæʹÓÃʱ£¬¿ÉÒÔ²»ÊÖ¶¯½«±ê־λÇåÁã
{
Do_count++;
spin_Turn(left_90); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
break;
case 2:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(40-5+15-HeadToWheelCM +3); //Stop_FlagÔÙ´ÎÖÃ1 +3ÊÇΪÁ˽øÍ£³µÇø
}
break;
case 3:
if(Stop_Flag ==1)
{
RedSignal_on;
//µãÁÁºìɫָʾµÆ
}
break;
}
}
else if(TargetRoom == 'B')
{
switch(Do_count)
{
case 0:
Do_count++;
Car_go(60-5+15+HeadToWheelCM); //´Ëʱ Stop_Flag ==0 Íê³Éºó×Ô¶¯ÖÃ1 //½ü¶Ë²¡·¿²»Ô¤Áôopenmvʶ±ðµÄλÖÃ
break;
case 1:
if(Stop_Flag ==1)
{
Do_count++;
spin_Turn(right_90); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
break;
case 2:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(40-5+15-HeadToWheelCM +3); //Stop_FlagÔÙ´ÎÖÃ1 +3ÊÇΪÁ˽øÍ£³µÇø
}
break;
case 3:
if(Stop_Flag ==1)
{
RedSignal_on;
//µãÁÁºìɫָʾµÆ
}
break;
}
}
if(OneLoadFlag == 1)
{
//È¥ÖжËËÍÒ©£¬¿¿Ò»³µµÈ´ýÄ¿±ê·¿ºÅÀ´È·¶¨£¬²»Ó÷µ»Ø
if(OneTargetRoom == 'C')
{
switch(Do_count)
{
case 0:
Do_count++;
Car_go(160+HeadToWheelCM); //´Ëʱ Stop_Flag ==0 Íê³Éºó×Ô¶¯ÖÃ1 //½ü¶Ë²¡·¿²»Ô¤Áôopenmvʶ±ðµÄλÖÃ
break;
case 1:
if(Stop_Flag ==1)
{
Do_count++;
spin_Turn(right_90 ); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
break;
case 2:
if(Spin_succeed_flag == 1) //ÕâÀï¶ãÈëÁíÍâÒ»¸öÖж˲¡·¿Í¨µÀ£¬ ²âÊÔһϣ¬¿´µôͷЧ¹ûºÃ»¹ÊÇÖ±½ÓºóÍËЧ¹ûºÃ
{
Do_count++;
Car_go(HideCM); //Stop_FlagÔÙ´ÎÖÃ1 //Èç¹ûºóÍ˵Ļ°£¬Ñ²Ïß²¹³¥µÄÖµÐèÒªÏà·´
}
break;
case 3:
if(Stop_Flag ==1)
{
YellowSignal_on; //µÈÒ»³µÈ¡Ò©
}
break;
}
}
else if(OneTargetRoom == 'D' )
{
switch(Do_count)
{
case 0:
Do_count++;
Car_go(150-5+15+HeadToWheelCM); //´Ëʱ Stop_Flag ==0 Íê³Éºó×Ô¶¯ÖÃ1 //½ü¶Ë²¡·¿²»Ô¤Áôopenmvʶ±ðµÄλÖÃ
break;
case 1:
if(Stop_Flag ==1)
{
Do_count++;
spin_Turn(left_90); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
break;
case 2:
if(Spin_succeed_flag == 1) //ÕâÀï¶ãÈëÁíÍâÒ»¸öÖж˲¡·¿Í¨µÀ£¬ ²âÊÔһϣ¬¿´µôͷЧ¹ûºÃ»¹ÊÇÖ±½ÓºóÍËЧ¹ûºÃ
{
Do_count++;
Car_go(HideCM); //Stop_FlagÔÙ´ÎÖÃ1 //Èç¹ûºóÍ˵Ļ°£¬Ñ²Ïß²¹³¥µÄÖµÐèÒªÏà·´
}
break;
case 3:
if(Stop_Flag ==1)
{
YellowSignal_on; //µÈÒ»³µÈ¡Ò©
}
break;
}
}
}
}
else if(Load_flag == 2) // Ö»ÔÚ½ü¶Ë²Å·µ»Ø,ÓÃÀ´²âÊÔ¶þ³µ¿ØÖÆЧ¹û
{
if(TargetRoom == 'A')
{
switch(Do2_count)
{
case 0:
Do2_count++;
spin_Turn(back_180 );
RedSignal_off; //¹Ø±ÕºìɫָʾµÆ
break;
case 1:
if(Spin_succeed_flag == 1)
{
Do2_count++;
Car_go(40-5+15-HeadToWheelCM+3); //´Ëʱ Stop_Flag ==0 Íê³Éºó×Ô¶¯ÖÃ1 //½ü¶Ë²¡·¿²»Ô¤Áôopenmvʶ±ðµÄλÖÃ
}
break;
case 2:
if(Stop_Flag ==1)
{
Do2_count++;
spin_Turn(right_90 ); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
break;
case 3:
if(Spin_succeed_flag == 1)
{
Do2_count++;
Car_go(60-5+15-HeadToWheelCM +3 ); //Stop_FlagÔÙ´ÎÖÃ1
}
break;
case 4:
if(Stop_Flag ==1)
{
GreenSignal_on; //µãÁÁÂÌɫָʾµÆ
}
break;
}
}
else if(TargetRoom == 'B')
{
switch(Do2_count)
{
case 0:
Do2_count++;
spin_Turn(back_180 );
RedSignal_off; //¹Ø±ÕºìɫָʾµÆ
break;
case 1:
if(Spin_succeed_flag == 1)
{
Do2_count++;
Car_go(40-5+15-HeadToWheelCM+3); //´Ëʱ Stop_Flag ==0 Íê³Éºó×Ô¶¯ÖÃ1 //½ü¶Ë²¡·¿²»Ô¤Áôopenmvʶ±ðµÄλÖÃ
}
break;
case 2:
if(Stop_Flag ==1)
{
Do2_count++;
spin_Turn(left_90 ); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
break;
case 3:
if(Spin_succeed_flag == 1)
{
Do2_count++;
Car_go(60-5+15-HeadToWheelCM +3); //Stop_FlagÔÙ´ÎÖÃ1
}
break;
case 4:
if(Stop_Flag ==1)
{
GreenSignal_on; //µãÁÁÂÌɫָʾµÆ
}
break;
}
}
}
if(OneLoadFlag == 2)
{
if(OneTargetRoom == 'C' || OneTargetRoom == 'D')
{
switch(Do_count)
{
case 3:
//À´¸öÑÓʱµÄ±ê־λ
WaitFlag = 1;
YellowSignal_off;
if(WaitTimeCount >= 6000) //ÑÓʱ6s
{
Do_count++;
WaitFlag = 0;
WaitTimeCount = 0;
spin_Turn(back_180);
}
break;
case 4:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(HideCM + 40-5+15-HeadToWheelCM +3);
}
break;
case 5:
if(Stop_Flag == 1)
{
Do_count++;
RedSignal_on;
}
break;
}
}
//È¥Ô¶¶ËÈ¡Ò©£¬²»ÓùÜÊÇ·ñ×°ÔØÒ©Æ·£¬ÏÈÔÚÖж˵ÈÒ»³µ¹ýÁËÔÙÈ¥Ô¶¶Ë¡£
if(OneTargetRoom == 'E' || OneTargetRoom == 'F' || OneTargetRoom == 'G' ||OneTargetRoom == 'H' ) // ÓÉÒ»³µ·¢Ë͵ıê־λ¿ØÖƶþ³µÆô¶¯
{
switch(Do_count)
{
case 0:
Do_count++;
Car_go(150-5+15+HeadToWheelCM-2); //´Ëʱ Stop_Flag ==0 Íê³Éºó×Ô¶¯ÖÃ1 //½ü¶Ë²¡·¿²»Ô¤Áôopenmvʶ±ðµÄλÖÃ
break;
case 1:
if(Stop_Flag ==1)
{
if(OneTargetRoom == 'E' || OneTargetRoom == 'F')
{
Do_count++;
spin_Turn(right_90); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
else if(OneTargetRoom == 'G' ||OneTargetRoom == 'H')
{
Do_count++;
spin_Turn(left_90); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
}
break;
case 2:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(HideCM);
}
break;
case 3:
if(Stop_Flag == 1)
{
WaitFlag = 1;
Do_count++;
}
break;
case 4:
if(WaitTimeCount >= 3000 ) //µÈ´ý3s
{
Do_count++;
WaitTimeCount = 0;
WaitFlag = 0;
spin_Turn(back_180);
}
break;
case 5:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(HideCM );
}
break;
case 6:
if(Stop_Flag ==1)
{
if(OneTargetRoom == 'E' || OneTargetRoom == 'F')
{
Do_count++;
spin_Turn(right_90 ); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
else if(OneTargetRoom == 'G' ||OneTargetRoom == 'H')
{
Do_count++;
spin_Turn(left_90 ); //´ËʱSpin_succeed_flag== 0 ,Íê³Éºó×Ô¶¯ÖÃ1
}
}
break;
case 7:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(90- (LookCM + HeadToWheelCM) );
}
break;
case 8:
if(Stop_Flag == 1 )
{
FindStartFlag = 1;
if(FindTimeCount < 500) //Ò»¶¨Ê±¼äÄÚʶ±ðµ½RoomNum, ÇÒÆäλÖÃÆ«×ó Èý¸öÌõ¼þ¶¼ÒªÂú×㣺ʱ¼ä¡¢Ê¶±ð³É¹¦±ê־λ¡¢Î»ÖÃ
{
if(RoomNum == TargetNum && LoR == 1)
{
Do_count++;
TargetRoom = 'E'; //Ïȼٶ¨ÊÇE
Car_go(LookCM+HeadToWheelCM); //²¹»Ø¸øopenmvʶ±ðÔ¤ÁôµÄλÖã¬Í¬Ê±ÈóµÂÖѹÏߣ¬ÒÔ±ãºóÐøתÍä
//Í£Ö¹¼Æʱ,²¢ÇåÁã
FindTimeCount = 0;
FindStartFlag = 0;
}
else if(RoomNum == TargetNum && LoR == 2 ) //ʶ±ðµ½Ä¿±êÊý×Ö, ÇÒÆäλÖÃÆ«ÓÒ
{
Do_count++;
TargetRoom = 'G'; //Ïȼٶ¨ÊÇG
Car_go(LookCM + HeadToWheelCM); //²¹»Ø¸øopenmvʶ±ðÔ¤ÁôµÄλÖã¬Í¬Ê±ÈóµÂÖѹÏߣ¬ÒÔ±ãºóÐøתÍä
//Í£Ö¹¼Æʱ,²¢ÇåÁã
FindTimeCount = 0;
FindStartFlag = 0;
}
}
else if(FindTimeCount >= 500) //ÈÔÈ»³¬Ê±²¢Î´Ê¶±ðµ½£¬Ôò±£Áô³õʼ»¯ÊǸ³ÓèµÄ'E'
{
Do_count++;
Car_go(LookCM + HeadToWheelCM); //²¹»Ø¸øopenmvʶ±ðÔ¤ÁôµÄλÖã¬Í¬Ê±ÈóµÂÖѹÏߣ¬ÒÔ±ãºóÐøתÍä
//Í£Ö¹¼Æʱ,²¢ÇåÁã
FindTimeCount = 0;
FindStartFlag = 0;
}
}
break;
case 9:
if(Stop_Flag == 1)
{
Do_count++;
if(TargetRoom == 'E')
{
spin_Turn(left_90);
}
else if(TargetRoom == 'G')
{
spin_Turn(right_90);
}
}
break;
case 10:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(90 -HeadToWheelCM -LookCM);
}
break;
case 11:
if(Stop_Flag == 1)
{
FindStartFlag = 1;
if(FindTimeCount < 500)
{
if(TargetRoom == 'E')
{
if(RoomNum == TargetNum && LoR == 1) //ʶ±ðµ½RoomNum, ÇÒÆäλÖÃÆ«×ó
{
Do_count ++;
TargetRoom = 'E';
Car_go(LookCM+HeadToWheelCM); //²¹»Ø¸øopenmvʶ±ðÔ¤ÁôµÄλÖã¬Í¬Ê±ÈóµÂÖѹÏߣ¬ÒÔ±ãºóÐøתÍä
FindStartFlag = 0;
FindTimeCount =0;
}
else if(RoomNum == TargetNum && LoR == 2) //Ò»¶¨Ê±¼äÄÚʶ±ðµ½RoomNum, ÇÒÆäλÖÃÆ«ÓÒ
{
Do_count++;
TargetRoom = 'F';
Car_go(LookCM + HeadToWheelCM);
FindStartFlag = 0;
FindTimeCount =0;
}
}
else if(TargetRoom == 'G')
{
if(RoomNum == TargetNum && LoR == 1) //Ò»¶¨Ê±¼äÄÚʶ±ðµ½RoomNum, ÇÒÆäλÖÃÆ«×ó
{
Do_count++;
TargetRoom = 'G';
Car_go(LookCM+HeadToWheelCM);
FindStartFlag = 0;
FindTimeCount =0;
}
else if(RoomNum == TargetNum && LoR == 2) //Ò»¶¨Ê±¼äÄÚʶ±ðµ½RoomNum, ÇÒÆäλÖÃÆ«ÓÒ
{
Do_count++;
TargetRoom = 'H';
Car_go(LookCM + HeadToWheelCM);
FindStartFlag = 0;
FindTimeCount =0;
}
}
}
else if(FindTimeCount >= 500)
{
Do_count++;
Car_go(LookCM + HeadToWheelCM);
FindStartFlag=0;
FindTimeCount=0;
}
}
break;
case 12:
if(Stop_Flag == 1)
{
Do_count++;
if( TargetRoom == 'E' || TargetRoom == 'G')
{
spin_Turn(left_90);
}
else if( TargetRoom == 'F' || TargetRoom == 'H')
{
spin_Turn(right_90);
}
}
break;
case 13:
if(Spin_succeed_flag == 1)
{
Do_count++;
Car_go(40-5+15-HeadToWheelCM +3);
}
break;
case 14:
if(Stop_Flag == 1)
{
RedSignal_on;
//µãÁÁºìɫָʾµÆ
}
break;
}
}
}
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC;
PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
365
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
