#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
*/
/**
* @brief    伺服电机速度模式写单个寄存器
* @param    motorNo：写垂直电机还是水平电机
* @param    regAddr：要写的寄存器
* @param    data：要向寄存器写入的数据
* @return   返回数组地址
*/
static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
{
      uint16_t crc;
      if ( motorNo == H_MOTOR )
      {
        g_writeOneRegBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
      }
      else
      {
        g_writeOneRegBuff[0] = V_MOTOR_ADDR;
      }
      g_writeOneRegBuff[1] = WRITE_ONE_REG;
      g_writeOneRegBuff[2] = regAddr >> 8;
      g_writeOneRegBuff[3] = 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] = crc & 0xff;
      g_writeOneRegBuff[7] = crc >> 8;
      return g_writeOneRegBuff;
}

/**
* @brief    写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
* @param    motorNo：写垂直电机还是水平电机
* @return   false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForWrite(uint8_t motorNo)
{
    static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
    /*提取数据失败*/
    if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
    {
        return false;
    }
    else
    {
        for( uint8_t i = 0; i < WRITE_ONE_REG_FRAME_NUM; i++ )
        {
            /*提取的数据不对*/
            if ( motorReplybuff[i] != g_writeOneRegBuff[i] )
            {
                return false;
            }
        }
    }
    return true;
}

/*
从机地址    功能码    寄存器地址高   寄存器地址低  reg数量高位    数量低位    crc校验高位     crc校验低位
    01H     03H         0BH             00H         00H         01H         86H             2EH
*/
/**
* @brief    伺服电机速度模式读单个寄存器
* @param    motorNo：读垂直电机还是水平电机
* @param    regAddr：要读的寄存器
* @return   返回数组地址
*/
static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
{
      uint16_t crc;
      if ( motorNo == H_MOTOR )
      {
        g_readOneRegBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
      }
      else
      {
        g_readOneRegBuff[0] = V_MOTOR_ADDR;
      }
      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;
      return g_readOneRegBuff;
}
/*
从机地址    功能码    返回数据字节数   返回数据高位  返回数据低位     crc校验高位     crc校验低位
    01H     03H         02H             00H            00H             B8H             44H
*/
/**
* @brief    读单个寄存器命令发送成功后，电机会返回一帧7字节的数据
* @param    motorNo：写垂直电机还是水平电机
* @param    data：写垂直电机还是水平电机
* @return   false:错误，读出的内容与写入的内容不一致
*/
bool MotorReplyForRead(uint8_t motorNo, uint16_t* data)
{
    /*简易判断*/
    if ( data == NULL )
    {
       return false;
    }
    static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM - 1];
    /*提取数据失败*/
    if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM - 1) == false )
    {
        return false;
    }
    else
    {
        *data = motorReplybuff[3];
        *data = (*data << 8) | motorReplybuff[4];
    }
    return true;
}


/**
* @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);
}