/*
 * msp_485MOTOR.c
 *
 *  Created on: Apr 3, 2026
 *      Author: SHI
 *      电机型号：42步进电机，减速比20
 *      驱动器型号：MKS SERVO42_RS485 （开环控制）
 *      左电机1#，右电机2#；1正2负为前进
 */
#include "msp_485MOTOR.h"
static const int32_t maxSpeed = 800;

MotorParameters *Motor485[ 2 +1];

uint8_t getCheckSum(uint8_t *buffer, uint8_t size);


/**
 * 速度控制模式运行指令
 * @param slaveAddr 从机地址 (0-255)
 * @param dir       方向: 0=正转(CCW), 1=反转(CW)
 * @param speed     转速 (0-800 RPM)
 * @param Handler   UART 句柄
 */
void speedModeRun_485(uint8_t slaveAddr, int32_t speed, struct UARTHandler *Handler)
{
	uint8_t dir = 0;
	uint8_t acc = 254;//0.1s加速到1000，246对应0.5s 加速到1000	// （256 -acc）×50 × RPM ÷1000 = 加速时间ms

    if(speed > maxSpeed) speed = maxSpeed;
    if(speed < -maxSpeed) speed = -maxSpeed;
	if(speed < 0)
	{
		dir = 1;
		speed = -speed;
	}

	Handler->Tx_Buf[0] = 0xFA;       //
	Handler->Tx_Buf[1] = slaveAddr;  //
	Handler->Tx_Buf[2] = 0xF6;       //
	Handler->Tx_Buf[3] = (dir<<7) | ((speed>>8) & 0x0F); //
	Handler->Tx_Buf[4] = speed & 0x00FF;   	//
	Handler->Tx_Buf[5] = acc;            	//
	Handler->Tx_Buf[6] = getCheckSum(Handler->Tx_Buf, 6);  //

	Handler->TxCount = 7;  //发送7个字节

	Handler->AddSendList(
			Handler,
			Handler->Tx_Buf,
			Handler->TxCount,
			15,
			Motor_485_Decode);
//	Handler->UART_Tx(Handler);
}

uint8_t getCheckSum(uint8_t *buffer, uint8_t size)
{
	uint16_t sum = 0;
	for(uint8_t i=0; i<size; i++)
	{
	  sum += buffer[i];  //累加
	}
	return(sum & 0xFF);     //返回校验和
}

void Decode485Motor(uint8_t *buffer, uint16_t length)
{
	//驱动器返回：FB 地址 功能码 01/00（运行成功/失败） 和校验
	//开环控制
}