Main Line STM
Main Line STM
/**
this avsdmu
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
int last_err,output,err_sum;
float p = 0.5; //0.215
float i =0;
float d = 0; /// 0.1
uint8_t run_ =0;
volatile uint16_t left_motor;
volatile uint16_t right_motor;
uint8_t first_line=0;
volatile int pos_x=0;
void get_adc_value()
{
uint32_t start_ = HAL_GetTick();
/*
ADC_Select_CH1();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[0] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH2();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[1] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH8();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[7] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
*/
ADC_Select_CH3();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[2] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH4();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[3] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH5();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[4] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH6();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[5] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH7();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[6] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
time_process=HAL_GetTick()-start_;
}
void get_adc_value2()
{
uint32_t start_ = HAL_GetTick();
ADC_Select_CH0();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[0] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH1();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[1] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH2();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[2] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH3();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[3] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH4();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[4] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH5();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[5] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
ADC_Select_CH6();
HAL_ADC_Start(&hadc1);
HAL_ADC_PollForConversion(&hadc1, 1000);
ADC_VAL[6] = HAL_ADC_GetValue(&hadc1);
HAL_ADC_Stop(&hadc1);
time_process=HAL_GetTick()-start_;
}
map_adc[i]=map(ADC_VAL[i],min_val[i],max_val[i],0,KS);
map_adc[i]=constrain(map_adc[i],0,KS); /// input --> 0-> 7000 ---> 0-
4095 10 bit ADC stm32 3700 1000 --> 4000 500 --> 0-1000 230->1000 --> <50
if(map_adc[i]>230)get_line=1;
if(map_adc[i]>45)
{
avgs+=(long)map_adc[i]*(i*KS); /// lech trai hay lech phai -->
3500 < 3500 -> lech phai hoac trai -> > 3500 -> nguoc lai
sums+=map_adc[i];
}
}
if(get_line)pos_x=avgs/sums; // posx -> vi tri --> neu ma dang ben trai ->
quay sang phai
else if(pos_x<pos_m)pos_x=0; // --> PID -> input 3500 output 0-7000 ->
>3500 --> lech trai <3500 -> lech phai
else pos_x=pos_m*2;
}
}
void move_robot_2()
{
if(thresh_hold[3] != 0)
{
uint8_t get_line=0;
get_adc_value2();
avgs=0;
sums=0;
uint8_t total_line=0;
for (uint8_t i = 0 ;i<7;i++)
{
/*
uint16_t denominator = max_val[i] - min_val[i];
if (denominator != 0)
{
ADC_VAL[i] = (((int32_t)ADC_VAL[i]) - min_val[i]) * 1000 / denominator;
}
*/
if(ADC_VAL[i]>thresh_hold[i])total_line+=1; //
map_adc[i]=map(ADC_VAL[i],min_val[i],max_val[i],0,KS);
map_adc[i]=constrain(map_adc[i],0,KS); /// input --> 0-> 7000 ---> 0-
4095 10 bit ADC stm32 3700 1000 --> 4000 500 --> 0-1000 230->1000 --> <50
if(map_adc[i]>230)get_line=1;
if(map_adc[i]>45)
{
avgs+=(long)map_adc[i]*(i*KS); /// lech trai hay lech phai -->
3500 < 3500 -> lech phai hoac trai -> > 3500 -> nguoc lai
sums+=map_adc[i];
}
}
/*
if(total_line>=5 && first_line==0)
{
left_motor=0;
right_motor=0;
motor1_act(0);
motor2_act(0);
HAL_Delay(10000);
}
*/
if(get_line)pos_x=avgs/sums; // posx -> vi tri --> neu ma dang ben trai ->
quay sang phai
else if(pos_x<pos_m)pos_x=0; // --> PID -> input 3500 output 0-7000 ->
>3500 --> lech trai <3500 -> lech phai
else pos_x=pos_m*2;
int err_now =pos_x-pos_m; // >3500 -> right > left --> right+=output >3500 ->
4000 --> 3500 --> 100
static int sum_ ;
sum_+=err_now;
sum_=constrain(sum_,-500,500); //
output=p*err_now+d*(err_now-last_err)+i*sum_; // 400 --> lech trai --> quay
phai --> toc do banh trai > toc do banh phai
last_err=err_now; /// --> 18 --> toc do banh trai +18 -> toc do banh phai -18
motor_act_2();
}
}
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) {
get_adc_now=1;
//move_robot();
}
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
//servo(10);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
//motor1_act(400);
// motor2_act(700);
//get_adc_value();
//motor_test();
move_robot_2();
//motor_test();
// //servo(10);
// hcsr_test();
}
/* 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};
/**
* @brief ADC1 Initialization Function
* @param None
* @retval None
*/
static void MX_ADC1_Init(void)
{
/**
* @brief TIM3 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM3_Init(void)
{
/**
* @brief TIM4 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM4_Init(void)
{
/**
* Enable DMA controller clock
*/
static void MX_DMA_Init(void)
{
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/**
* @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 */