devbox
繁依Fanyi0
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

openmv识别红色物体并返回坐标给stm32单片机，通过pid控制舵机云台_舵机云台用什么控制

作者：繁依Fanyi0 | 2024-05-15 16:37:06

赞

踩

舵机云台用什么控制

本人搜索了有关于舵机云台pid控制的代码，但是都没有搜到想要的结果，现在自己写出来了代码，所以就将自己写的代码分享出来，和大家一起学习进步。

1.openmv识别红色物体+返回中心坐标的的代码

  1. import sensor,image,time,math,pyb,lcd
  2. from pyb import UART,LED
  3. import json
  4. import ustruct
  5. lcd.init()
  6. sensor.reset()
  7. sensor.set_pixformat(sensor.RGB565)
  8. sensor.set_framesize(sensor.QQVGA)#QQVGA的分辨率为120*160
  9. sensor.skip_frames(time = 2000)
  10. sensor.set_auto_gain(False) # 关闭自动增益
  11. sensor.set_auto_whitebal(False) #关闭白平衡
  12. sensor.set_vflip(True)#垂直方向翻转
  13. red_threshold_01=(18, 56, 103, 35, -57, 116)
  14. clock = time.clock()
  15.  
  16. uart = UART(3,9600)   #定义串口3变量
  17. uart.init(9600, bits=8, parity=None, stop=1) # init with given parameters
  18.  
  19. def find_max(blobs):    #定义寻找色块面积最大的函数
  20.     max_size=0
  21.     for blob in blobs:
  22.         if blob.pixels() > max_size:
  23.             max_blob = blob
  24.             max_size = blob.pixels()
  25.     return max_blob
  26.  
  27.  
  28. def sending_data(cx,cy,cw,ch):
  29.     global uart;
  30.     data = ustruct.pack("<bbhhhhb",      #格式为俩个字符俩个短整型(2字节)
  31.                    0x2C,                      #帧头1
  32.                    0x12,                      #帧头2
  33.                    int(cx), # up sample by 4   #数据1
  34.                    int(cy), # up sample by 4    #数据2
  35.                    int(cw), # up sample by 4    #数据1
  36.                    int(ch), # up sample by 4    #数据2
  37.                    0x5B)
  38.     uart.write(data);   #必须要传入一个字节数组
  39.  
  40. led = pyb.LED(3)
  41. while(True):
  42.     led.on()
  43.     clock.tick()
  44.     img = sensor.snapshot()
  45.     blobs = img.find_blobs([red_threshold_01])
  46.     if blobs:
  47.         max_b = find_max(blobs)
  48.         cx=max_b[5]
  49.         cy=max_b[6]
  50.         cw=max_b[2]
  51.         ch=max_b[3]
  52.         img.draw_rectangle(max_b.rect(),color=(255,0,0)) # rect
  53.         img.draw_cross(max_b[5], max_b[6]) # cx, cy
  54.         FH = bytearray([0x2C,0x12,int(cx),int(cy),int(cw),int(ch),0x5B])
  55.         uart.write(FH)
  56.         print(cx,cy,cw,ch)
  57.         lcd.display(img)
  58.     else:
  59.         FH = bytearray([0x2C,0x12,0,0,0,0,0x5B])
  60.         uart.write(FH)
  61.         img.draw_string(0,0,"no object",color = (120,0,0))
  62.         lcd.display(img)
  63.  
  64.

注意：

（1）sensor.set_framesize(sensor.QQVGA)#QQVGA的分辨率为120*160，这行代码是设置分辨率为120*160,QQVGA的分辨率为120*160，QVGA的分辨率为320*240，不同的分辨率对应stm32端的代码也不同，在追踪云台的代码里，分辨率不能设的太高，太高的话，虽然画质会更加清晰，但是画面会出现延时，响应会变慢。

（2）sensor.set_vflip(True)#垂直方向翻转，这行代码是将openmv传回的画面垂直翻转，因为我们的openmv是倒置放的，如果不进行垂直翻转，在电脑端显示的画面就是颠倒的。这行代码是否书写也会影响到后面stm32端代码的书写。

2.stm32通过pid控制舵机云台的代码

