#include "stm32f10x.h"
#include "stm32f10x_gpio.h"
#include "stm32f10x_rcc.h"
#include "stm32f10x_tim.h"
#include "misc.h"
#include <math.h>
#define M_PI 3.1515926
// 云台电机引脚定义 - 移除了使能引脚控制
#define PAN_STEP_PIN GPIO_Pin_0 // PA0 - 水平轴步进
#define PAN_STEP_PORT GPIOA
#define PAN_DIR_PIN GPIO_Pin_1 // PA1 - 水平轴方向
#define PAN_DIR_PORT GPIOA
#define TILT_STEP_PIN GPIO_Pin_6 // PA6 - 垂直轴步进
#define TILT_STEP_PORT GPIOA
#define TILT_DIR_PIN GPIO_Pin_7 // PA7 - 垂直轴方向
#define TILT_DIR_PORT GPIOA
// 步进电机参数
#define STEPS_PER_REV 200 // 每转步数
#define MICROSTEPS 16 // 细分倍数
#define DEG_PER_STEP (360.0f/(STEPS_PER_REV*MICROSTEPS)) // 每步角度
#define MAX_FREQ 1000 // 最大脉冲频率(Hz)
#define ACCEL_RATE 50 // 加速度(Hz/ms)
// 云台控制结构体 (添加运动控制参数)
typedef struct {
volatile float current_angle; // 当前角度(度)
volatile float target_angle; // 目标角度(度)
volatile float current_speed; // 当前速度(Hz)
volatile float target_speed; // 目标速度(Hz)
volatile float acceleration; // 加速度(Hz/ms)
volatile int32_t total_steps; // 总步数
volatile int32_t step_count; // 当前步数
volatile uint8_t is_moving; // 运动标志
GPIO_TypeDef* dir_port; // 方向端口
uint16_t dir_pin; // 方向引脚
TIM_TypeDef* timer; // 定时器
} StepperAxis;
StepperAxis pan_axis, tilt_axis;
// 摄像头参数
#define FOCAL_LENGTH 3.04f // 焦距(mm)
#define SENSOR_WIDTH 3.674f // 传感器宽度(mm)
#define IMAGE_WIDTH 320 // 图像宽度(pixel)
// GPIO初始化 (移除了使能引脚配置)
void GPIO_Config(void) {
GPIO_InitTypeDef GPIO_InitStruct;
// 启用时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
// 配置水平轴引脚
GPIO_InitStruct.GPIO_Pin = PAN_STEP_PIN | PAN_DIR_PIN;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(PAN_STEP_PORT, &GPIO_InitStruct);
// 配置垂直轴引脚
GPIO_InitStruct.GPIO_Pin = TILT_STEP_PIN | TILT_DIR_PIN;
GPIO_Init(TILT_STEP_PORT, &GPIO_InitStruct);
}
// 定时器配置
void TIM_Config(void) {
TIM_TimeBaseInitTypeDef TIM_InitStruct;
NVIC_InitTypeDef NVIC_InitStruct;
// 启用定时器时钟
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3, ENABLE);
// 配置水平轴定时器 (TIM2)
TIM_InitStruct.TIM_Period = SystemCoreClock / (MAX_FREQ * 2) - 1;
TIM_InitStruct.TIM_Prescaler = 0;
TIM_InitStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_InitStruct.TIM_ClockDivision = 0;
TIM_TimeBaseInit(TIM2, &TIM_InitStruct);
// 配置垂直轴定时器 (TIM3)
TIM_TimeBaseInit(TIM3, &TIM_InitStruct);
// 配置中断
NVIC_InitStruct.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStruct);
NVIC_InitStruct.NVIC_IRQChannel = TIM3_IRQn;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 2;
NVIC_Init(&NVIC_InitStruct);
// 使能中断
TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE);
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
}
// 步进电机初始化
void Stepper_Init(void) {
// 水平轴初始化
pan_axis.current_angle = 0;
pan_axis.target_angle = 0;
pan_axis.current_speed = 0;
pan_axis.target_speed = MAX_FREQ;
pan_axis.acceleration = ACCEL_RATE;
pan_axis.is_moving = 0;
pan_axis.dir_port = PAN_DIR_PORT;
pan_axis.dir_pin = PAN_DIR_PIN;
pan_axis.timer = TIM2;
// 垂直轴初始化
tilt_axis.current_angle = 0;
tilt_axis.target_angle = 0;
tilt_axis.current_speed = 0;
tilt_axis.target_speed = MAX_FREQ;
tilt_axis.acceleration = ACCEL_RATE;
tilt_axis.is_moving = 0;
tilt_axis.dir_port = TILT_DIR_PORT;
tilt_axis.dir_pin = TILT_DIR_PIN;
tilt_axis.timer = TIM3;
}
// 坐标转角度
void TargetToAngle(int img_x, int img_y, float* pan_angle, float* tilt_angle) {
// 转换为传感器坐标 (mm)
float x_mm = (img_x - IMAGE_WIDTH/2.0f) * (SENSOR_WIDTH/IMAGE_WIDTH);
float y_mm = (img_y - IMAGE_WIDTH/2.0f) * (SENSOR_WIDTH/IMAGE_WIDTH);
// 计算角度 (弧度转角度)
*pan_angle = atan2(x_mm, FOCAL_LENGTH) * (180.0f/M_PI);
*tilt_angle = atan2(y_mm, FOCAL_LENGTH) * (180.0f/M_PI);
}
// 设置目标角度 (带梯形加速)
void SetTargetAngle(float pan_deg, float tilt_deg) {
// 计算角度差和方向
float pan_diff = pan_deg - pan_axis.current_angle;
float tilt_diff = tilt_deg - tilt_axis.current_angle;
// 设置水平轴
if(fabs(pan_diff) > 0.1f) {
pan_axis.target_angle = pan_deg;
pan_axis.total_steps = (int32_t)(fabs(pan_diff) / DEG_PER_STEP);
pan_axis.step_count = 0;
// 设置方向
if(pan_diff > 0) {
GPIO_SetBits(PAN_DIR_PORT, PAN_DIR_PIN); // 正转
} else {
GPIO_ResetBits(PAN_DIR_PORT, PAN_DIR_PIN); // 反转
}
// 启动运动
pan_axis.is_moving = 1;
pan_axis.current_speed = 100; // 初始低速
TIM_Cmd(pan_axis.timer, ENABLE);
// 更新定时器速度
uint32_t reload = SystemCoreClock / (pan_axis.current_speed * 2) - 1;
TIM_SetAutoreload(pan_axis.timer, reload);
TIM_SetCounter(pan_axis.timer, 0);
}
// 设置垂直轴 (类似逻辑)
if(fabs(tilt_diff) > 0.1f) {
tilt_axis.target_angle = tilt_deg;
tilt_axis.total_steps = (int32_t)(fabs(tilt_diff) / DEG_PER_STEP);
tilt_axis.step_count = 0;
if(tilt_diff > 0) {
GPIO_SetBits(TILT_DIR_PORT, TILT_DIR_PIN);
} else {
GPIO_ResetBits(TILT_DIR_PORT, TILT_DIR_PIN);
}
tilt_axis.is_moving = 1;
tilt_axis.current_speed = 100;
TIM_Cmd(tilt_axis.timer, ENABLE);
uint32_t reload = SystemCoreClock / (tilt_axis.current_speed * 2) - 1;
TIM_SetAutoreload(tilt_axis.timer, reload);
TIM_SetCounter(tilt_axis.timer, 0);
}
}
// 更新电机速度 (梯形加速)
void UpdateSpeed(StepperAxis* axis) {
// 计算加速阶段
if(axis->step_count < axis->total_steps / 3) {
axis->current_speed += axis->acceleration;
if(axis->current_speed > axis->target_speed)
axis->current_speed = axis->target_speed;
}
// 计算减速阶段
else if(axis->step_count > (2 * axis->total_steps) / 3) {
axis->current_speed -= axis->acceleration;
if(axis->current_speed < 100) axis->current_speed = 100;
}
// 更新定时器速度
uint32_t reload = SystemCoreClock / (axis->current_speed * 2) - 1;
TIM_SetAutoreload(axis->timer, reload);
}
// 定时器中断处理 (带梯形加速)
void TIM2_IRQHandler(void) {
if(TIM_GetITStatus(TIM2, TIM_IT_Update)) {
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
// 生成脉冲
GPIO_SetBits(PAN_STEP_PORT, PAN_STEP_PIN);
GPIO_ResetBits(PAN_STEP_PORT, PAN_STEP_PIN);
// 更新位置
if(GPIO_ReadInputDataBit(pan_axis.dir_port, pan_axis.dir_pin)) {
pan_axis.current_angle += DEG_PER_STEP;
} else {
pan_axis.current_angle -= DEG_PER_STEP;
}
pan_axis.step_count++;
// 更新速度 (梯形加速)
UpdateSpeed(&pan_axis);
// 检查是否到达目标
if(pan_axis.step_count >= pan_axis.total_steps) {
pan_axis.is_moving = 0;
TIM_Cmd(TIM2, DISABLE);
}
}
}
void TIM3_IRQHandler(void) {
if(TIM_GetITStatus(TIM3, TIM_IT_Update)) {
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
GPIO_SetBits(TILT_STEP_PORT, TILT_STEP_PIN);
GPIO_ResetBits(TILT_STEP_PORT, TILT_STEP_PIN);
if(GPIO_ReadInputDataBit(tilt_axis.dir_port, tilt_axis.dir_pin)) {
tilt_axis.current_angle += DEG_PER_STEP;
} else {
tilt_axis.current_angle -= DEG_PER_STEP;
}
tilt_axis.step_count++;
UpdateSpeed(&tilt_axis);
if(tilt_axis.step_count >= tilt_axis.total_steps) {
tilt_axis.is_moving = 0;
TIM_Cmd(TIM3, DISABLE);
}
}
}
// 系统时钟配置
void SystemClock_Config(void) {
SystemInit();
RCC_DeInit();
RCC_HSEConfig(RCC_HSE_ON);
while(!RCC_WaitForHSEStartUp());
RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
RCC_PLLCmd(ENABLE);
while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET);
RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
while(RCC_GetSYSCLKSource() != 0x08);
RCC_HCLKConfig(RCC_SYSCLK_Div1);
RCC_PCLK1Config(RCC_HCLK_Div2);
RCC_PCLK2Config(RCC_HCLK_Div1);
SystemCoreClockUpdate();
}
// 简单延时函数
void Delay_ms(uint32_t ms) {
volatile uint32_t i, j;
for(i = 0; i < ms; i++)
for(j = 0; j < 7200; j++);
}
// 主函数
int main(void) {
// 初始化
SystemClock_Config();
GPIO_Config();
TIM_Config();
Stepper_Init();
// 设置方向引脚默认状态
GPIO_SetBits(PAN_DIR_PORT, PAN_DIR_PIN);
GPIO_SetBits(TILT_DIR_PORT, TILT_DIR_PIN);
// 等待系统稳定
Delay_ms(100);
while(1) {
// 伪代码:从K230获取目标坐标
// 实际应用中应替换为摄像头数据
static int target_x = 160; // 图像中心
static int target_y = 120;
// 模拟目标移动 (测试用)
static uint8_t dir = 0;
if(dir == 0) {
target_x += 2;
if(target_x > 260) dir = 1;
} else {
target_x -= 2;
if(target_x < 60) dir = 0;
}
// 计算云台角度
float pan_deg, tilt_deg;
TargetToAngle(target_x, target_y, &pan_deg, &tilt_deg);
// 设置目标角度 (如果当前没有运动)
if(!pan_axis.is_moving && !tilt_axis.is_moving) {
SetTargetAngle(pan_deg, tilt_deg);
}
// 延时等待下一次更新
Delay_ms(20);
}
}
请解析每个函数实现的功能和用法
最新发布