#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足够
                                    
      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();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
    H_MOTOR_OPEN;
    V_MOTOR_OPEN;
    
    CommuDrvInit();//伺服电机RS485通讯初始化
    OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
    WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0);//H0200,选择速度模式

    WriteMotorOneReg(H_MOTOR, H04_DO1_FUNC_SELEC, 19);

    WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 100);//速度设置为100rpm

    WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000);//加速度3000

    WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000);//减速度2000

    WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1);//只启动水平电机       
}