（1）usart2.c代码

  1. #include "usart2.h"
  2. #include "usart.h"
  3. #include "led.h"
  4.  int  Cx,Cy,Cw,Ch;
  5. void uart2_init(u32 bound)
  6. 	{
  7.   
  8.     GPIO_InitTypeDef GPIO_InitStructure;
  9. 	USART_InitTypeDef USART_InitStructure;
  10. 	NVIC_InitTypeDef NVIC_InitStructure;
  11. 	
  12. 	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE); 
  13. 	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);
  14.  
  15. 	
  16. 	GPIO_PinAFConfig(GPIOA,GPIO_PinSource2,GPIO_AF_USART2); 
  17. 	GPIO_PinAFConfig(GPIOA,GPIO_PinSource3,GPIO_AF_USART2); 
  18. 	
  19. 	
  20.     GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; 
  21. 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  22. 	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;	
  23. 	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 
  24. 	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
  25. 	GPIO_Init(GPIOA,&GPIO_InitStructure); 
  26.  
  27.  
  28. 	USART_InitStructure.USART_BaudRate = bound;
  29. 	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  30. 	USART_InitStructure.USART_StopBits = USART_StopBits_1;
  31. 	USART_InitStructure.USART_Parity = USART_Parity_No;
  32. 	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  33.  
  34. 	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	
  35.     USART_Init(USART2, &USART_InitStructure); 
  36. 	
  37.     USART_Cmd(USART2, ENABLE); 
  38. 	
  39. 	USART_ClearFlag(USART2, USART_FLAG_TC);
  40. 	
  41. 	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
  42.  
  43. 	
  44.     NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
  45. 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;
  46. 	NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;		
  47. 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			
  48. 	NVIC_Init(&NVIC_InitStructure);	
  49.  
  50.  
  51. 	
  52. }
  53.  
  54.  
  55.  
  56.  
  57. void USART2_IRQHandler(void)
  58. {
  59.  
  60. u8 com_data; 
  61. 		u8 i;
  62. 		static u8 RxCounter1=0;
  63. 		static u16 RxBuffer1[10]={0};
  64. 		static u8 RxState = 0;	
  65. 		static u8 RxFlag1 = 0;
  66.  
  67. 		if( USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET)
  68. 		{
  69. 				USART_ClearITPendingBit(USART2,USART_IT_RXNE);  
  70. 				com_data = USART_ReceiveData(USART2);
  71. 			
  72. 				if(RxState==0&&com_data==0x2C)  
  73. 				{
  74. 					RxState=1;
  75. 					RxBuffer1[RxCounter1++]=com_data;
  76. 				}
  77. 		
  78. 				else if(RxState==1&&com_data==0x12) 
  79. 				{
  80. 					RxState=2;
  81. 					RxBuffer1[RxCounter1++]=com_data;
  82. 				}
  83. 		
  84. 				else if(RxState==2)
  85. 				{
  86. 					RxBuffer1[RxCounter1++]=com_data;
  87.  
  88. 					if(RxCounter1>=10||com_data == 0x5B)     
  89. 					{
  90. 						RxState=3;
  91. 						RxFlag1=1;
  92. 						Cx=RxBuffer1[RxCounter1-5];
  93. 						Cy=RxBuffer1[RxCounter1-4];
  94. 						Cw=RxBuffer1[RxCounter1-3];
  95. 						Ch=RxBuffer1[RxCounter1-2];
  96.             
  97. 					}
  98. 				}
  99. 		
  100. 				else if(RxState==3)		
  101. 				{
  102. 						if(RxBuffer1[RxCounter1-1] == 0x5B)
  103. 						{
  104. 									USART_ITConfig(USART2,USART_IT_RXNE,DISABLE);
  105. 									if(RxFlag1)
  106. 									{
  107. 									
  108. 								 
  109.                                //printf("Cx=%d\r,Cy=%d\r,Cw=%d\r,Ch=%d\r\n",Cx,Cy,Cw,Ch);
  110. 							   //printf("xerror=%d,   yerror=%d\r\n",80-Cx,60-Cy);
  111. 									}
  112.   								
  113. 									RxFlag1 = 0;
  114. 									RxCounter1 = 0;
  115. 									RxState = 0;
  116. 									USART_ITConfig(USART2,USART_IT_RXNE,ENABLE);
  117. 						}
  118. 						else   
  119. 						{
  120. 									RxState = 0;
  121. 									RxCounter1=0;
  122. 									for(i=0;i<10;i++)
  123. 									{
  124. 											RxBuffer1[i]=0x00;      
  125. 									}
  126. 						}
  127. 				} 
  128. 	
  129. 				else   
  130. 				{
  131. 						RxState = 0;
  132. 						RxCounter1=0;
  133. 						for(i=0;i<10;i++)
  134. 						{
  135. 								RxBuffer1[i]=0x00;     
  136. 						}
  137. 				}
  138.  
  139. 		}
  140. 	}
  141.  
  142. 	
  143.

