Xavier Bouvard / herkulex

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.

Fork of herkulex by Yoonseok Pyo

herkulex.cpp@1:874d1f42989c, 2013-01-14 (annotated)

Committer:
passionvirus
Date:
Mon Jan 14 06:46:31 2013 +0000
Revision:
1:874d1f42989c
Parent:
0:0eef242852bb
Child:
5:f737e5c70115
change licence

Who changed what in which revision?

UserRevisionLine numberNew contents of line
passionvirus 0:0eef242852bb 1 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 2 /* herkulex servo library for mbed
passionvirus 0:0eef242852bb 3 *
passionvirus 1:874d1f42989c 4 * Copyright (c) 2012-2013 Yoonseok Pyo, MIT License
passionvirus 0:0eef242852bb 5 *
passionvirus 1:874d1f42989c 6 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
passionvirus 1:874d1f42989c 7 * and associated documentation files (the "Software"), to deal in the Software without restriction,
passionvirus 1:874d1f42989c 8 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
passionvirus 1:874d1f42989c 9 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
passionvirus 1:874d1f42989c 10 * furnished to do so, subject to the following conditions:
passionvirus 0:0eef242852bb 11 *
passionvirus 1:874d1f42989c 12 * The above copyright notice and this permission notice shall be included in all copies or
passionvirus 1:874d1f42989c 13 * substantial portions of the Software.
passionvirus 0:0eef242852bb 14 *
passionvirus 1:874d1f42989c 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
passionvirus 1:874d1f42989c 16 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
passionvirus 1:874d1f42989c 17 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
passionvirus 1:874d1f42989c 18 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
passionvirus 1:874d1f42989c 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
passionvirus 0:0eef242852bb 20 */
passionvirus 0:0eef242852bb 21 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 22 #include "mbed.h"
passionvirus 0:0eef242852bb 23 #include "herkulex.h"
passionvirus 0:0eef242852bb 24
passionvirus 0:0eef242852bb 25 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 26 Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
passionvirus 0:0eef242852bb 27 {
passionvirus 0:0eef242852bb 28 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 29 pc = new Serial(USBTX, USBRX);
passionvirus 0:0eef242852bb 30 pc->baud(57600);
passionvirus 0:0eef242852bb 31 pc->printf("Herkulex Init!\n");
passionvirus 0:0eef242852bb 32 #endif
passionvirus 0:0eef242852bb 33
passionvirus 0:0eef242852bb 34 txd = new Serial(tx, NC);
passionvirus 0:0eef242852bb 35 rxd = new Serial(NC, rx);
passionvirus 0:0eef242852bb 36
passionvirus 0:0eef242852bb 37 txd->baud(baudRate);
passionvirus 0:0eef242852bb 38 rxd->baud(baudRate);
passionvirus 0:0eef242852bb 39 }
passionvirus 0:0eef242852bb 40
passionvirus 0:0eef242852bb 41 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 42 Herkulex::~Herkulex()
passionvirus 0:0eef242852bb 43 {
passionvirus 0:0eef242852bb 44 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 45 if(pc != NULL)
passionvirus 0:0eef242852bb 46 delete pc;
passionvirus 0:0eef242852bb 47 #endif
passionvirus 0:0eef242852bb 48
passionvirus 0:0eef242852bb 49 if(txd != NULL)
passionvirus 0:0eef242852bb 50 delete txd;
passionvirus 0:0eef242852bb 51 if(rxd != NULL)
passionvirus 0:0eef242852bb 52 delete rxd;
passionvirus 0:0eef242852bb 53 }
passionvirus 0:0eef242852bb 54
passionvirus 0:0eef242852bb 55 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 56 void Herkulex::txPacket(uint8_t packetSize, uint8_t* data)
passionvirus 0:0eef242852bb 57 {
passionvirus 0:0eef242852bb 58 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 59 pc->printf("[TX]");
passionvirus 0:0eef242852bb 60 #endif
passionvirus 0:0eef242852bb 61
passionvirus 0:0eef242852bb 62 for(uint8_t i = 0; i < packetSize ; i++)
passionvirus 0:0eef242852bb 63 {
passionvirus 0:0eef242852bb 64 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 65 pc->printf("%02X ",data[i]);
passionvirus 0:0eef242852bb 66 #endif
passionvirus 0:0eef242852bb 67
passionvirus 0:0eef242852bb 68 txd->putc(data[i]);
passionvirus 0:0eef242852bb 69 }
passionvirus 0:0eef242852bb 70
passionvirus 0:0eef242852bb 71 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 72 pc->printf("\n");
passionvirus 0:0eef242852bb 73 #endif
passionvirus 0:0eef242852bb 74 }
passionvirus 0:0eef242852bb 75
passionvirus 0:0eef242852bb 76 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 77 void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
passionvirus 0:0eef242852bb 78 {
passionvirus 0:0eef242852bb 79 uint8_t txBuf[10];
passionvirus 0:0eef242852bb 80
passionvirus 0:0eef242852bb 81 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 82 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 83 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
passionvirus 0:0eef242852bb 84 txBuf[3] = id; // Servo ID
passionvirus 0:0eef242852bb 85 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
passionvirus 0:0eef242852bb 86 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 87 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 88 txBuf[7] = RAM_TORQUE_CONTROL; // Address 52
passionvirus 0:0eef242852bb 89 txBuf[8] = BYTE1; // Length
passionvirus 0:0eef242852bb 90 txBuf[9] = cmdTorue; // Torque ON
passionvirus 0:0eef242852bb 91
passionvirus 0:0eef242852bb 92 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 93 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 94 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
passionvirus 0:0eef242852bb 95 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 96
passionvirus 0:0eef242852bb 97 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 98 txPacket(10, txBuf);
passionvirus 0:0eef242852bb 99 }
passionvirus 0:0eef242852bb 100
passionvirus 0:0eef242852bb 101 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 102 void Herkulex::movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED)
passionvirus 0:0eef242852bb 103 {
passionvirus 0:0eef242852bb 104 if (pos > 1023) return;
passionvirus 0:0eef242852bb 105 if (playtime > 255) return;
passionvirus 0:0eef242852bb 106
passionvirus 0:0eef242852bb 107 uint8_t txBuf[12];
passionvirus 0:0eef242852bb 108
passionvirus 0:0eef242852bb 109 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 110 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 111 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
passionvirus 0:0eef242852bb 112 txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
passionvirus 0:0eef242852bb 113 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
passionvirus 0:0eef242852bb 114 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 115 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 116 txBuf[7] = playtime; // Playtime
passionvirus 0:0eef242852bb 117 txBuf[8] = pos & 0x00FF; // JOG(LSB)
passionvirus 0:0eef242852bb 118 txBuf[9] =(pos & 0xFF00) >> 8; // JOG(MSB)
passionvirus 0:0eef242852bb 119 txBuf[10] = setMode | setLED; // Set mode and LED on/off
passionvirus 0:0eef242852bb 120 txBuf[11] = id; // Servo ID
passionvirus 0:0eef242852bb 121
passionvirus 0:0eef242852bb 122 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 123 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 124 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
passionvirus 0:0eef242852bb 125 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 126
passionvirus 0:0eef242852bb 127 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 128 txPacket(12, txBuf);
passionvirus 0:0eef242852bb 129 }
passionvirus 0:0eef242852bb 130
passionvirus 0:0eef242852bb 131 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 132