dynamixel protocal by Liews Wuttipat.
Dynamixel.cpp
- Committer:
- sweilz
- Date:
- 2017-12-08
- Revision:
- 0:e4ba03a21caa
File content as of revision 0:e4ba03a21caa:
#include <mbed.h> #include <Dynamixel.h> uint8_t Instruction_Packet[15]; // Array to hold instruction packet data uint8_t Status_Packet[8]; // Array to hold returned status packet data uint8_t Status_Return_Value = READ; // Status package return states (NON, READ, ALL) Timer timer; //------------------------------------------------------------------------------------------------------------------------------- // Private Methods //------------------------------------------------------------------------------------------------------------------------------- void Dynamixel::writePacket(void) { if (dynamixelSerial->readable()) while (dynamixelSerial->readable()) dynamixelSerial->getc(); dynamixelDi->write(1); dynamixelSerial->putc(HEADER); dynamixelSerial->putc(HEADER); dynamixelSerial->putc(Instruction_Packet[0]); dynamixelSerial->putc(Instruction_Packet[1]); for (uint8_t i = 0; i < Instruction_Packet[1]; i++) { dynamixelSerial->putc(Instruction_Packet[2 + i]); } wait_us((Instruction_Packet[1] + 4) * 5); dynamixelDi->write(0); } unsigned int Dynamixel::readPacket(void) { // wait_us(250); // uint8_t Counter = 0; // uint8_t InBuff[20]; // uint8_t i = 0, j = 0, RxState = 0; // Status_Packet[0] = 0; // Status_Packet[1] = 0; // Status_Packet[2] = 0; // Status_Packet[3] = 0; // timer.start(); // int bytes = 2; // int timeout = 0; // int plen = 0; // // while (timer.read_ms() < 3000) // // { // // if (dynamixelSerial->readable()) // // { // // InBuff[plen] = dynamixelSerial->getc(); // // plen++; // // } // // } // while ((timeout < ((6 + bytes) * 10000)) && (plen < (6 + bytes))) // { // if (dynamixelSerial->readable()) // { // InBuff[plen] = dynamixelSerial->getc(); // plen++; // timeout = 0; // } // // wait for the bit period // wait_us(1); // timeout++; // } // timer.stop(); // for (int i = 0; i < plen; i++) // { // Status_Packet[i] = InBuff[i]; // printf("0x%X,", Status_Packet[i]); // } // printf("\r\n"); return 0x01; } //------------------------------------------------------------------------------------------------------------------------------- // Public Methods //------------------------------------------------------------------------------------------------------------------------------- Dynamixel::Dynamixel(PinName tx, PinName rx, int baud, PinName di) { dynamixelSerial = new Serial(tx, rx); dynamixelSerial->baud(baud); dynamixelDi = new DigitalOut(di); dynamixelDi->write(0); } Dynamixel::~Dynamixel(void) { if (dynamixelSerial != NULL) delete dynamixelSerial; } void Dynamixel::setMode(uint8_t ID, uint8_t Mode, uint16_t CW_limit, uint16_t CCW_limit) { uint8_t L_CW = (uint8_t)(CW_limit & 0xFF); uint8_t H_CW = (uint8_t)(CW_limit >> 8); uint8_t L_CCW = (uint8_t)(CCW_limit & 0xFF); uint8_t H_CCW = (uint8_t)(CCW_limit >> 8); Instruction_Packet[0] = ID; Instruction_Packet[1] = SET_MODE_LENGTH; Instruction_Packet[2] = COMMAND_WRITE_DATA; Instruction_Packet[3] = EEPROM_CW_ANGLE_LIMIT_L; Instruction_Packet[4] = L_CW; Instruction_Packet[5] = H_CW; Instruction_Packet[6] = L_CCW; Instruction_Packet[7] = H_CCW; Instruction_Packet[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L + L_CW + H_CW + L_CCW + H_CCW); writePacket(); } void Dynamixel::setLed(uint8_t ID, bool State) { Instruction_Packet[0] = ID; Instruction_Packet[1] = LED_LENGTH; Instruction_Packet[2] = COMMAND_WRITE_DATA; Instruction_Packet[3] = RAM_LED; Instruction_Packet[4] = State; Instruction_Packet[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + State); writePacket(); } void Dynamixel::setPosition(uint8_t ID, uint16_t Position, uint16_t Speed = 0) { uint8_t L_Position = (uint8_t)(Position & 0xFF); uint8_t H_Position = (uint8_t)(Position >> 8); uint8_t L_Speed = (uint8_t)(Speed & 0xFF); uint8_t H_Speed = (uint8_t)(Speed >> 8); Instruction_Packet[0] = ID; Instruction_Packet[1] = SERVO_GOAL_LENGTH; Instruction_Packet[2] = COMMAND_WRITE_DATA; Instruction_Packet[3] = RAM_GOAL_POSITION_L; Instruction_Packet[4] = L_Position; Instruction_Packet[5] = H_Position; Instruction_Packet[6] = L_Speed; Instruction_Packet[7] = H_Speed; Instruction_Packet[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + L_Position + H_Position + L_Speed + H_Speed); writePacket(); } void Dynamixel::setWheelSpeed(uint8_t ID, bool Direction, uint16_t Speed) { uint8_t L_Speed = (uint8_t)(Speed & 0xFF); uint8_t H_Speed = (uint8_t)(Direction ? (Speed >> 8) : (Speed >> 8) | 0x04); Instruction_Packet[0] = ID; Instruction_Packet[1] = WHEEL_LENGTH; Instruction_Packet[2] = COMMAND_WRITE_DATA; Instruction_Packet[3] = RAM_GOAL_SPEED_L; Instruction_Packet[4] = L_Speed; Instruction_Packet[5] = H_Speed; Instruction_Packet[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + L_Speed + H_Speed); writePacket(); } void Dynamixel::setWheel3Speed(uint8_t ID[], bool Direction[], uint16_t Speed[]) { uint8_t L_Speed[3], H_Speed[3]; L_Speed[0] = (uint8_t)(Speed[0] & 0xFF); H_Speed[0] = (uint8_t)(Direction[0] ? (Speed[0] >> 8) : (Speed[0] >> 8) | 0x04); L_Speed[1] = (uint8_t)(Speed[1] & 0xFF); H_Speed[1] = (uint8_t)(Direction[1] ? (Speed[1] >> 8) : (Speed[1] >> 8) | 0x04); L_Speed[2] = (uint8_t)(Speed[2] & 0xFF); H_Speed[2] = (uint8_t)(Direction[2] ? (Speed[2] >> 8) : (Speed[2] >> 8) | 0x04); Instruction_Packet[0] = 0xFE; Instruction_Packet[1] = SYNC_LOAD_LENGTH; Instruction_Packet[2] = COMMAND_SYNC_WRITE; Instruction_Packet[3] = RAM_GOAL_SPEED_L; Instruction_Packet[4] = SYNC_DATA_LENGTH; Instruction_Packet[5] = ID[0]; Instruction_Packet[6] = L_Speed[0]; Instruction_Packet[7] = H_Speed[0]; Instruction_Packet[8] = ID[1]; Instruction_Packet[9] = L_Speed[1]; Instruction_Packet[10] = H_Speed[1]; Instruction_Packet[11] = ID[2]; Instruction_Packet[12] = L_Speed[2]; Instruction_Packet[13] = H_Speed[2]; Instruction_Packet[14] = ~(0xFE + SYNC_LOAD_LENGTH + COMMAND_SYNC_WRITE + RAM_GOAL_SPEED_L + SYNC_DATA_LENGTH + ID[0] + L_Speed[0] + H_Speed[0] + ID[1] + L_Speed[1] + H_Speed[1] + ID[2] + L_Speed[2] + H_Speed[2]); writePacket(); } // unsigned int Dynamixel::ping(uint8_t ID) // { // Instruction_Packet[0] = ID; // Instruction_Packet[1] = PING_LENGTH; // Instruction_Packet[2] = COMMAND_PING; // Instruction_Packet[3] = ~(ID + PING_LENGTH + COMMAND_PING); // transmitInstructionPacket(); // // if ((ID == 0xFE) || (Status_Return_Value != ALL)) // // return 0x00; // // else // // { // readStatusPacket(); // return 0x01; // // if (Status_Packet[2] == 0) // // return (Status_Packet[0]); // // else // // return (Status_Packet[2] | 0xF000); // // } // } // unsigned int Dynamixel::getTemperature(uint8_t ID) // { // Instruction_Packet[0] = ID; // Instruction_Packet[1] = READ_TEMP_LENGTH; // Instruction_Packet[2] = COMMAND_READ_DATA; // Instruction_Packet[3] = RAM_PRESENT_TEMPERATURE; // Instruction_Packet[4] = READ_ONE_BYTE_LENGTH; // Instruction_Packet[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH); // transmitInstructionPacket(); // return readStatusPacket(); // // return Instruction_Packet[5]; // /* // if (Status_Packet[2] == 0) // { // If there is no status packet error return value // // return Status_Packet[3]; // return 0; // } // else // { // return 1; // // return (Status_Packet[2] | 0xF000); // If there is a error Returns error value // }*/ // }