这是接受openmv端传回坐标的代码，此代码是借鉴网上其他博主写的代码，感兴趣的小伙伴也可搜索查阅。

（2）pid.c的代码

  1. #include "pid.h"
  2. //x轴舵机的pid运算
  3. float Kp_x=2,     //2
  4. 	    Ki_x=0.15,  //0.15
  5.       Kd_x=2;    //2
  6. float www,zzz;
  7. int pwm_xpid(int xerror)
  8. {
  9. 	int pid_ActualPwm;
  10.     static float pid_Integral,pid_Voltage,error_Last;//注意这里需要使用static关键字
  11. 	pid_Integral+=xerror;
  12. 	www=pid_Integral;
  13. 	if (pid_Integral<-6000) pid_Integral=-6000;//积分限幅
  14. 	if (pid_Integral>6000) pid_Integral=6000;//积分限幅
  15. 	pid_Voltage=Kp_x*xerror+Ki_x*pid_Integral+Kd_x*(xerror-error_Last);	
  16. 	error_Last=xerror;
  17. 	pid_ActualPwm=pid_Voltage*1;
  18. 	if (pid_ActualPwm<-1000) pid_ActualPwm=-1000;//pwm的范围是500到2500,这里要对pwm进行限幅
  19. 	if (pid_ActualPwm>1000) pid_ActualPwm=1000;
  20. 	return pid_ActualPwm;
  21. }
  22.  
  23. //y轴舵机的pid运算
  24. float Kp_y=1,     //1
  25. 	    Ki_y=0.15,  //0.15
  26.       Kd_y=2;    //2
  27.  
  28. int pwm_ypid(int yerror)
  29. {
  30. 	int pid_ActualPwm;
  31.     static float pid_Integral,pid_Voltage,error_Last;//注意这里需要使用static关键字
  32. 	pid_Integral+=yerror;
  33.     zzz=pid_Integral;
  34. 	if (pid_Integral<-6000) pid_Integral=-6000;//积分限幅
  35. 	if (pid_Integral>6000) pid_Integral=6000;//积分限幅
  36. 	pid_Voltage=Kp_y*yerror+Ki_y*pid_Integral+Kd_y*(yerror-error_Last);	
  37. 	error_Last=yerror;
  38. 	pid_ActualPwm=pid_Voltage*1;
  39. 	if (pid_ActualPwm<-1000) pid_ActualPwm=-1000;//pwm的范围是500到2500,这里要对pwm进行限幅
  40. 	if (pid_ActualPwm>1000) pid_ActualPwm=1000;
  41. 	return pid_ActualPwm;
  42. }

（3）timer.c代码

  1. #include "timer.h"
  2. #include "led.h"
  3. #include "pid.h"
  4. #include "usart.h"
  5.  
  6. int pid_xerror,pid_yerror,xpwm,ypwm;
  7. extern  int Cx,Cy;
  8. void TIM3_Int_Init(u16 arr,u16 psc)
  9. {
  10. 	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
  11. 	NVIC_InitTypeDef NVIC_InitStructure;
  12. 	
  13. 	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);  
  14. 	
  15.     TIM_TimeBaseInitStructure.TIM_Period = arr; 	
  16. 	TIM_TimeBaseInitStructure.TIM_Prescaler=psc;  
  17. 	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; 
  18. 	TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
  19. 	
  20. 	TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);
  21. 	
  22. 	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); 
  23. 	TIM_Cmd(TIM3,ENABLE); 
  24. 	
  25. 	NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn; 
  26. 	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; 
  27. 	NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03;
  28. 	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
  29. 	NVIC_Init(&NVIC_InitStructure);
  30. 	
  31. }
  32.  
  33.  
  34. void TIM3_IRQHandler(void)
  35. {
  36. 	if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) 
  37. 	{
  38. 		if (Cx>0&&Cy>0)//Cx、Cy分别为红色物体中心点的横、纵坐标，当Cx、Cy都大于0时代表识别到红色物体
  39. 		{	pid_xerror=80-Cx;//80为画面中心点的横坐标，这行代码是计算红色物体中心点横坐标离画面中心点横坐标的偏差值
  40. 			pid_yerror=60-Cy;//60为画面中心点的纵坐标，这行代码是计算红色物体中心点纵坐标离画面中心点纵坐标的偏差值
  41. 			xpwm=pwm_xpid(pid_xerror);//通过pid计算得到x轴舵机运动的pwm值
  42. 			ypwm=pwm_ypid(pid_yerror);//通过pid计算得到y轴舵机运动的pwm值
  43. 			TIM_SetCompare1(TIM4,1500-xpwm);//1500对应x轴舵机转到90度，这行代码是让x轴舵机转到对应的角度
  44. 			TIM_SetCompare2(TIM4,1500-ypwm);//1500对应y轴舵机转到90度，这行代码是让y轴舵机转到对应的角度
  45. 		}
  46.     else 
  47.     {
  48.       	TIM_SetCompare1(TIM4,1500-xpwm);//这两行代码是当没有识别到红色物体时，舵机在当前位置停下
  49. 	    TIM_SetCompare2(TIM4,1500-ypwm);
  50.     }	
  51. 		
  52. 	}
  53. 	TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  
  54. }

