#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);
    
}

static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_BUFFNUM];//由于写寄存器，电机返回的数据和写入的数据完全一致，故设此数组


/*
从机地址    功能码    寄存器地址高   寄存器地址低  数据高位    数据低位    crc校验高位     crc校验低位
    01H     06H         02H             00H         00H         01H         49H             B2H
*/
/**
* @brief    伺服电机速度模式写单个寄存器
* @param    motorNo：写垂直电机还是水平电机
* @param    regAddr：要写的寄存器
* @param    data：要向寄存器写入的数据
* @return   false:写失败，当前DMA正在发送数据
*/
bool WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
{
      uint8_t frameBuff[WRITE_ONE_REG_BUFFNUM] = {0};
      uint16_t crc;
      if ( motorNo == H_MOTOR )
      {
        frameBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
      }
      else
      {
        frameBuff[0] = V_MOTOR_ADDR;
      }
      frameBuff[1] = WRITE_ONE_REG;
      frameBuff[2] = regAddr >> WRITE_ONE_REG_BUFFNUM;
      frameBuff[3] = regAddr & 0xff;
      frameBuff[4] = data >> WRITE_ONE_REG_BUFFNUM;
      frameBuff[5] = data & 0xff;
      crc = ModbusCRC16(frameBuff, WRITE_ONE_REG_BUFFNUM - 2);
      frameBuff[6] = crc & 0xff;
      frameBuff[7] = crc >> WRITE_ONE_REG_BUFFNUM;
  
      if ( CommuTransData(motorNo, frameBuff, WRITE_ONE_REG_BUFFNUM) == false)
      {
         return false;
      }
      memcpy(g_writeOneRegBuff, frameBuff, WRITE_ONE_REG_BUFFNUM);
      return true;
}
/**
* @brief    写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param    motorNo：写垂直电机还是水平电机
* @param    userBuff：用来接收返回的数据的数组
* @param    len：接收的数据长度，对于此函数必须为WRITE_ONE_REG_BUFFNUM
* @return   false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForWrite(uint8_t motorNo, uint8_t* userBuff, uint8_t len)
{
    CommuRsvData(motorNo, userBuff, len);
    for( uint8_t i = 0; i < len; i++ )
    {
        if ( userBuff[i] != g_writeOneRegBuff[i] )
        {
            memset(g_writeOneRegBuff, 0x00, len);
            return false;
        }
    }
    memset(g_writeOneRegBuff, 0x00, len);
    return true;
}

/*
从机地址    功能码    寄存器地址高   寄存器地址低  数据高位    数据低位    crc校验高位     crc校验低位
    01H     06H         02H             00H         00H         01H         49H             B2H
*/
/**
* @brief    伺服电机速度模式写单个寄存器
* @param    motorNo：写垂直电机还是水平电机
* @param    regAddr：要写的寄存器
* @param    data：要向寄存器写入的数据
* @param    frameBuff: 用来发送的数组
* @return   写入frameBuff的长度
*/
uint8_t WriteMotorOneReg_buffer(uint8_t motorNo, uint16_t regAddr, uint16_t data, uint8_t *frameBuff)
{
    uint16_t crc;
    if ( motorNo == H_MOTOR )
    {
        frameBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
    }
    else
    {
        frameBuff[0] = V_MOTOR_ADDR;
    }
    frameBuff[1] = WRITE_ONE_REG;
    frameBuff[2] = regAddr >> WRITE_ONE_REG_BUFFNUM;
    frameBuff[3] = regAddr & 0xff;
    frameBuff[4] = data >> WRITE_ONE_REG_BUFFNUM;
    frameBuff[5] = data & 0xff;
    crc = ModbusCRC16(frameBuff, WRITE_ONE_REG_BUFFNUM - 2);
    frameBuff[6] = crc & 0xff;
    frameBuff[7] = crc >> WRITE_ONE_REG_BUFFNUM;

    return 8;
}

/**
* @brief 伺服电机驱动初始化
* @param
* @return 
*/
void servoMotorInit(void)
{
    MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
    H_MOTOR_OPEN;
    OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
    V_MOTOR_OPEN;
    
    CommuDrvInit();//伺服电机RS485通讯初始化
    // OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
    // WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0);//H0200,选择速度模式
    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    
    // WriteMotorOneReg(H_MOTOR, H04_DO1_FUNC_SELEC, 19);
    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    
    // WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 100);//速度设置为100rpm
    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    
    // WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000);//加速度3000
    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    
    // WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000);//减速度2000
    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    
    // WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1);//只启动水平电机  
    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
}