87 lines
2.5 KiB
C
87 lines
2.5 KiB
C
#include "servoMotor.h"
|
||
#include <ucos_ii.h>
|
||
/*
|
||
水平垂直电机使能开关引脚配置
|
||
*/
|
||
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
|
||
*/
|
||
bool WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
|
||
{
|
||
uint8_t frameBuff[8] = {0};
|
||
uint8_t replyTemp[8];
|
||
uint16_t crc;
|
||
frameBuff[0] = 0x01;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
|
||
frameBuff[1] = WRITE_ONE_REG;
|
||
frameBuff[2] = regAddr >> 8;
|
||
frameBuff[3] = regAddr & 0xff;
|
||
frameBuff[4] = data >> 8;
|
||
frameBuff[5] = data & 0xff;
|
||
crc = ModbusCRC16(frameBuff, 6);
|
||
frameBuff[6] = crc & 0xff;
|
||
frameBuff[7] = crc >> 8;
|
||
|
||
CommuTransData(motorNo, frameBuff, 8);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);//发送数据完成需要2ms，接收电机返回的数据需要2ms，延时5ms足够
|
||
// DelayNms(5);
|
||
|
||
CommuRsvData(motorNo, replyTemp, 8);//数据缓冲区已经有数据了才能调用此函数提取数据进行解析
|
||
for( uint8_t i = 0; i < 8; i++)
|
||
{
|
||
if ( frameBuff[i] != replyTemp[i] )
|
||
{
|
||
H_MOTOR_STOP;
|
||
return false;
|
||
}
|
||
}
|
||
return true;
|
||
}
|
||
|
||
|
||
|
||
/**
|
||
* @brief 伺服电机驱动初始化
|
||
* @param
|
||
* @return
|
||
*/
|
||
void ServoMotorInit(void)
|
||
{
|
||
MotorSwitchGpioCofig();
|
||
H_MOTOR_OPEN;
|
||
V_MOTOR_OPEN;
|
||
CommuDrvInit();
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 500u);
|
||
WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0);//H0200,选择速度模式
|
||
// DelayNms(50);
|
||
|
||
WriteMotorOneReg(H_MOTOR, H04_DO1_FUNC_SELEC, 19);
|
||
// DelayNms(50);
|
||
|
||
WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 100);//速度设置为100rpm
|
||
// DelayNms(50);
|
||
|
||
WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000);//加速度3000
|
||
// DelayNms(50);
|
||
|
||
WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000);//减速度2000
|
||
// DelayNms(50);
|
||
|
||
|
||
WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1);//只启动水平电机
|
||
// DelayNms(50);
|
||
|
||
} |