（4）pwm.c代码

  1. #include "pwm.h"
  2. #include "led.h"
  3. #include "usart.h"
  4.  
  5.  
  6. void TIM4_PWM_Init(u32 arr,u32 psc)
  7. {		 					 
  8. 	
  9. 	
  10. 	GPIO_InitTypeDef GPIO_InitStructure;
  11. 	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  12. 	TIM_OCInitTypeDef  TIM_OCInitStructure;
  13. 	
  14. 	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);  	   
  15. 	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); 	
  16. 	
  17. 	GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);
  18.     GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4); 
  19.  
  20. 	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;           
  21. 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;     
  22. 	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;	
  23. 	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;      
  24. 	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        
  25. 	GPIO_Init(GPIOB,&GPIO_InitStructure);              
  26. 	  
  27. 	TIM_TimeBaseStructure.TIM_Prescaler=psc; 
  28. 	TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; 
  29. 	TIM_TimeBaseStructure.TIM_Period=arr;   
  30. 	TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1; 
  31. 	
  32. 	TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure);
  33. 	
  34.  
  35. 	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; 
  36.  	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; 
  37. 	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
  38. 	TIM_OC1Init(TIM4, &TIM_OCInitStructure); 
  39. 	TIM_OC2Init(TIM4, &TIM_OCInitStructure);  
  40.  
  41. 	TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); 
  42.  	TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);  
  43.  
  44.     TIM_ARRPreloadConfig(TIM4,ENABLE);
  45. 	
  46. 	TIM_Cmd(TIM4, ENABLE);  
  47.  
  48. 										  
  49. }  
  50.  
  51.

（5）main.c代码

  1. #include "sys.h"
  2. #include "delay.h"
  3. #include "usart.h"
  4. #include "led.h"
  5. #include "timer.h"
  6. #include "pwm.h"
  7. #include "usart2.h"
  8.  
  9. //PA9----TX    PA10-----RX
  10. extern float www,zzz;
  11. extern int  xpwm;
  12. extern int pid_xerror,pid_yerror,xpwm,ypwm;
  13. extern void TIM4_PWM_Init(u32 arr,u32 psc);
  14. int main(void)
  15. { 
  16. 	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
  17. 	delay_init(168);  //初始化延时函数
  18. 	LED_Init();	      //初始化LED端口
  19. 	uart_init(9600);
  20. 	uart2_init(9600); //   openmv       stm32
  21. 	                  //    p4(TX)------A3(RX) 
  22. 	                  //    p5(RX)------A2(TX)
  23.  	TIM3_Int_Init(100-1,8400-1);  //定时器3每10ms中断一次
  24. 	TIM4_PWM_Init(20000-1,84-1);  //舵机  20ms   0度---pwm50   90度---pwm150  180度-pwm250
  25.     TIM_SetCompare1(TIM4,1500);//让x轴舵机转到90度
  26.     TIM_SetCompare2(TIM4,1500);//让y轴舵机转到90度
  27. 	while(1)
  28. 	{
  29.       //printf("xpwm=%d\r",xpwm);
  30. 	  //printf("ypwm=%d\r\n",ypwm);
  31.  
  32. 	};
  33. }

以上代码可直接复制到自己的工程中运行，openmv端识别的代码以及和32端通信的代码参考其他博主的代码，感兴趣的小伙伴可以自行搜索，欢迎大家在评论区评论，有什么问题可在评论区提问。

以下是完整的代码链接：

链接：https://pan.baidu.com/s/1lF4xOCTfemaRtiFwLq6W8w?pwd=y0rw 
提取码：y0rw 
--来自百度网盘超级会员V4的分享
 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/繁依Fanyi0/article/detail/573869
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号