#include "main.h"

int ID = 0x04; // 0-252 0x00-0xFC
int len1 = 0x09; // number of parameters + 3
int len2 = 0x00;
int instruction = 0x03; //0x03  write
uint8_t param1 = 0x74;  
uint8_t param2 = 0x00;  
uint8_t param3 = 0x00;  
uint8_t param4 = 0x02;  
uint8_t param5 = 0x00;  
uint8_t param6 = 0x00;  
uint8_t crc_L = 0xCA;
uint8_t crc_H = 0x89;

uint8_t dynamixel[15];

dynamixel[0] = 0xFF;
dynamixel[1] = 0xFF;
dynamixel[2] = 0xFD;
dynamixel[3] = 0x00;
dynamixel[4] = ID;
dynamixel[5] = len1;
dynamixel[6] = len2;
dynamixel[7] = instruction;
dynamixel[8] = param1;
dynamixel[9] = param2;
dynamixel[10] = param3;
dynamixel[11] = param4;
dynamixel[12] = param5;
dynamixel[13] = param6;
dynamixel[14] = crc_L;
dynamixel[15] = crc_H;

uint8_t dynamixel_write(void)
{
	for (int i = 0; i <= 15; i++)
	{
		HAL_UART_Transmit(&huart1, pData, Size, Timeout);
	}
	
}
