🌈个人主页：嵌入点滴
🔥🔥🔥
目录
任务三：实现激光点绕 A4 纸边缘一周（基础部分） （与任务二的差别：矩形的放置角度可能倾斜；需要区分两个矩形的宽度以识别 A4 纸）
3. 给定两点以及等分数计算所有等分点算法（线上移动减少偏差）
4. 在 PID 寻点时获取基本点（矩形顶点及中心）PWM 值算法
前言
💬今日更新了 23年电赛E题 的相关内容
题目分析| 解决方案| 核心代码
🎉欢迎大家关注🔍点赞👍收藏⭐️留言📝
-1- 题目要求
任务一：达成按键按下复位（基础部分）
方式①：识别四个顶点的位置，接着连接对角线获取中心点，通过 PID 调节让激光点与中心点重合。
方式②：识别四个顶点的位置，对对角顶点的坐标求平均值得出中心点位置，利用 PID 调节使其重合。
方式③：固定所有器件的位置，保证各点的 PWM 值不变，获取中心点的固定 PWM 值，采用开环设定。
任务二：实现激光点绕边框一周（基础部分）
步骤一：激光点从中心点抵达边线左上角。
步骤二：按顺时针方向绕一圈。
方式①：确定两点坐标以定线，连线确定等分点，运用 PID 算法在等分点之间移动。
方式②：不使用 PID，依据与目标点坐标的差值计算移动方向，每次移动的距离为舵机的最小精度值。
方式③：求出 PWM 和坐标(x,y)的函数关系（近似线性），直接设定 PWM 值以到达指定点。
任务三：实现激光点绕 A4 纸边缘一周（基础部分） （与任务二的差别：矩形的放置角度可能倾斜；需要区分两个矩形的宽度以识别 A4 纸）
任务四：实现绿色激光追踪红色激光（发挥部分）
方式①：区分红绿色激光并获取其坐标，通过 PID 直接跟踪。
-2- 任务实现与分工
上述任务的进一步技术分解：
一、硬件平台搭建
1. 元件选型与采购
*负责挑选适合项目需求的传感器、激光模块、STM32 开发板等硬件元件，并进行采购。
2. 电路设计与布线
*设计硬件电路原理图，包括电源管理、通信接口、驱动电路等。
*完成 PCB 布线和制版工作。
3. 硬件组装与调试
* 将采购的元件进行组装焊接。
*对组装好的硬件进行初步调试，检查电路是否正常工作。
二、STM32 控制算法
1. PID 控制激光点移动到目标点算法（核心）
* 数学模型建立
*分析系统特性，建立激光点移动的数学模型。
*参数整定
*通过实验和仿真，调整 PID 控制器的参数，如比例系数、积分时间和微分时间。
*实时优化
*在实际运行中，根据反馈数据对 PID 参数进行实时优化。
2. 舵机以最小分度值移动算法（细微调节）
*分度值定义
*确定舵机能够实现的最小分度值，并进行精确测量。
*驱动程序开发
*编写舵机的驱动程序，实现按照设定的分度值进行移动控制。
*精度测试
*对舵机的移动精度进行测试和评估。
3. 给定两点以及等分数计算所有等分点算法（线上移动减少偏差）
*算法设计
*设计高效的算法，根据给定的两点坐标和等分数，准确计算出所有等分点的坐标。
*误差分析
*分析算法可能产生的误差，并采取相应的补偿措施。
*性能优化
*对算法进行优化，提高计算速度和精度。
4. 在 PID 寻点时获取基本点（矩形顶点及中心）PWM 值算法
*位置与 PWM 关系建模
*建立矩形顶点及中心位置与 PWM 值之间的数学关系。
* 数据采集与拟合
*通过实验采集数据，进行曲线拟合，得到准确的关系模型。
*实时获取与更新
*在 PID 寻点过程中，能够实时获取并更新对应的 PWM 值。
三、OpenMV 识别算法
1. 识别铅笔线边框
*图像预处理
*负责实现灰度图转换和阈值分割成二值图的相关代码。
*直线检测与筛选
*运用霍夫直线变换得到直线上两点（非端点），并从得到的多条直线中筛选去重。
*交点计算
*编写“已知两直线上两点求直线交点”的算法，计算出四个端点。
2. 识别 A4 纸边框
* 角点检测
*进行阈值分割后，利用角点检测算法检测出 A4 框的 8 个顶点。
* 顶点识别与分组
*编写“从 8 个顶点中识别两两相邻顶点”的算法，对顶点进行分组。
*中心线顶点计算
*根据分组结果，计算出框中心线的 4 个顶点。
3. 区分红绿激光点算法
*色彩空间转换
*实现将图像转换到 HSV 色彩空间的代码。
* 阈值设置与二值化
*分别设置红绿色的阈值，在 HSV 空间中二值化图像。
*坐标提取
*从二值化后的图像中提取红绿色区域，得到激光点的坐标。
-3- 主要代码
1.STM32上的关键源码
*1* 基本部分
（1）引脚使用说明
//*************************引脚使用说明*************************
/*
oled.h GPIOA PIN0/1
bluetooth.h GPIOA PIN2/3
joystick.h GPIOA PIN4/5 ADC1_CH4/5 GPIOB PIN11/12/13 EXTI12/13
Pwm.h GPIOA PIN8/11 TIM1_CH1/4 50hz
usart.h GPIOA PIN9/10 TX/RX Black/White
beep.h GPIOB PIN14
led.h GPIOB PIN15
Timer.h TIM2/3
*/（2）头文件声明
//************************头文件声明************************
#include "public.h" //公用引用函数封装
//#include "bluetooth.h" //蓝牙模块
#include "oled.h" //OLED显示屏模块
#include "Pwm.h" //PWM波生成模块
#include "servo_motor.h" //云台控制函数模块
#include "joystick.h" //摇杆控制模块
#include "string.h"
#include "Delay.h"
#include "Timer.h" //定时器模块
#include "usart.h" //uart通信模块
#include "beep.h" //蜂鸣器模块
#include "led.h" //led灯模块
#include "dma.h" //dma数据转存模块（3）全局变量和宏定义声明
//************************全局变量和宏定义声明************************
//#define OpenLoop_OL //开环实现功能执行
#define CloseLoop_CL //闭环实现功能执行
extern float Voltage[2]; //ad测量电压值[0.3.3] //ad.c
extern char USART_RX_INFO[USART_REC_LEN]; //uart接收数据 //usart.c
extern int x,y; //激光当前坐标 //servo_motor.c
extern int Vertex[4][2]; //四顶点位置 //servo_motor.c
extern int Vertex_Peak_Pos[4][2];
extern int Vertex_A4[4][2];
extern Pwm Center_Pwm;
extern Pwm Peak_Pwm[4];
extern Pwm A4_Pwm[4];
int Programme_Progress=0; //比赛程序进度
int order=0; //蓝牙接收到的命令
int Main_Wait_Stop_Sign =1; //主程序等待标志位
extern int JoyStick_Control_Stop_Sign; //摇杆控制程序结束标志位
int Get_Depend_Point_Pos_Stop_Sign=1;
int Get_A4_Point_Pos_Stop_Sign=1;
extern int Follow_Track_Stop_Sign; //矩形寻迹结束标志位
extern int Follow_Point_Stop_Sign; //绿激光跟随红激光结束标志位*2* 模块代码
（1）定时器延时函数模块
#include "Timer.h"
//TIM2/3
void Timer_Init(void)
{
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
TIM_InternalClockConfig(TIM2);
TIM_InternalClockConfig(TIM3);
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_Period = 60000 - 1; //分辨率1us,最大60ms
TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1;
TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseInitStructure);
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStructure);
}
void Timer_delay_us(int xus)
{
TIM_Cmd(TIM2,ENABLE); //启动定时器
while(TIM2->CNT < xus);
TIM2->CNT = 0;
TIM_Cmd(TIM2,DISABLE); //关闭定时器
}
void Timer_delay_ms(int xms)
{
int i=0;
for(i=0;i<xms;i++)Timer_delay_us(1000);
}
//外部中断专用延时函数
void EXIT_LINE_Timer_delay_us(int xus)
{
TIM_Cmd(TIM3,ENABLE); //启动定时器
while(TIM3->CNT < xus);
TIM3->CNT = 0;
TIM_Cmd(TIM3,DISABLE); //关闭定时器
}
void EXIT_LINE_Timer_delay_ms(int xms)
{
int i=0;
for(i=0;i<xms;i++)EXIT_LINE_Timer_delay_us(1000);
}（2）云台舵机控制模块
*控制舵机旋转
int Oc_Lp[4]={750,750,750,750};
int Oc_Vp[4]={763,763,763,763};
/*********************************************************
函数功能：云台水平方向旋转
*********************************************************/
void Spinnig_Level(int diff)
{
if(diff<0)
{
Oc_Lp[0]=Oc_L=(Oc_L+diff)<660?660:(Oc_L+diff);
}
else if(diff>0)
{
Oc_Lp[0]=Oc_L=(Oc_L+diff)>840?840:(Oc_L+diff);
}
TIM_SetCompare1(TIM1,Oc_L);
int i;
for(i=3;i>0;i--)Oc_Lp[i]=Oc_Lp[i-1];
}*控制激光点到达某一像素点
/*********************************************************
函数功能：云台控制激光点到达某一点
函数参数：目标点的坐标
*********************************************************/
int x=360,y=360; //跟随点当前坐标
int Reach_Pos_CL_Stop_Sign=1;
//云台水平方向旋转PID值
float Level_Kp=0.06;
float Level_Ki=0.02;
float Level_Kd=0.01;
//云台竖直方向旋转PID值
float Vert_Kp=0.06;
float Vert_Ki=0.02;
float Vert_Kd=0.01;
void Reach_Pos_CL(int Target_X,int Target_Y,int Reach_Pos_CL_MODE)
{
int Sign(int num);
void Get_Point_Pos(void);
int near(int Target_X,int Target_Y);
int diff_x,diff_y;
while(Reach_Pos_CL_Stop_Sign)
{
Timer_delay_ms(30);
Get_Point_Pos();
if(near(Target_X,Target_Y)<=6)
{
Beep_Times(10,1,NORMAL_MODE);
break;
}
if(Reach_Pos_CL_MODE==PID_MODE && near(Target_X,Target_Y)>60) //用pid计算舵机单位数
{
diff_x=Pid_Control(Level_Kp,Level_Ki,Level_Kd,Target_X,x,PID_REALIZE);
diff_y=Pid_Control(Vert_Kp,Vert_Ki,Vert_Kd,Target_Y,y,PID_REALIZE);
}
else if(Reach_Pos_CL_MODE==MINMIZE_MODE) //以舵机最小分辨率为单位
{
diff_x=-Sign(x-Target_X);
diff_y=-Sign(y-Target_Y);
}
else if(Reach_Pos_CL_MODE==PID_MODE && near(Target_X,Target_Y)<=60) //用pid计算舵机单位数
{
diff_x=-Sign(x-Target_X);
diff_y=-Sign(y-Target_Y);
Timer_delay_ms(30);
}
Spinnig_Level(X_DIR*diff_x);
Spinnig_Vert(Y_DIR*diff_y);
Timer_delay_ms(20);
}
}
int Sign(int num)
{
if(num>5)return 1;
else if(num<-5)return -1;
else return 0;
}
int my_abs(int a,int b)
{
return a-b>0?a-b:b-a;
}
int near(int Target_X,int Target_Y)
{
return my_abs(Target_X,x)+my_abs(Target_Y,y);
}*控制激光点沿四边形巡线
//巡线
void Follow_Track(int Vertex[4][2],int divide_num)
{
int i,j;
float sub_l,sub_v;
Pwm Vertex_Pwm[4];
for(i=0;i<4;i++)Get_Pwm(Vertex[i][0],Vertex[i][1],&Vertex_Pwm[i],1);
for(i=0;i<4;i++)
{
sub_l=(Vertex_Pwm[(i+1)%4].level-Vertex_Pwm[i].level); //下一个顶点与当前顶点pwm之差
sub_v=(Vertex_Pwm[(i+1)%4].vert-Vertex_Pwm[i].vert); //下一个顶点与当前顶点纵坐标之差
for(j=0;j<divide_num;j++)
{
Reach_Pos_OL(Vertex_Pwm[i].level+j*sub_l/divide_num,Vertex_Pwm[i].vert+j*sub_v/divide_num);
Timer_delay_ms(200);
}
Reach_Pos_OL(Vertex_Pwm[(i+1)%4].level,Vertex_Pwm[(i+1)%4].vert);
Timer_delay_ms(300);
}
Beep_Times(50,5,NORMAL_MODE);
}*3*主函数与中断函数部分
（1）红色激光云台
//*************************主函数部分*************************
//重新重启初值还原设置
void Programme_Reset(void)
{
Beep_Times(1000,1,NORMAL_MODE);
Led_Times(1000,1,NORMAL_MODE);
Programme_Progress=0;
Main_Wait_Stop_Sign=1;
JoyStick_Control_Stop_Sign=1;
Follow_Track_Stop_Sign=1;
Get_A4_Point_Pos_Stop_Sign=1;
Get_Depend_Point_Pos_Stop_Sign=1;
}
int main(void)
{
//********************初始化程序********************
Timer_Init(); //定时器初始化
// BlueToothInit(9600,USART_Parity_No,USART_StopBits_1,USART_WordLength_8b); //蓝牙初始化
OLED_Init(); //oled初始化
Beep_Init(); //蜂鸣器初始化
Led_Init(); //led灯初始化
TIM1_PWM_Init(9999,143); //一周期20ms，分辨率20ms/10000）
TIM_SetCompare1(TIM1,750); //对齐角度为90度(1.5ms)
TIM_SetCompare4(TIM1,763); //对齐角度为90度(1.5ms)
uart_init(115200); //uart1初始化
JoyStick_Init(); //JoyStick摇杆初始化
//*************************比赛程序部分*************************
while(1)
{
int i;
//重新重启初值还原设置
Programme_Reset();
// Reach_Pos_CL(50,50,PID_MODE);
Axes_Init();
while(Main_Wait_Stop_Sign);
//摇杆控制
JoyStick_Control();
Pwm_Track(Peak_Pwm,1);
while(Follow_Track_Stop_Sign);
Get_A4_Point_Pos();
Timer_delay_ms(2000);
for(i=0;i<4;i++)Get_Pwm(Vertex_A4[i][0],Vertex_A4[i][1],&A4_Pwm[i],1);
Pwm_Track(A4_Pwm,6);
while(Get_A4_Point_Pos_Stop_Sign);
}
}
//--------------------------------中断函数部分--------------------------------------------
//按键中断函数
void EXTI15_10_IRQHandler()
{
if (EXTI_GetITStatus(EXTI_Line11) == SET)
{
EXIT_LINE_Timer_delay_ms(10);
if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11)==0) //软件防抖
{
Beep_Times(50,2,EXIT_LINE_MODE);
Reach_Pos_OL(Oc_L,Oc_V); //保持激光当前指向位置
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11)==0); //等待按键松开
//再次按下才退出
EXIT_LINE_Timer_delay_ms(10);
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11)==1);
EXIT_LINE_Timer_delay_ms(10);
if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11)==0); //软件防抖
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_11==1)); //等待按键松开
Beep_Times(50,2,EXIT_LINE_MODE);
EXTI_ClearITPendingBit(EXTI_Line11);
}
}
else if (EXTI_GetITStatus(EXTI_Line12) == SET)
{
EXIT_LINE_Timer_delay_ms(10);
if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_12)==0) //软件防抖
{
Programme_Progress++;
Beep_Times(500,1,EXIT_LINE_MODE);
if(Programme_Progress==1)
{
Main_Wait_Stop_Sign=0;
}
else if(Programme_Progress==2)
{
JoyStick_Control_Stop_Sign=0;
}
else if(Programme_Progress==3)
{
// Get_Depend_Point_Pos_Stop_Sign=0;
Follow_Track_Stop_Sign=0;
}
else if(Programme_Progress==4)
{
Get_A4_Point_Pos_Stop_Sign=0;
// Follow_Track_Stop_Sign=0;
}
else if(Programme_Progress==5)
{
// Get_A4_Point_Pos_Stop_Sign=0;
}
else if(Programme_Progress==6)
{
;
}
else if(Programme_Progress==7)
{
;
}
else
{
Programme_Reset();
}
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_12)==0); //等待按键松开
EXTI_ClearITPendingBit(EXTI_Line12);
}
}
else if (EXTI_GetITStatus(EXTI_Line13) == SET)
{
EXIT_LINE_Timer_delay_ms(10);
if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_13)==0) //软件防抖
{
Beep_Times(50,3,EXIT_LINE_MODE);
Reach_Pos_OL(Center_Pwm.level,Center_Pwm.vert);
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_13)==0); //等待按键松开
//再次按下才退出
EXIT_LINE_Timer_delay_ms(10);
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_13)==1);
EXIT_LINE_Timer_delay_ms(10);
if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_13)==0); //软件防抖
while(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_13==1)); //等待按键松开
Beep_Times(50,3,EXIT_LINE_MODE);
EXTI_ClearITPendingBit(EXTI_Line13);
}
}
}2.OpenMV上关键识别代码
import sensor, image, time,math
from pyb import Pin
#from pid import PID
from pyb import Servo
from pyb import LED
#红追绿，由于红色激光照到黑胶布上，红色激光无法识别。而绿色的可以，
#因此我们使用绿色作为移动目标，红色作为跟踪。
#本舵机使用红色激光笔，去追踪绿设激光点。
#舵机结构红色激光笔与镜头一体，同时运动
#红色中心点初始位置为 170，110，这个根据自己激光笔和摄像头固定的位置测量得到，
#有可能存在误差
red_x=170
red_y=110
pan_servo=Servo(1)
tilt_servo=Servo(2)
#p_in0 = Pin('P0', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，复位用
p_in1 = Pin('P1', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，停止光标移动
p_in2 = Pin('P2', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻，重启光标移动
#如果单纯实现追踪，可忽略以上设置。
pan_servo.calibration(500,2500,500)
tilt_servo.calibration(500,2500,500)
time.sleep_ms(300)
gray_threshold = (0, 191)#灰度阈值根据实际情况可微调，排除最亮的点即可，后续程序有取反
green_threshold = (100, 78, -32, 91, -60, 99)#此处LAB阈值红绿通用，不改程序名称了
sensor.reset() # Initialize the camera sensor.
#sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_pixformat(sensor.GRAYSCALE) # use GRAYSCALE.
sensor.set_framesize(sensor.QVGA) # use QVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
while(True):
# 但由于安装不够稳固，每次安装会有少许变化，需要在程序中进行修正。red_x,red_y
# 跟踪过程实际上是用修正后的红色坐标去追踪绿色目标
ss=[[0,0],[0,0],[0,0]] #存储识别到的亮点坐标，实际上只需要两个，防止溢出多开一个空间
min_size=400
for i in range (1000):
if p_in1.value()==0: #暂停mov_pwm
while(True):
if p_in2.value()==0: #恢复mov_pwm
break
j=0
r=[900,900] #初始化两个亮点到标准绿点的距离为一个比较大的值
img = sensor.snapshot()
#blobs = img.find_blobs([green_threshold],x_stride=1,y_stride=1)
blobs = img.find_blobs([gray_threshold],x_stride=1,y_stride=1,invert=True)
if i > 10 :
for blob in blobs:
#尽量过滤掉一些杂散的blobs
if (blob[2]*blob[3] < min_size) and (blob[2]>2) and (blob[3]>2) and (blob[2]<2*blob[3]) and (blob[3]<2*blob[2]):
#确保色块较小且较方（圆）
yuandian = blob
img.draw_rectangle(yuandian.rect(),color = (0,0,0)) # rect
img.draw_cross(yuandian.cx(), yuandian.cy()) # cx, cyj v
print(yuandian.cx(), yuandian.cy(),blob[2],blob[3])
if j>1: #蓝色LED点亮，表示搜索到3个以上的目标，设备不能正常跟踪
LED(3).on() #点亮蓝灯
time.sleep_ms(500)
#LED(3).off()
ss=[[0,0],[0,0],[0,0]]
break
else: #逐个记录识别到的激光点
ss[j][0]=yuandian.cx()
ss[j][1]=yuandian.cy()
j=j+1
if j==2: #只发现两个亮点
LED(3).off() #关闭蓝灯
#计算两个亮点到初始红点的距离，谁近则谁就是追踪点，远的就是目标点
r[0]=math.sqrt((ss[0][0]-red_x)*(ss[0][0]-red_x) + (ss[0][1]-red_y)*(ss[0][1]-red_y))
r[1]=math.sqrt((ss[1][0]-red_x)*(ss[1][0]-red_x) + (ss[1][1]-red_y)*(ss[1][1]-red_y))
#计算两个亮点之间的距离
rr =math.sqrt((ss[0][0]-ss[1][0])*(ss[0][0]-ss[1][0]) + (ss[0][1]-ss[1][1])*(ss[0][1]-ss[1][1]))
if rr>20: #当两个亮点距离比较大的时候，动态的修正red_x，和red_y
#即将距离近的点坐标幅值给初始红坐标。
if (r[0]<r[1]) and (r[0]<30) :
red_x = ss[0][0]
red_y = ss[0][1]
if (r[1]<r[0]) and (r[1]<30) :
red_x = ss[1][0]
red_y = ss[1][1]
if rr<6: #如果两点之间的距离小于6个像素，则认为已经跟踪上了
LED(2).on() #点亮绿灯
else:
LED(2).off() #熄灭绿灯
now_x=pan_servo.pulse_width()
now_y=tilt_servo.pulse_width()
dx = ss[0][0]-ss[1][0]
dy = ss[0][1]-ss[1][1]
if r[0]>r[1]: # 再次根据远近判断红绿点，并确定距离方向。因为两点初始距离若小于等于20，则程序没有对预设的红点坐标进行动态校正。
dx=-dx
dy=-dy
if p_in1.value()==0: #暂停mov_pwm
while(True):
if p_in2.value()==0: #恢复mov_pwm
break
if dx>5:
pan_servo.pulse_width(now_x+10) #最小步进是10，也许不同舵机有出入，可自行调整尝试。
if dx<-5:
pan_servo.pulse_width(now_x-10)
if dy>5:
tilt_servo.pulse_width(now_y+10)
if dy<-5:
tilt_servo.pulse_width(now_y-10)
if math.sqrt(dx*dx + dy*dy)>30 :
time.sleep_ms(10)
else:
time.sleep_ms(50)
if j==1 and r[0]>6: #如果只发现一个亮点，则用标准红点red_x,red_y,去追踪这个点
now_x=pan_servo.pulse_width()
now_y=tilt_servo.pulse_width()
dx = red_x-ss[0][0]
dy = red_y-ss[0][1]
if p_in1.value()==0: #暂停mov_pwm
while(True):
if p_in2.value()==0: #恢复mov_pwm
break
if dx>5:
pan_servo.pulse_width(now_x+10) #最小步进是10
if dx<-5:
pan_servo.pulse_width(now_x-10)
if dy>5:
tilt_servo.pulse_width(now_y+10)
if dy<-5:
tilt_servo.pulse_width(now_y-10)
if math.sqrt(dx*dx + dy*dy)>30 : #根据距离不同设置休眠时间
time.sleep_ms(10)
else:
time.sleep_ms(50)
结束
7730
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
