111 lines
3.8 KiB
C
111 lines
3.8 KiB
C
#include "servoMotor.h"
|
||
#include <ucos_ii.h>
|
||
|
||
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
|
||
/*
|
||
水平垂直电机使能开关引脚配置
|
||
*/
|
||
static void MotorSwitchGpioCofig(void)
|
||
{
|
||
/*GPIO时钟初始化*/
|
||
rcu_periph_clock_enable(RCU_GPIOE);
|
||
/*水平电机打开引脚*/
|
||
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_0);
|
||
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
|
||
/*垂直电机打开引脚*/
|
||
gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_1);
|
||
gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
|
||
|
||
}
|
||
|
||
|
||
/*
|
||
从机地址 功能码 寄存器地址高 寄存器地址低 数据高位 数据低位 crc校验高位 crc校验低位
|
||
01H 06H 02H 00H 00H 01H 49H B2H
|
||
*/
|
||
/**
|
||
* @brief 伺服电机速度模式写单个寄存器
|
||
* @param motorNo：写垂直电机还是水平电机
|
||
* @param regAddr：要写的寄存器
|
||
* @param data：要向寄存器写入的数据
|
||
* @return 返回数组地址
|
||
*/
|
||
uint8_t g_HwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
|
||
uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
|
||
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data)
|
||
{
|
||
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
|
||
uint16_t crc;
|
||
g_writeOneRegBuff[0] = 0x01;
|
||
g_writeOneRegBuff[1] = WRITE_ONE_REG;
|
||
g_writeOneRegBuff[2] = regAddr >> 8;
|
||
g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
|
||
g_writeOneRegBuff[4] = data >> 8;
|
||
g_writeOneRegBuff[5] = data & 0xff;
|
||
crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
|
||
g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
|
||
g_writeOneRegBuff[7] = crc >> 8;
|
||
if ( motorNo == H_MOTOR )
|
||
{
|
||
memcpy(g_HwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
|
||
return g_HwriteOneRegBuff;
|
||
}
|
||
|
||
memcpy(g_VwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
|
||
return g_VwriteOneRegBuff;
|
||
}
|
||
|
||
|
||
|
||
/*
|
||
从机地址 功能码 寄存器地址高 寄存器地址低 reg数量高位 数量低位 crc校验高位 crc校验低位
|
||
01H 03H 0BH 00H 00H 01H 86H 2EH
|
||
*/
|
||
/**
|
||
* @brief 伺服电机速度模式读单个寄存器
|
||
* @param motorNo：读垂直电机还是水平电机
|
||
* @param regAddr：要读的寄存器
|
||
* @return 返回数组地址
|
||
*/
|
||
static uint8_t g_HreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
|
||
static uint8_t g_VreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
|
||
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
|
||
{
|
||
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
|
||
uint16_t crc;
|
||
g_readOneRegBuff[0] = 0x01;
|
||
g_readOneRegBuff[1] = READ_ONE_REG;
|
||
g_readOneRegBuff[2] = regAddr >> 8;
|
||
g_readOneRegBuff[3] = regAddr & 0xff;
|
||
g_readOneRegBuff[4] = 0x00;
|
||
g_readOneRegBuff[5] = 0x01;
|
||
crc = ModbusCRC16(g_readOneRegBuff, READ_ONE_REG_FRAME_NUM - 2);
|
||
g_readOneRegBuff[6] = crc & 0xff;
|
||
g_readOneRegBuff[7] = crc >> 8;
|
||
if ( motorNo == H_MOTOR )
|
||
{
|
||
memcpy(g_HreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
|
||
return g_HreadOneRegBuff;
|
||
}
|
||
|
||
memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
|
||
return g_VreadOneRegBuff;
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* @brief 伺服电机驱动初始化
|
||
* @param
|
||
* @return
|
||
*/
|
||
void servoMotorInit(void)
|
||
{
|
||
MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
|
||
H_MOTOR_OPEN;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
|
||
V_MOTOR_OPEN;
|
||
|
||
CommuDrvInit();//伺服电机RS485通讯初始化
|
||
}
|
||
#endif |