void wheel(unsigned char ID, bool Rotation,unsigned int Speed) { byte Speed_H,Speed_L; Speed_L = Speed; if (Rotation == 0){ // Move Left Speed_H = Speed >> 8; }else if (Rotation == 1){ // Move Right Speed_H = (Speed >> 8)+4; } Dynamixel_data[0]=HEADER; // start Dynamixel_data[1]=HEADER; Dynamixel_data[2] = ID; Dynamixel_data[3] = WHEEL_LENGTH; Dynamixel_data[4] = COMMAND_WRITE_DATA; Dynamixel_data[5] = RAM_GOAL_SPEED_L; Dynamixel_data[6] = Speed_L; Dynamixel_data[7] = Speed_H; Dynamixel_data[8]= ~(Dynamixel_data[2]+Dynamixel_data[3]+Dynamixel_data[4]+Dynamixel_data[5]+Dynamixel_data[6]+Dynamixel_data[7]);// Check Sum string msg; msg = ByteArrayToStr(Dynamixel_data);// convert to string SendRS485String(msg);//send WaitForMessageToBeSent(); }