|
STM32F103RCT6PID算法控制四個(gè)電機(jī),基于HAL,希望可以給各位一些幫助,僅供參考,請(qǐng)勿他用
單片機(jī)源程序如下:
- /* USER CODE END Header */
- /* Includes ------------------------------------------------------------------*/
- #include "main.h"
- #include "dma.h"
- #include "tim.h"
- #include "usart.h"
- #include "gpio.h"
- /* Private includes ----------------------------------------------------------*/
- /* USER CODE BEGIN Includes */
- #include "string.h"
- #include "Motr_conter.h"
- #include "pid.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 */
- /* 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 */
-
- uint8_t Left_Front_or_Right_rear; //用于保存電機(jī)行駛方向的變量
- uint8_t RX_buff[100],Start_Flag=0; //接收緩沖區(qū),和電機(jī)運(yùn)行開始的標(biāo)志位
- int16_t pwm_duth_A=0,pwm_duth_B=0,pwm_duth_C=0,pwm_duth_D=0;//四個(gè)電機(jī)的pwm變量
- uint16_t save_Ecoder_A=0,save_Ecoder_B=0,save_Ecoder_C=0,save_Ecoder_D=0; //保存四個(gè)電機(jī)的編碼值
- extern PID sPID; //PID結(jié)構(gòu)體變量,用于改變目標(biāo)值
- /* USER CODE END 0 */
- /**
- * @brief The application entry point.
- * @retval int
- */
- int main(void)
- {
- /* USER CODE BEGIN 1 */
- uint8_t TX_buff[500]={"今天是個(gè)好日子,天氣有點(diǎn)涼快!\n"};
- /* USER CODE END 1 */
-
- /* MCU Configuration--------------------------------------------------------*/
- /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
- HAL_Init();
- /* USER CODE BEGIN Init */
- /* USER CODE END Init */
- /* Configure the system clock */
- SystemClock_Config();
- /* USER CODE BEGIN SysInit */
- /* USER CODE END SysInit */
- /* Initialize all configured peripherals */
- MX_GPIO_Init(); //GPIO的初始化
- MX_DMA_Init(); //DMA的初始化
- MX_TIM2_Init(); //定時(shí)器2的初始化
- MX_USART1_UART_Init(); //串口1的初始化
- MX_TIM1_Init(); //定時(shí)器1的初始化
- MX_TIM6_Init(); //定時(shí)器6的初始化
- MX_USART2_UART_Init(); //串口2的初始化
- MX_TIM3_Init(); //定時(shí)器3的初始化
- MX_TIM4_Init(); //定時(shí)器4的初始化
- MX_TIM5_Init(); //定時(shí)器5的初始化
- MX_TIM8_Init(); //定時(shí)器8的初始化
- /* USER CODE BEGIN 2 */
- Motor_Stop; //電機(jī)初始化狀態(tài)位停止的
- HAL_TIM_Base_Start_IT(&htim6); //啟動(dòng)定時(shí)器
- HAL_UART_Transmit(&huart2,TX_buff,strlen((char*)TX_buff),1000); //串口2發(fā)送一串?dāng)?shù)據(jù),表示串口2工作正常
- HAL_UART_Transmit(&huart1,TX_buff,strlen((char*)TX_buff),100); //串口發(fā)送數(shù)據(jù)測(cè)試 ,表示串口1工作正常
- printf("電機(jī)編碼器測(cè)試,測(cè)試通過(guò)\n"); //發(fā)送實(shí)驗(yàn)測(cè)試名稱
- IncPIDInit(); //pPID參數(shù)初始化
- HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL); //開始電機(jī)D編碼器接口計(jì)數(shù)
- HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL); //開啟電機(jī)C的編碼器接口計(jì)數(shù)
- HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL); //開啟電機(jī)B的編碼器接口計(jì)數(shù)
- HAL_TIM_Encoder_Start(&htim5,TIM_CHANNEL_ALL); //開啟電機(jī)C的編碼器接口計(jì)數(shù)
- HAL_UART_Receive_DMA(&huart2,RX_buff,5); //開啟串口2的DMA接收數(shù)據(jù)
- /* USER CODE END 2 */
- /* Infinite loop */
- /* USER CODE BEGIN WHILE */
- while (1)
- {
- /* USER CODE END WHILE */
- /* USER CODE BEGIN 3 */
- if(RX_buff[0]==0x31)
- {
- if(RX_buff[1]==0x31&&RX_buff[2]==0x31&&RX_buff[3]==0x31) Derection_Control(1,3,1,3); //左前行駛
- }
- if(RX_buff[0]==0x32)
- {
- if(RX_buff[1]==0x32&&RX_buff[2]==0x32&&RX_buff[3]==0x32) Derection_Control(1,1,1,1); //向前行駛
- }
- if(RX_buff[0]==0x33)
- {
- if(RX_buff[1]==0x33&&RX_buff[2]==0x33&&RX_buff[3]==0x33) Derection_Control(3,1,3,1); //右前行駛
- }
- if(RX_buff[0]==0x34)
- {
- if(RX_buff[1]==0x34&&RX_buff[2]==0x34&&RX_buff[3]==0x34) Derection_Control(1,2,1,2); //向左平行行駛
- }
- if(RX_buff[0]==0x35)
- {
- if(RX_buff[1]==0x35&&RX_buff[2]==0x35&&RX_buff[3]==0x35) Derection_Control(1,2,2,1); //順時(shí)針旋轉(zhuǎn)
- }
- if(RX_buff[0]==0x36)
- {
- if(RX_buff[1]==0x36&&RX_buff[2]==0x36&&RX_buff[3]==0x36) Derection_Control(2,1,2,1); //向右平行行駛
- }
- if(RX_buff[0]==0x37)
- {
- if(RX_buff[1]==0x37&&RX_buff[2]==0x37&&RX_buff[3]==0x37) Derection_Control(3,2,3,2); //左后行駛
- }
- if(RX_buff[0]==0x38)
- {
- if(RX_buff[1]==0x38&&RX_buff[2]==0x38&&RX_buff[3]==0x38) Derection_Control(2,2,2,2); //向后行駛
- }
- if(RX_buff[0]==0x39)
- {
- if(RX_buff[1]==0x39&&RX_buff[2]==0x39&&RX_buff[3]==0x39) Derection_Control(2,3,2,3); //右后行駛
- }
- if(RX_buff[0]==0x30)
- {
- if(RX_buff[1]==0x30&&RX_buff[2]==0x30&&RX_buff[3]==0x30) Derection_Control(2,1,1,2); //逆時(shí)針旋轉(zhuǎn)
- }
- if(RX_buff[0]==0x2E)
- {
- if(RX_buff[1]==0x31&&RX_buff[2]==0x32&&RX_buff[3]==0x33) {Derection_Control(3,3,3,3); } //停止
- }
- if(RX_buff[0]==0x2B)
- {
- sPID.SetPoint_A=sPID.SetPoint_B=sPID.SetPoint_C=sPID.SetPoint_D=(RX_buff[1]-0x30)*1000+(RX_buff[2]-0x30)*100+(RX_buff[3]-0x30)*10+(RX_buff[4]-0x30); //設(shè)置行駛的速度
- }
- }
- /* USER CODE END 3 */
- }
- /**
- * @brief System Clock Configuration
- * @retval None
- */
- void SystemClock_Config(void)
- {
- RCC_OscInitTypeDef RCC_OscInitStruct = {0};
- RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
- /** Initializes the CPU, AHB and APB busses clocks
- */
- 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 busses 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();
- }
- /** Enables the Clock Security System
- */
- HAL_RCC_EnableCSS();
- }
- /* USER CODE BEGIN 4 */
- uint8_t direction_A,direction_B,direction_C,direction_D; //保存四個(gè)電機(jī)方向的變量
- uint32_t Code_val_A=0,Code_val_B=0,Code_val_C=0,Code_val_D=0; //保存四個(gè)編碼器計(jì)數(shù)器的的值
- uint16_t last_Code_val_A=0,last_Code_val_B=0,last_Code_val_C=0,last_Code_val_D=0;//保存上一秒計(jì)數(shù)器的值,與下一秒的相比較到底計(jì)數(shù)了多少次
- void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
- {
-
- if(htim->Instance==TIM6) //判斷是不是定時(shí)器6達(dá)到更新事件
- {
- if(Start_Flag==1) //判斷電機(jī)的行駛標(biāo)志位
- {
- direction_A=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim5);//獲取電機(jī)A轉(zhuǎn)動(dòng)的方向
- direction_B=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim4);//獲取電機(jī)B 的旋轉(zhuǎn)方向
- direction_C=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim3);//獲取電機(jī)C 的旋轉(zhuǎn)方向
- direction_D=__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim2);//獲取電機(jī)D的旋轉(zhuǎn)方向
- Code_val_A=__HAL_TIM_GET_COUNTER(&htim5); //讀取電機(jī)A的計(jì)數(shù)器的值
- Code_val_B=__HAL_TIM_GET_COUNTER(&htim4); //讀取電機(jī)B的計(jì)數(shù)器的值
- Code_val_C=__HAL_TIM_GET_COUNTER(&htim3); //讀取電機(jī)C的計(jì)數(shù)器的值
- Code_val_D=__HAL_TIM_GET_COUNTER(&htim2); //讀取電機(jī)D的計(jì)數(shù)器的值
- if(direction_A==0) save_Ecoder_A=(Code_val_A>=last_Code_val_A?Code_val_A-last_Code_val_A:Code_val_A+65535-last_Code_val_A); //方向?yàn)檎〞r(shí)器向上計(jì)數(shù)
- if(direction_A==1) save_Ecoder_A=(Code_val_A>last_Code_val_A?last_Code_val_A+65535-Code_val_A:last_Code_val_A-Code_val_A); //方向?yàn)榉崔D(zhuǎn)代表計(jì)數(shù)器是向下計(jì)數(shù)
- if(direction_B==0) save_Ecoder_B=(Code_val_B>=last_Code_val_B?Code_val_B-last_Code_val_B:Code_val_B+65535-last_Code_val_B); //方向?yàn)檎〞r(shí)器向上計(jì)數(shù)
- if(direction_B==1) save_Ecoder_B=(Code_val_B>last_Code_val_B?last_Code_val_B+65535-Code_val_B:last_Code_val_B-Code_val_B); //方向?yàn)榉崔D(zhuǎn)代表計(jì)數(shù)器是向下計(jì)數(shù)
- if(direction_C==0) save_Ecoder_C=(Code_val_C>=last_Code_val_C?Code_val_C-last_Code_val_C:Code_val_C+65535-last_Code_val_C); //方向?yàn)檎〞r(shí)器向上計(jì)數(shù)
- if(direction_C==1) save_Ecoder_C=(Code_val_C>last_Code_val_C?last_Code_val_C+65535-Code_val_C:last_Code_val_C-Code_val_C); //方向?yàn)榉崔D(zhuǎn)代表計(jì)數(shù)器是向下計(jì)數(shù)
- if(direction_D==0) save_Ecoder_D=(Code_val_D>=last_Code_val_D?Code_val_D-last_Code_val_D:Code_val_D+65535-last_Code_val_D); //方向?yàn)檎〞r(shí)器向上計(jì)數(shù)
- if(direction_D==1) save_Ecoder_D=(Code_val_D>last_Code_val_D?last_Code_val_D+65535-Code_val_D:last_Code_val_D-Code_val_D); //方向?yàn)榉崔D(zhuǎn)代表計(jì)數(shù)器是向下計(jì)數(shù)
- if(Left_Front_or_Right_rear==0) //如果小車是按照前后左右的某一個(gè)方向行駛,將ABCD四個(gè)電機(jī)進(jìn)行PID控制
- {
- pwm_duth_A+=(IncPIDCalc_C(save_Ecoder_A)); //電機(jī)A的PID算法值返回
- pwm_duth_B+=(IncPIDCalc_B(save_Ecoder_B)); //電機(jī)B的PID算法值返回
- pwm_duth_C+=(IncPIDCalc_C(save_Ecoder_C)); //電機(jī)C的PID算法值返回
- pwm_duth_D+=(IncPIDCalc_D(save_Ecoder_D)); //電機(jī)D的PID算法值返回
- }
- if(Left_Front_or_Right_rear==1) //電機(jī)向左前或者右后行駛
- {
- pwm_duth_A+=(IncPIDCalc_C(save_Ecoder_A)); //就只進(jìn)行電機(jī)AC的PID算法,不進(jìn)行電機(jī)BD的算法
- pwm_duth_C+=(IncPIDCalc_C(save_Ecoder_C)); //就只進(jìn)行電機(jī)AC的PID算法,不進(jìn)行電機(jī)BD的算法
- }
- if(Left_Front_or_Right_rear==2) //電機(jī)向右前或者左后行駛
- {
- pwm_duth_B+=(IncPIDCalc_B(save_Ecoder_B)); //就只進(jìn)行電機(jī)BD的PID算法,不進(jìn)行電機(jī)AC的PID算法
- pwm_duth_D+=(IncPIDCalc_D(save_Ecoder_D)); //就只進(jìn)行電機(jī)BD的PID算法,不進(jìn)行電機(jī)AC的PID算法
- }
- if(pwm_duth_A<0) pwm_duth_A=-pwm_duth_A; //處理電機(jī)A,當(dāng)PWM值為負(fù)時(shí)變?yōu)檎?br />
- if(pwm_duth_B<0) pwm_duth_B=-pwm_duth_B; //處理電機(jī)B,當(dāng)PWM值為負(fù)時(shí)變?yōu)檎?br />
- if(pwm_duth_C<0) pwm_duth_C=-pwm_duth_C; //處理電機(jī)C,當(dāng)PWM值為負(fù)時(shí)變?yōu)檎?nbsp;
- if(pwm_duth_D<0) pwm_duth_D=-pwm_duth_D; //處理電機(jī)D,當(dāng)PWM值為負(fù)時(shí)變?yōu)檎?br />
- if(pwm_duth_A>=7199) pwm_duth_A=7200*4/6; //限制電機(jī)A的速度為最大占空比的百分之六十六
- if(pwm_duth_B>=7199) pwm_duth_B=7200*4/6; //限制電機(jī)B的速度為最大占空比的百分之六十六
- if(pwm_duth_C>=7199) pwm_duth_C=7200*4/6; //限制電機(jī)C的速度為最大占空比的百分之六十六
- if(pwm_duth_D>=7199) pwm_duth_D=7200*4/6; //限制電機(jī)D的速度為最大占空比的百分之六十六
- Set_compare(motor_A,pwm_duth_A); //將電機(jī)A經(jīng)過(guò)PID算法之后得到的PWM賦給電機(jī),讓電機(jī)真正的按照目標(biāo)運(yùn)行起來(lái)
- Set_compare(motor_B,pwm_duth_B); //將電機(jī)B經(jīng)過(guò)PID算法之后得到的PWM賦給電機(jī),讓電機(jī)真正的按照目標(biāo)運(yùn)行起來(lái)
- Set_compare(motor_C,pwm_duth_C); //將電機(jī)C經(jīng)過(guò)PID算法之后得到的PWM賦給電機(jī),讓電機(jī)真正的按照目標(biāo)運(yùn)行起來(lái)
- Set_compare(motor_D,pwm_duth_D); //將電機(jī)D經(jīng)過(guò)PID算法之后得到的PWM賦給電機(jī),讓電機(jī)真正的按照目標(biāo)運(yùn)行起來(lái)
- last_Code_val_A=Code_val_A; //保存電機(jī)A的編碼值,便于進(jìn)行脈沖值的計(jì)算
- last_Code_val_B=Code_val_B; //保存電機(jī)B的編碼值,便于進(jìn)行脈沖值的計(jì)算
- last_Code_val_C=Code_val_C; //保存電機(jī)C的編碼值,便于進(jìn)行脈沖值的計(jì)算
- last_Code_val_D=Code_val_D; //保存電機(jī)D的編碼值,便于進(jìn)行脈沖值的計(jì)算
- }
- }
- }
- void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
- {
- if(huart->Instance==USART1) //判斷是不是串口1接收完成
- {
- // HAL_UART_Receive_IT(&huart1,RX_buff,8);
- }
- if(huart->Instance==USART2) //判斷是否是串口2接收完成
- {
- HAL_UART_Transmit_DMA(&huart2,RX_buff,5); //DMA發(fā)送數(shù)據(jù)
- }
- }
- void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) //發(fā)送完成回調(diào)函數(shù)
- {
- if(huart->Instance==USART2)
- {
- HAL_UART_Receive_DMA(&huart2,RX_buff,5); //使能DMA接收數(shù)據(jù)
- }
- }
- /* 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 */
- /* 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,
- tex: 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****/
復(fù)制代碼
所有資料51hei提供下載:
藍(lán)牙+PID控制的小車.7z
(264.82 KB, 下載次數(shù): 91)
2019-7-30 23:27 上傳
點(diǎn)擊文件名下載附件
基于HAL庫(kù)的 下載積分: 黑幣 -5
|
|