hisyam fs / Mbed 2 deprecated Test_all

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ADS1115 StepperMotor SRF05 TPA81new

Dynamixel.cpp@0:79e2a8171b16, 2015-08-18 (annotated)

Committer:
dmgongora
Date:
Tue Aug 18 09:15:09 2015 +0000
Revision:
0:79e2a8171b16
Child:
1:ef90d942ce78
First working version of the Dynamixel class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dmgongora 0:79e2a8171b16 1 #include "Dynamixel.h"
dmgongora 0:79e2a8171b16 2 #include "mbed.h"
dmgongora 0:79e2a8171b16 3
dmgongora 0:79e2a8171b16 4 Serial pc(USBTX, USBRX);
dmgongora 0:79e2a8171b16 5
dmgongora 0:79e2a8171b16 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:79e2a8171b16 7 {
dmgongora 0:79e2a8171b16 8 m_link.baud(m_baudrate);
dmgongora 0:79e2a8171b16 9 }
dmgongora 0:79e2a8171b16 10
dmgongora 0:79e2a8171b16 11 void Dynamixel::ping()
dmgongora 0:79e2a8171b16 12 {
dmgongora 0:79e2a8171b16 13 uint8_t elements = 6;
dmgongora 0:79e2a8171b16 14 uint8_t packetBuffer[elements];
dmgongora 0:79e2a8171b16 15 if (m_link.writeable() ) {
dmgongora 0:79e2a8171b16 16 packetBuffer[0] = 0xff;
dmgongora 0:79e2a8171b16 17 packetBuffer[1] = 0xff;
dmgongora 0:79e2a8171b16 18 packetBuffer[2] = m_motorID; // ID
dmgongora 0:79e2a8171b16 19 packetBuffer[3] = 0x02; // Length
dmgongora 0:79e2a8171b16 20 packetBuffer[4] = PING; // Instruction
dmgongora 0:79e2a8171b16 21 packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF; // Check sum
dmgongora 0:79e2a8171b16 22
dmgongora 0:79e2a8171b16 23 pc.printf("\n Instruction packet:\n");
dmgongora 0:79e2a8171b16 24 for (int i = 0; i<= elements; i++) {
dmgongora 0:79e2a8171b16 25 pc.printf("0x%x\t", packetBuffer[i]);
dmgongora 0:79e2a8171b16 26 }
dmgongora 0:79e2a8171b16 27
dmgongora 0:79e2a8171b16 28 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:79e2a8171b16 29 for (int i = 0; i<= elements; i++) {
dmgongora 0:79e2a8171b16 30 m_link.putc(packetBuffer[i]);
dmgongora 0:79e2a8171b16 31 }
dmgongora 0:79e2a8171b16 32 wait_ms(1); // fix this!!!
dmgongora 0:79e2a8171b16 33 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:79e2a8171b16 34 } else {
dmgongora 0:79e2a8171b16 35 pc.printf("Dynamixel not writeable\n");
dmgongora 0:79e2a8171b16 36 }
dmgongora 0:79e2a8171b16 37 //pc.baud(9600);
dmgongora 0:79e2a8171b16 38 wait_ms(10);
dmgongora 0:79e2a8171b16 39 pc.printf("\n Status packet:\n");
dmgongora 0:79e2a8171b16 40 while ( m_link.readable() ) {
dmgongora 0:79e2a8171b16 41 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:79e2a8171b16 42 }
dmgongora 0:79e2a8171b16 43 pc.printf("\n PING: Done.\n");
dmgongora 0:79e2a8171b16 44 }
dmgongora 0:79e2a8171b16 45
dmgongora 0:79e2a8171b16 46 void Dynamixel::toggleLED(uint8_t ledState)
dmgongora 0:79e2a8171b16 47 {
dmgongora 0:79e2a8171b16 48 uint8_t elements = 8;
dmgongora 0:79e2a8171b16 49 uint8_t packetBuffer[elements];
dmgongora 0:79e2a8171b16 50 if (m_link.writeable() ) {
dmgongora 0:79e2a8171b16 51 packetBuffer[0] = 0xff;
dmgongora 0:79e2a8171b16 52 packetBuffer[1] = 0xff;
dmgongora 0:79e2a8171b16 53 packetBuffer[2] = m_motorID; // ID
dmgongora 0:79e2a8171b16 54 packetBuffer[3] = 0x04; // Length
dmgongora 0:79e2a8171b16 55 packetBuffer[4] = WRITE_DATA; // Instruction
dmgongora 0:79e2a8171b16 56 packetBuffer[5] = ADDRESS_LED; // Parameter 1: Starting address
dmgongora 0:79e2a8171b16 57 packetBuffer[6] = ledState; // Parameter 2: First value to be writen
dmgongora 0:79e2a8171b16 58 packetBuffer[7] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6]) & 0xFF; // Check sum
dmgongora 0:79e2a8171b16 59
dmgongora 0:79e2a8171b16 60 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:79e2a8171b16 61 for (int i = 0; i<= elements; i++) {
dmgongora 0:79e2a8171b16 62 m_link.putc(packetBuffer[i]);
dmgongora 0:79e2a8171b16 63 }
dmgongora 0:79e2a8171b16 64 wait_ms(2); // fix this!!!
dmgongora 0:79e2a8171b16 65 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:79e2a8171b16 66 } else {
dmgongora 0:79e2a8171b16 67 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:79e2a8171b16 68 }
dmgongora 0:79e2a8171b16 69 wait_ms(10);
dmgongora 0:79e2a8171b16 70 pc.printf("\n Status packet:\n");
dmgongora 0:79e2a8171b16 71 while ( m_link.readable() ) {
dmgongora 0:79e2a8171b16 72 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:79e2a8171b16 73 }
dmgongora 0:79e2a8171b16 74 }
dmgongora 0:79e2a8171b16 75
dmgongora 0:79e2a8171b16 76 void Dynamixel::move(uint16_t position)
dmgongora 0:79e2a8171b16 77 {
dmgongora 0:79e2a8171b16 78 // 0 to 1023 (0x3FF)
dmgongora 0:79e2a8171b16 79 uint8_t elements = 9;
dmgongora 0:79e2a8171b16 80 uint8_t packetBuffer[elements];
dmgongora 0:79e2a8171b16 81 if (m_link.writeable() ) {
dmgongora 0:79e2a8171b16 82 packetBuffer[0] = 0xff;
dmgongora 0:79e2a8171b16 83 packetBuffer[1] = 0xff;
dmgongora 0:79e2a8171b16 84 packetBuffer[2] = m_motorID; // ID
dmgongora 0:79e2a8171b16 85 packetBuffer[3] = 0x05; // Length
dmgongora 0:79e2a8171b16 86 packetBuffer[4] = WRITE_DATA; // Instruction
dmgongora 0:79e2a8171b16 87 packetBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address
dmgongora 0:79e2a8171b16 88 packetBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen
dmgongora 0:79e2a8171b16 89 packetBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen
dmgongora 0:79e2a8171b16 90 packetBuffer[8] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + + packetBuffer[7]) & 0xff; // Check sum
dmgongora 0:79e2a8171b16 91
dmgongora 0:79e2a8171b16 92 /*
dmgongora 0:79e2a8171b16 93 for (int i = 0; i<= elements; i++) {
dmgongora 0:79e2a8171b16 94 pc.printf("%c ", packetBuffer[i]);
dmgongora 0:79e2a8171b16 95 }
dmgongora 0:79e2a8171b16 96 pc.printf("\n");
dmgongora 0:79e2a8171b16 97 */
dmgongora 0:79e2a8171b16 98 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:79e2a8171b16 99 for (int i = 0; i<= elements; i++) {
dmgongora 0:79e2a8171b16 100 m_link.putc(packetBuffer[i]);
dmgongora 0:79e2a8171b16 101 }
dmgongora 0:79e2a8171b16 102 wait_ms(2); // fix this!!!
dmgongora 0:79e2a8171b16 103 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:79e2a8171b16 104 } else {
dmgongora 0:79e2a8171b16 105 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:79e2a8171b16 106 }
dmgongora 0:79e2a8171b16 107 wait_ms(10);
dmgongora 0:79e2a8171b16 108 pc.printf("\n Status packet:\n");
dmgongora 0:79e2a8171b16 109 while ( m_link.readable() ) {
dmgongora 0:79e2a8171b16 110 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:79e2a8171b16 111 }
dmgongora 0:79e2a8171b16 112 }