Revision 7:0d98883391a2, committed 2018-06-21

Comitter:

genath6

Date:

Thu Jun 21 10:28:43 2018 +0000

Parent:

6:1dacff31b77a

Commit message:

Cr?ation du programme

Changed in this revision

diff -r 1dacff31b77a -r 0d98883391a2 herkulex.cpp
--- a/herkulex.cpp	Mon Jan 14 21:03:43 2013 +0000
+++ b/herkulex.cpp	Thu Jun 21 10:28:43 2018 +0000
@@ -3,102 +3,96 @@
  *
  * Copyright (c) 2012-2013 Yoonseok Pyo, MIT License
  *
- * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
- * and associated documentation files (the "Software"), to deal in the Software without restriction, 
- * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
- * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
  *
- * The above copyright notice and this permission notice shall be included in all copies or 
+ * The above copyright notice and this permission notice shall be included in all copies or
  * substantial portions of the Software.
  *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
- * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 //------------------------------------------------------------------------------
 #include "mbed.h"
 #include "herkulex.h"
 
+Serial *pc;
+
 //------------------------------------------------------------------------------
 Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
 {
-    #ifdef HERKULEX_DEBUG
-        pc = new Serial(USBTX, USBRX);
-        pc->baud(57600);
-        pc->printf("\n\nHerkulex Init!\n");
-    #endif
-    
-    txd = new Serial(tx, NC);
-    rxd = new Serial(NC, rx);
-    
+#ifdef HERKULEX_DEBUG
+    pc = new Serial(USBTX, USBRX);
+    pc->baud(115200); //57600);
+    pc->printf("\n\r\n\rHerkulex Init!\n\r");
+#endif
+
+    txd = new RawSerial(tx, rx);
+
     txd->baud(baudRate);
-    rxd->baud(baudRate);
 }
 
 //------------------------------------------------------------------------------
 Herkulex::~Herkulex()
 {
-    #ifdef HERKULEX_DEBUG
-        if(pc != NULL)
-            delete pc;
-    #endif
-    
+#ifdef HERKULEX_DEBUG
+    if(pc != NULL)
+        delete pc;
+#endif
+
     if(txd != NULL)
         delete txd;
-    if(rxd != NULL)
-        delete rxd;
 }
 
 //------------------------------------------------------------------------------
 void Herkulex::txPacket(uint8_t packetSize, uint8_t* data)
 {
-    #ifdef HERKULEX_DEBUG
-        pc->printf("[TX]");
-    #endif
-    
-    for(uint8_t i = 0; i < packetSize ; i++) 
-    {
-        #ifdef HERKULEX_DEBUG
-            pc->printf("%02X ",data[i]);
-        #endif
-        
+#ifdef HERKULEX_DEBUG
+    pc->printf("[TX]");
+#endif
+
+    for(uint8_t i = 0; i < packetSize ; i++) {
+#ifdef HERKULEX_DEBUG
+        pc->printf("%02X ",data[i]);
+#endif
+
         txd->putc(data[i]);
     }
-    
-    #ifdef HERKULEX_DEBUG
-        pc->printf("\n");
-    #endif
+
+#ifdef HERKULEX_DEBUG
+    pc->printf("\n\r");
+#endif
 }
 
 //------------------------------------------------------------------------------
 void Herkulex::rxPacket(uint8_t packetSize, uint8_t* data)
 {
-    #ifdef HERKULEX_DEBUG
-        pc->printf("[RX]");
-    #endif
-       
-    for (uint8_t i=0; i < packetSize; i++) 
-    {   
-        data[i] = rxd->getc();
-        
-        #ifdef HERKULEX_DEBUG
-            pc->printf("%02X ",data[i]);
-        #endif
-    }      
-    
-    #ifdef HERKULEX_DEBUG
-        pc->printf("\n");
-    #endif
+#ifdef HERKULEX_DEBUG
+    pc->printf("[RX]");
+#endif
+
+    for (uint8_t i=0; i < packetSize; i++) {
+        data[i] = txd->getc();
+    }
+#ifdef HERKULEX_DEBUG
+    for (uint8_t i=0; i < packetSize; i++) {
+        pc->printf("%02X ",data[i]);
+    }
+    pc->printf("\n\r");
+#endif
 }
 
 //------------------------------------------------------------------------------
 void Herkulex::clear(uint8_t id)
 {
     uint8_t txBuf[11];
-    
+
     txBuf[0] = HEADER;              // Packet Header (0xFF)
     txBuf[1] = HEADER;              // Packet Header (0xFF)
     txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
@@ -110,7 +104,7 @@
     txBuf[8] = BYTE2;               // Length
     txBuf[9] = 0;                   // Clear RAM_STATUS_ERROR
     txBuf[10]= 0;                   // Clear RAM_STATUS_DETAIL
-    
+
     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
     // Checksum2 = (~Checksum1)&0xFE
     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
@@ -121,10 +115,35 @@
 }
 
 //------------------------------------------------------------------------------
-void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
+void Herkulex::leds(uint8_t id, uint8_t val)
 {
     uint8_t txBuf[10];
-    
+
+    txBuf[0] = HEADER;              // Packet Header (0xFF)
+    txBuf[1] = HEADER;              // Packet Header (0xFF)
+    txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
+    txBuf[3] = id;                  // Servo ID
+    txBuf[4] = CMD_RAM_WRITE;       // Command Ram Write (0x03)
+    txBuf[5] = 0;                   // Checksum1
+    txBuf[6] = 0;                   // Checksum2
+    txBuf[7] = RAM_LED_CONTROL;    // Address 48
+    txBuf[8] = BYTE1;               // Length
+    txBuf[9] = val;
+
+    // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+    // Checksum2 = (~Checksum1)&0xFE
+    txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
+    txBuf[6] = (~txBuf[5])&0xFE;
+
+    // send packet (mbed -> herkulex)
+    txPacket(10, txBuf);
+}
+
+//------------------------------------------------------------------------------
+void Herkulex::setTorque(uint8_t id, uint8_t cmdTrue)
+{
+    uint8_t txBuf[10];
+
     txBuf[0] = HEADER;              // Packet Header (0xFF)
     txBuf[1] = HEADER;              // Packet Header (0xFF)
     txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
@@ -134,7 +153,7 @@
     txBuf[6] = 0;                   // Checksum2
     txBuf[7] = RAM_TORQUE_CONTROL;  // Address 52
     txBuf[8] = BYTE1;               // Length
-    txBuf[9] = cmdTorue;            // Torque ON
+    txBuf[9] = cmdTrue;            // Torque ON
 
     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
     // Checksum2 = (~Checksum1)&0xFE
@@ -150,13 +169,13 @@
 {
     if (position > 1023) return;
     if (playtime > 255) return;
-    
+
     uint8_t txBuf[12];
-    
+
     txBuf[0]  = HEADER;                 // Packet Header (0xFF)
     txBuf[1]  = HEADER;                 // Packet Header (0xFF)
     txBuf[2]  = MIN_PACKET_SIZE + 5;    // Packet Size
-    txBuf[3]  = MAX_PID;                // pID is total number of servos in the network (0 ~ 253)
+    txBuf[3]  = id; //MAX_PID;                // pID is total number of servos in the network (0 ~ 253)
     txBuf[4]  = CMD_S_JOG;              // Command S JOG (0x06)
     txBuf[5]  = 0;                      // Checksum1
     txBuf[6]  = 0;                      // Checksum2
@@ -165,7 +184,7 @@
     txBuf[9]  =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
     txBuf[10] = POS_MODE | setLED;      // Pos Mode and LED on/off
     txBuf[11] = id;                     // Servo ID
-    
+
     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
     // Checksum2 = (~Checksum1)&0xFE
     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
@@ -179,9 +198,9 @@
 void Herkulex::velocityControl(uint8_t id, int16_t speed, uint8_t setLED)
 {
     if (speed > 1023 || speed < -1023) return;
-    
+
     uint8_t txBuf[12];
-    
+
     txBuf[0]  = HEADER;                 // Packet Header (0xFF)
     txBuf[1]  = HEADER;                 // Packet Header (0xFF)
     txBuf[2]  = MIN_PACKET_SIZE + 5;    // Packet Size
@@ -194,7 +213,7 @@
     txBuf[9]  =(speed & 0xFF00) >> 8;   // Speed (MSB, Most Significanct Bit)
     txBuf[10] = TURN_MODE | setLED;     // Turn Mode and LED on/off
     txBuf[11] = id;                     // Servo ID
-    
+
     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
     // Checksum2 = (~Checksum1)&0xFE
     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
@@ -205,11 +224,11 @@
 }
 
 //------------------------------------------------------------------------------
-int8_t Herkulex::getStatus(uint8_t id) 
+int8_t Herkulex::getStatus(uint8_t id)
 {
     uint8_t status;
     uint8_t txBuf[7];
-    
+
     txBuf[0] = HEADER;                  // Packet Header (0xFF)
     txBuf[1] = HEADER;                  // Packet Header (0xFF)
     txBuf[2] = MIN_PACKET_SIZE;         // Packet Size
@@ -222,58 +241,56 @@
 
     // send packet (mbed -> herkulex)
     txPacket(7, txBuf);
-    
+
     uint8_t rxBuf[9];
     rxPacket(9, rxBuf);
 
     // Checksum1
-    uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;    
-    if (chksum1 != rxBuf[5])
-    {
-        #ifdef HERKULEX_DEBUG
-            pc->printf("Checksum1 fault\n");
-        #endif
-        
+    uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;
+    if (chksum1 != rxBuf[5]) {
+#ifdef HERKULEX_DEBUG
+        pc->printf("Checksum1 fault\n\r");
+#endif
+
         return -1;
     }
-    
+
     // Checksum2
     uint8_t chksum2 = (~rxBuf[5]&0xFE);
-    if (chksum2 != rxBuf[6])
-    {
-        #ifdef HERKULEX_DEBUG
-            pc->printf("Checksum2 fault\n");
-        #endif
-        
+    if (chksum2 != rxBuf[6]) {
+#ifdef HERKULEX_DEBUG
+        pc->printf("Checksum2 fault\n\r");
+#endif
+
         return -1;
     }
 
     status = rxBuf[7];  // Status Error
-  //status = rxBuf[8];  // Status Detail
-    
-    #ifdef HERKULEX_DEBUG
-        pc->printf("Status = %02X\n", status);
-    #endif
-    
+    //status = rxBuf[8];  // Status Detail
+
+#ifdef HERKULEX_DEBUG
+    pc->printf("Status = %02X\n\r", status);
+#endif
+
     return status;
 }
 
 //------------------------------------------------------------------------------
-int16_t Herkulex::getPos(uint8_t id) 
+int16_t Herkulex::getPos(uint8_t id)
 {
     uint16_t position = 0;
-    
+
     uint8_t txBuf[9];
-    
+
     txBuf[0] = HEADER;                  // Packet Header (0xFF)
     txBuf[1] = HEADER;                  // Packet Header (0xFF)
     txBuf[2] = MIN_PACKET_SIZE + 2;     // Packet Size
     txBuf[3] = id;                      // Servo ID
     txBuf[4] = CMD_RAM_READ;            // Status Error, Status Detail request
     txBuf[5] = 0;                       // Checksum1
-    txBuf[6] = 0;                       // Checksum2    
+    txBuf[6] = 0;                       // Checksum2
     txBuf[7] = RAM_CALIBRATED_POSITION; // Address 52
-    txBuf[8] = BYTE2;                   // Address 52 and 53      
+    txBuf[8] = BYTE2;                   // Address 52 and 53
 
     // Check Sum1 and Check Sum2
     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
@@ -281,38 +298,36 @@
 
     // send packet (mbed -> herkulex)
     txPacket(9, txBuf);
-    
+
     uint8_t rxBuf[13];
     rxPacket(13, rxBuf);
 
     // Checksum1
-    uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;    
-    if (chksum1 != rxBuf[5])
-    {
-        #ifdef HERKULEX_DEBUG
-            pc->printf("Checksum1 fault\n");
-        #endif
-        
+    uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;
+    if (chksum1 != rxBuf[5]) {
+#ifdef HERKULEX_DEBUG
+        pc->printf("Checksum1 fault\n\r");
+#endif
+
         return -1;
     }
-    
+
     // Checksum2
     uint8_t chksum2 = (~rxBuf[5]&0xFE);
-    if (chksum2 != rxBuf[6])
-    {
-        #ifdef HERKULEX_DEBUG
-            pc->printf("Checksum2 fault\n");
-        #endif
-        
+    if (chksum2 != rxBuf[6]) {
+#ifdef HERKULEX_DEBUG
+        pc->printf("Checksum2 fault\n\r");
+#endif
+
         return -1;
     }
 
     position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
-    
-    #ifdef HERKULEX_DEBUG
-        pc->printf("position = %04X(%d)\n", position, position);
-    #endif
-    
+
+#ifdef HERKULEX_DEBUG
+    pc->printf("position = %04X(%d)\n\r", position, position);
+#endif
+
     return position;
 }
 

diff -r 1dacff31b77a -r 0d98883391a2 herkulex.h
--- a/herkulex.h	Mon Jan 14 21:03:43 2013 +0000
+++ b/herkulex.h	Thu Jun 21 10:28:43 2018 +0000
@@ -242,6 +242,9 @@
  * @endcode
  */
 //------------------------------------------------------------------------------
+
+extern Serial *pc;
+    
 class Herkulex
 {
 public:
@@ -281,9 +284,9 @@
     /** Set torque setting
      *        
      * @param id The herkulex servo ID.
-     * @param cmdTorue The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60)
+     * @param cmdTrue The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60)
      */      
-    void setTorque(uint8_t id, uint8_t cmdTorue);
+    void setTorque(uint8_t id, uint8_t cmdTrue);
     
     /** Position Control
      *        
@@ -316,15 +319,17 @@
      */         
     int16_t getPos(uint8_t id);
          
+    void leds(uint8_t id, uint8_t val);     
+         
 private :
 
     /** PC serial connection used in debug mode.
     */
-    Serial *pc;
+
     
     /** Serial connection (mbed <-> herkulex).
     */  
-    Serial *txd, *rxd;
+    RawSerial *txd;
 };
 
 //------------------------------------------------------------------------------