Liews Wuttipat
dynamixel protocal by Liews Wuttipat.
Diff: Dynamixel.cpp
- Revision:
- 0:e4ba03a21caa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamixel.cpp Fri Dec 08 23:32:57 2017 +0000
@@ -0,0 +1,223 @@
+#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
+// }*/
+// }