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@1:ef90d942ce78, 2015-08-19 (annotated)

Committer:
dmgongora
Date:
Wed Aug 19 05:36:55 2015 +0000
Revision:
1:ef90d942ce78
Parent:
0:79e2a8171b16 
Added method to set the velocity but it's not working.

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 1:ef90d942ce78 81 uint16_t checkSum = 0;
dmgongora 0:79e2a8171b16 82 if (m_link.writeable() ) {
dmgongora 0:79e2a8171b16 83 packetBuffer[0] = 0xff;
dmgongora 0:79e2a8171b16 84 packetBuffer[1] = 0xff;
dmgongora 1:ef90d942ce78 85 packetBuffer[2] = m_motorID; // ID
dmgongora 1:ef90d942ce78 86 packetBuffer[3] = 0x05; // Length
dmgongora 1:ef90d942ce78 87 packetBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ef90d942ce78 88 packetBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address
dmgongora 0:79e2a8171b16 89 packetBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen
dmgongora 0:79e2a8171b16 90 packetBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen
dmgongora 1:ef90d942ce78 91 checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7];
dmgongora 1:ef90d942ce78 92 packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
dmgongora 1:ef90d942ce78 93
dmgongora 1:ef90d942ce78 94 /*
dmgongora 1:ef90d942ce78 95 for (int i = 0; i<= elements; i++) {
dmgongora 1:ef90d942ce78 96 pc.printf("%c ", packetBuffer[i]);
dmgongora 1:ef90d942ce78 97 }
dmgongora 1:ef90d942ce78 98 pc.printf("\n");
dmgongora 1:ef90d942ce78 99 */
dmgongora 1:ef90d942ce78 100 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 1:ef90d942ce78 101 for (int i = 0; i<= elements; i++) {
dmgongora 1:ef90d942ce78 102 m_link.putc(packetBuffer[i]);
dmgongora 1:ef90d942ce78 103 }
dmgongora 1:ef90d942ce78 104 wait_ms(2); // fix this!!!
dmgongora 1:ef90d942ce78 105 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 1:ef90d942ce78 106 } else {
dmgongora 1:ef90d942ce78 107 //pc.printf("Dynamixel not writeable\n");
dmgongora 1:ef90d942ce78 108 }
dmgongora 1:ef90d942ce78 109 wait_ms(10);
dmgongora 1:ef90d942ce78 110 pc.printf("\n Status packet:\n");
dmgongora 1:ef90d942ce78 111 while ( m_link.readable() ) {
dmgongora 1:ef90d942ce78 112 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 1:ef90d942ce78 113 }
dmgongora 1:ef90d942ce78 114 }
dmgongora 1:ef90d942ce78 115
dmgongora 1:ef90d942ce78 116 void Dynamixel::setSpeed(uint16_t speed)
dmgongora 1:ef90d942ce78 117 {
dmgongora 1:ef90d942ce78 118 // 0 to 1023 (0x3FF)
dmgongora 1:ef90d942ce78 119 uint8_t elements = 9;
dmgongora 1:ef90d942ce78 120 uint8_t packetBuffer[elements];
dmgongora 1:ef90d942ce78 121 uint16_t checkSum = 0;
dmgongora 1:ef90d942ce78 122 if (m_link.writeable() ) {
dmgongora 1:ef90d942ce78 123 packetBuffer[0] = 0xff;
dmgongora 1:ef90d942ce78 124 packetBuffer[1] = 0xff;
dmgongora 1:ef90d942ce78 125 packetBuffer[2] = m_motorID; // ID
dmgongora 1:ef90d942ce78 126 packetBuffer[3] = 0x05; // Length
dmgongora 1:ef90d942ce78 127 packetBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ef90d942ce78 128 packetBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address
dmgongora 1:ef90d942ce78 129 packetBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen
dmgongora 1:ef90d942ce78 130 packetBuffer[7] = (uint8_t) (speed >> 8); // Parameter 3: Second value to be writen
dmgongora 1:ef90d942ce78 131 checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7];
dmgongora 1:ef90d942ce78 132 packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
dmgongora 0:79e2a8171b16 133
dmgongora 0:79e2a8171b16 134 /*
dmgongora 0:79e2a8171b16 135 for (int i = 0; i<= elements; i++) {
dmgongora 0:79e2a8171b16 136 pc.printf("%c ", packetBuffer[i]);
dmgongora 0:79e2a8171b16 137 }
dmgongora 0:79e2a8171b16 138 pc.printf("\n");
dmgongora 0:79e2a8171b16 139 */
dmgongora 0:79e2a8171b16 140 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:79e2a8171b16 141 for (int i = 0; i<= elements; i++) {
dmgongora 0:79e2a8171b16 142 m_link.putc(packetBuffer[i]);
dmgongora 0:79e2a8171b16 143 }
dmgongora 0:79e2a8171b16 144 wait_ms(2); // fix this!!!
dmgongora 0:79e2a8171b16 145 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:79e2a8171b16 146 } else {
dmgongora 0:79e2a8171b16 147 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:79e2a8171b16 148 }
dmgongora 0:79e2a8171b16 149 wait_ms(10);
dmgongora 0:79e2a8171b16 150 pc.printf("\n Status packet:\n");
dmgongora 0:79e2a8171b16 151 while ( m_link.readable() ) {
dmgongora 0:79e2a8171b16 152 pc.printf("0x%x\t", m_link.getc()); // Read status packet if available
dmgongora 0:79e2a8171b16 153 }
dmgongora 0:79e2a8171b16 154 }