
#include <stdint.h>
#include <main.h>

extern UART_HandleTypeDef huart1;

void dynamixel_proto1_write (uint8_t position)
	
{
	uint8_t dyn1_trqen[] = { 0xFF, 0xFF, 0x01, 0x04, 0x03, 0x40, 0x01, 0xB6 };
	uint8_t dyn1_trqen1[] = { 0xFF, 0xFF, 0x01, 0x04, 0x02, 0x40, 0x01, 0xB7 };
	uint8_t dyn1_write[11];
	
	uint8_t ID = 0x01;
	uint8_t LEN = 0x07;
	uint8_t INST = 0x03;
	uint8_t P1 = 0x74;
	uint8_t P2 = 0x00;
	uint8_t P3 = 0x02;
	uint8_t P4 = 0x00;
	uint8_t P5 = 0x00;
	
	dyn1_write[0] = 0xFF;
	dyn1_write[1] = 0xFF;
	dyn1_write[2] = ID;
	dyn1_write[3] = LEN;
	dyn1_write[4] = INST;
	dyn1_write[5] = P1;
	dyn1_write[6] = P2;
	dyn1_write[7] = P3;
	dyn1_write[8] = P4;
	dyn1_write[9] = P5;
	uint8_t CKSM = ~(ID+LEN+INST+P1+P2+P3+P4+P5);
	dyn1_write[10] = CKSM;
	
	uint8_t* pTrq = dyn1_trqen;
	uint8_t* pTrq1 = dyn1_trqen1;
	uint8_t* pData = dyn1_write;
	uint16_t sTrq = 8;
	uint16_t sTrq1 = 8; 
	uint16_t Size = 11;
	uint32_t Timeout = 10;
	
	HAL_UART_Transmit(&huart1, pTrq, sTrq, Timeout);
	HAL_Delay(10);
	HAL_UART_Transmit(&huart1, pTrq1, sTrq1, Timeout);
	HAL_Delay(10);
	HAL_UART_Transmit(&huart1, pData, Size, Timeout);

}