Dan Gongora / Dynamixel

mbed to Dynamixel servo communication.

Dependents:   dynamixel_wheel_test

Dynamixel.cpp@0:fb8a2d249639, 2015-08-19 (annotated)

Committer:
dmgongora
Date:
Wed Aug 19 05:39:57 2015 +0000
Revision:
0:fb8a2d249639
Child:
1:ce081666d225
Dynamixel files packed into a library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dmgongora 0:fb8a2d249639 1 #include "Dynamixel.h"
dmgongora 0:fb8a2d249639 2 #include "mbed.h"
dmgongora 0:fb8a2d249639 3
dmgongora 0:fb8a2d249639 4 Serial pc(USBTX, USBRX);
dmgongora 0:fb8a2d249639 5
dmgongora 0:fb8a2d249639 6 Dynamixel::Dynamixel(PinName tx, PinName rx, PinName txEnable, uint8_t motorID, int baudrate) : m_link(tx, rx), m_txEnable(txEnable), m_motorID(motorID), m_baudrate(baudrate)
dmgongora 0:fb8a2d249639 7 {
dmgongora 0:fb8a2d249639 8 m_link.baud(m_baudrate);
dmgongora 0:fb8a2d249639 9 }
dmgongora 0:fb8a2d249639 10
dmgongora 0:fb8a2d249639 11 void Dynamixel::ping()
dmgongora 0:fb8a2d249639 12 {
dmgongora 0:fb8a2d249639 13 uint8_t elements = 6;
dmgongora 0:fb8a2d249639 14 uint8_t packetBuffer[elements];
dmgongora 0:fb8a2d249639 15 if (m_link.writeable() ) {
dmgongora 0:fb8a2d249639 16 packetBuffer[0] = 0xff;
dmgongora 0:fb8a2d249639 17 packetBuffer[1] = 0xff;
dmgongora 0:fb8a2d249639 18 packetBuffer[2] = m_motorID; // ID
dmgongora 0:fb8a2d249639 19 packetBuffer[3] = 0x02; // Length
dmgongora 0:fb8a2d249639 20 packetBuffer[4] = PING; // Instruction
dmgongora 0:fb8a2d249639 21 packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 22
dmgongora 0:fb8a2d249639 23 pc.printf("\n Instruction packet:\n");
dmgongora 0:fb8a2d249639 24 for (int i = 0; i<= elements; i++) {
dmgongora 0:fb8a2d249639 25 pc.printf("0x%x\t", packetBuffer[i]);
dmgongora 0:fb8a2d249639 26 }
dmgongora 0:fb8a2d249639 27
dmgongora 0:fb8a2d249639 28 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 29 for (int i = 0; i<= elements; i++) {
dmgongora 0:fb8a2d249639 30 m_link.putc(packetBuffer[i]);
dmgongora 0:fb8a2d249639 31 }
dmgongora 0:fb8a2d249639 32 wait_ms(1); // fix this!!!
dmgongora 0:fb8a2d249639 33 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 34 } else {
dmgongora 0:fb8a2d249639 35 pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 36 }
dmgongora 0:fb8a2d249639 37 //pc.baud(9600);
dmgongora 0:fb8a2d249639 38 wait_ms(10);
dmgongora 0:fb8a2d249639 39 pc.printf("\n Status packet:\n");
dmgongora 0:fb8a2d249639 40 while ( m_link.readable() ) {
dmgongora 0:fb8a2d249639 41 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:fb8a2d249639 42 }
dmgongora 0:fb8a2d249639 43 pc.printf("\n PING: Done.\n");
dmgongora 0:fb8a2d249639 44 }
dmgongora 0:fb8a2d249639 45
dmgongora 0:fb8a2d249639 46 void Dynamixel::toggleLED(uint8_t ledState)
dmgongora 0:fb8a2d249639 47 {
dmgongora 0:fb8a2d249639 48 uint8_t elements = 8;
dmgongora 0:fb8a2d249639 49 uint8_t packetBuffer[elements];
dmgongora 0:fb8a2d249639 50 if (m_link.writeable() ) {
dmgongora 0:fb8a2d249639 51 packetBuffer[0] = 0xff;
dmgongora 0:fb8a2d249639 52 packetBuffer[1] = 0xff;
dmgongora 0:fb8a2d249639 53 packetBuffer[2] = m_motorID; // ID
dmgongora 0:fb8a2d249639 54 packetBuffer[3] = 0x04; // Length
dmgongora 0:fb8a2d249639 55 packetBuffer[4] = WRITE_DATA; // Instruction
dmgongora 0:fb8a2d249639 56 packetBuffer[5] = ADDRESS_LED; // Parameter 1: Starting address
dmgongora 0:fb8a2d249639 57 packetBuffer[6] = ledState; // Parameter 2: First value to be writen
dmgongora 0:fb8a2d249639 58 packetBuffer[7] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 59
dmgongora 0:fb8a2d249639 60 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 61 for (int i = 0; i<= elements; i++) {
dmgongora 0:fb8a2d249639 62 m_link.putc(packetBuffer[i]);
dmgongora 0:fb8a2d249639 63 }
dmgongora 0:fb8a2d249639 64 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 65 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 66 } else {
dmgongora 0:fb8a2d249639 67 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 68 }
dmgongora 0:fb8a2d249639 69 wait_ms(10);
dmgongora 0:fb8a2d249639 70 pc.printf("\n Status packet:\n");
dmgongora 0:fb8a2d249639 71 while ( m_link.readable() ) {
dmgongora 0:fb8a2d249639 72 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:fb8a2d249639 73 }
dmgongora 0:fb8a2d249639 74 }
dmgongora 0:fb8a2d249639 75
dmgongora 0:fb8a2d249639 76 void Dynamixel::move(uint16_t position)
dmgongora 0:fb8a2d249639 77 {
dmgongora 0:fb8a2d249639 78 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 79 uint8_t elements = 9;
dmgongora 0:fb8a2d249639 80 uint8_t packetBuffer[elements];
dmgongora 0:fb8a2d249639 81 uint16_t checkSum = 0;
dmgongora 0:fb8a2d249639 82 if (m_link.writeable() ) {
dmgongora 0:fb8a2d249639 83 packetBuffer[0] = 0xff;
dmgongora 0:fb8a2d249639 84 packetBuffer[1] = 0xff;
dmgongora 0:fb8a2d249639 85 packetBuffer[2] = m_motorID; // ID
dmgongora 0:fb8a2d249639 86 packetBuffer[3] = 0x05; // Length
dmgongora 0:fb8a2d249639 87 packetBuffer[4] = WRITE_DATA; // Instruction
dmgongora 0:fb8a2d249639 88 packetBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address
dmgongora 0:fb8a2d249639 89 packetBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen
dmgongora 0:fb8a2d249639 90 packetBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen
dmgongora 0:fb8a2d249639 91 checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7];
dmgongora 0:fb8a2d249639 92 packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
dmgongora 0:fb8a2d249639 93
dmgongora 0:fb8a2d249639 94 /*
dmgongora 0:fb8a2d249639 95 for (int i = 0; i<= elements; i++) {
dmgongora 0:fb8a2d249639 96 pc.printf("%c ", packetBuffer[i]);
dmgongora 0:fb8a2d249639 97 }
dmgongora 0:fb8a2d249639 98 pc.printf("\n");
dmgongora 0:fb8a2d249639 99 */
dmgongora 0:fb8a2d249639 100 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 101 for (int i = 0; i<= elements; i++) {
dmgongora 0:fb8a2d249639 102 m_link.putc(packetBuffer[i]);
dmgongora 0:fb8a2d249639 103 }
dmgongora 0:fb8a2d249639 104 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 105 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 106 } else {
dmgongora 0:fb8a2d249639 107 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 108 }
dmgongora 0:fb8a2d249639 109 wait_ms(10);
dmgongora 0:fb8a2d249639 110 pc.printf("\n Status packet:\n");
dmgongora 0:fb8a2d249639 111 while ( m_link.readable() ) {
dmgongora 0:fb8a2d249639 112 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:fb8a2d249639 113 }
dmgongora 0:fb8a2d249639 114 }
dmgongora 0:fb8a2d249639 115
dmgongora 0:fb8a2d249639 116 void Dynamixel::setSpeed(uint16_t speed)
dmgongora 0:fb8a2d249639 117 {
dmgongora 0:fb8a2d249639 118 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 119 uint8_t elements = 9;
dmgongora 0:fb8a2d249639 120 uint8_t packetBuffer[elements];
dmgongora 0:fb8a2d249639 121 uint16_t checkSum = 0;
dmgongora 0:fb8a2d249639 122 if (m_link.writeable() ) {
dmgongora 0:fb8a2d249639 123 packetBuffer[0] = 0xff;
dmgongora 0:fb8a2d249639 124 packetBuffer[1] = 0xff;
dmgongora 0:fb8a2d249639 125 packetBuffer[2] = m_motorID; // ID
dmgongora 0:fb8a2d249639 126 packetBuffer[3] = 0x05; // Length
dmgongora 0:fb8a2d249639 127 packetBuffer[4] = WRITE_DATA; // Instruction
dmgongora 0:fb8a2d249639 128 packetBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address
dmgongora 0:fb8a2d249639 129 packetBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen
dmgongora 0:fb8a2d249639 130 packetBuffer[7] = (uint8_t) (speed >> 8); // Parameter 3: Second value to be writen
dmgongora 0:fb8a2d249639 131 checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7];
dmgongora 0:fb8a2d249639 132 packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
dmgongora 0:fb8a2d249639 133
dmgongora 0:fb8a2d249639 134 /*
dmgongora 0:fb8a2d249639 135 for (int i = 0; i<= elements; i++) {
dmgongora 0:fb8a2d249639 136 pc.printf("%c ", packetBuffer[i]);
dmgongora 0:fb8a2d249639 137 }
dmgongora 0:fb8a2d249639 138 pc.printf("\n");
dmgongora 0:fb8a2d249639 139 */
dmgongora 0:fb8a2d249639 140 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 141 for (int i = 0; i<= elements; i++) {
dmgongora 0:fb8a2d249639 142 m_link.putc(packetBuffer[i]);
dmgongora 0:fb8a2d249639 143 }
dmgongora 0:fb8a2d249639 144 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 145 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 146 } else {
dmgongora 0:fb8a2d249639 147 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 148 }
dmgongora 0:fb8a2d249639 149 wait_ms(10);
dmgongora 0:fb8a2d249639 150 pc.printf("\n Status packet:\n");
dmgongora 0:fb8a2d249639 151 while ( m_link.readable() ) {
dmgongora 0:fb8a2d249639 152 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:fb8a2d249639 153 }
dmgongora 0:fb8a2d249639 154 }