Revision 0:e4ba03a21caa, committed 2017-12-08

Comitter:

sweilz

Date:

Fri Dec 08 23:32:57 2017 +0000

Commit message:

first commit.

Changed in this revision

diff -r 000000000000 -r e4ba03a21caa Dynamixel.cpp
--- /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
+//     }*/
+// }

diff -r 000000000000 -r e4ba03a21caa Dynamixel.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamixel.h	Fri Dec 08 23:32:57 2017 +0000
@@ -0,0 +1,152 @@
+#include <mbed.h>
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// define - Dynamixel Hex code table
+//-------------------------------------------------------------------------------------------------------------------------------
+// EEPROM AREA
+#define EEPROM_MODEL_NUMBER_L 0x00
+#define EEPROM_MODEL_NUMBER_H 0x01
+#define EEPROM_VERSION 0x02
+#define EEPROM_ID 0x03
+#define EEPROM_BAUD_RATE 0x04
+#define EEPROM_RETURN_DELAY_TIME 0x05
+#define EEPROM_CW_ANGLE_LIMIT_L 0x06
+#define EEPROM_CW_ANGLE_LIMIT_H 0x07
+#define EEPROM_CCW_ANGLE_LIMIT_L 0x08
+#define EEPROM_CCW_ANGLE_LIMIT_H 0x09
+#define EEPROM_DRIVE_MODE 0x0A
+#define EEPROM_LIMIT_TEMPERATURE 0x0B
+#define EEPROM_LOW_LIMIT_VOLTAGE 0x0C
+#define EEPROM_HIGN_LIMIT_VOLTAGE 0x0D
+#define EEPROM_MAX_TORQUE_L 0x0E
+#define EEPROM_MAX_TORQUE_H 0x0F
+#define EEPROM_RETURN_LEVEL 0x10
+#define EEPROM_ALARM_LED 0x11
+#define EEPROM_ALARM_SHUTDOWN 0x12
+#define EEPROM_MULTITURN_L 0x14
+#define EEPROM_MULTITURN_H 0x15
+#define EEPROM_RESOLUTION_DIV 0x16
+// RAM AREA
+#define RAM_TORQUE_ENABLE 0x18
+#define RAM_LED 0x19
+#define RAM_DERIVATIVE_GAIN 0x1A
+#define RAM_INTERGRAL_GAIN 0x1B
+#define RAM_PROPORTIONAL_GAIN 0x1C
+#define RAM_GOAL_POSITION_L 0x1E
+#define RAM_GOAL_POSITION_H 0x1F
+#define RAM_GOAL_SPEED_L 0x20
+#define RAM_GOAL_SPEED_H 0x21
+#define RAM_TORQUE_LIMIT_L 0x22
+#define RAM_TORQUE_LIMIT_H 0x23
+#define RAM_PRESENT_POSITION_L 0x24
+#define RAM_PRESENT_POSITION_H 0x25
+#define RAM_PRESENT_SPEED_L 0x26
+#define RAM_PRESENT_SPEED_H 0x27
+#define RAM_PRESENT_LOAD_L 0x28
+#define RAM_PRESENT_LOAD_H 0x29
+#define RAM_PRESENT_VOLTAGE 0x2A
+#define RAM_PRESENT_TEMPERATURE 0x2B
+#define RAM_REGISTER 0x2C
+#define RAM_MOVING 0x2E
+#define RAM_LOCK 0x2F
+#define RAM_PUNCH_L 0x30
+#define RAM_PUNCH_H 0x31
+#define RAM_SENSED_CURRENT_L 0x38
+#define RAM_SENSED_CURRENT_H 0x39
+#define RAM_CURRENT_L 0x44
+#define RAM_CURRENT_H 0x45
+#define RAM_TORQUE_CONTROL_MODE_ENABLE 0x46
+#define RAM_GOAL_TORQUE_L 0x47
+#define RAM_GOAL_TORQUE_H 0x48
+#define RAM_GOAL_ACCELERATION 0x49
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Instruction commands Set
+//-------------------------------------------------------------------------------------------------------------------------------
+#define COMMAND_PING 0x01
+#define COMMAND_READ_DATA 0x02
+#define COMMAND_WRITE_DATA 0x03
+#define COMMAND_REG_WRITE_DATA 0x04
+#define COMMAND_ACTION 0x05
+#define COMMAND_RESET 0x06
+#define COMMAND_SYNC_WRITE 0x83
+
+//-------------------------------------------------------------------------------------------------------------------------------
+//Instruction packet lengths
+#define READ_ONE_BYTE_LENGTH 0x01
+#define READ_TWO_BYTE_LENGTH 0x02
+#define RESET_LENGTH 0x02
+#define PING_LENGTH 0x02
+#define ACTION_LENGTH 0x02
+#define SET_ID_LENGTH 0x04
+#define SET_BD_LENGTH 0x04
+#define SET_RETURN_LEVEL_LENGTH 0x04
+#define READ_TEMP_LENGTH 0x04
+#define READ_POS_LENGTH 0x04
+#define READ_LOAD_LENGTH 0x04
+#define READ_SPEED_LENGTH 0x04
+#define READ_VOLT_LENGTH 0x04
+#define READ_REGISTER_LENGTH 0x04
+#define READ_MOVING_LENGTH 0x04
+#define READ_LOCK_LENGTH 0x04
+#define LED_LENGTH 0x04
+#define SET_HOLDING_TORQUE_LENGTH 0x04
+#define SET_MAX_TORQUE_LENGTH 0x05
+#define SET_ALARM_LENGTH 0x04
+#define READ_LOAD_LENGTH 0x04
+#define SET_RETURN_LENGTH 0x04
+#define WHEEL_LENGTH 0x05
+#define SERVO_GOAL_LENGTH 0x07
+#define SET_MODE_LENGTH 0x07
+#define SET_PUNCH_LENGTH 0x04
+#define SET_PID_LENGTH 0x06
+#define SET_TEMP_LENGTH 0x04
+#define SET_VOLT_LENGTH 0x05
+#define SYNC_LOAD_LENGTH 0x0D
+#define SYNC_DATA_LENGTH 0x02
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Specials
+//-------------------------------------------------------------------------------------------------------------------------------
+
+#define OFF 0x00
+#define ON 0x01
+
+#define SERVO 0x01
+#define WHEEL 0x00
+
+#define LEFT 0x00
+#define RIGHT 0x01
+
+#define NONE 0x00
+#define READ 0x01
+#define ALL 0x02
+
+#define BROADCAST_ID 0xFE
+
+#define HEADER 0xFF
+
+#define STATUS_PACKET_TIMEOUT 50 // in millis()
+#define STATUS_FRAME_BUFFER 5
+
+class Dynamixel
+{
+private:
+    DigitalOut *dynamixelDi;
+    Serial *dynamixelSerial;
+    void writePacket(void);
+    unsigned int readPacket(void);
+
+
+public:
+    Dynamixel(PinName tx, PinName rx, int baud, PinName di);
+    ~Dynamixel(void);
+
+    void setMode(uint8_t ID, uint8_t Mode, uint16_t CW_limit = 0, uint16_t CCW_limit = 0);
+
+    void setLed(uint8_t ID, bool State);
+    void setPosition(uint8_t ID, uint16_t Position, uint16_t Speed);
+    void setWheelSpeed(uint8_t ID, bool Direction, uint16_t Speed);
+    void setWheel3Speed(uint8_t ID[], bool Direction[], uint16_t Speed[]);
+
+};