Revision 0:6b3497b2f2ec, committed 2015-04-24

Comitter:

masasin

Date:

Fri Apr 24 01:55:32 2015 +0000

Commit message:

Initial commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamixel/AX12.cpp	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,492 @@
+/* mbed AX-12+ Servo Library
+ *
+ * Copyright (c) 2010, cstyles (http://mbed.org)
+ *
+ * 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 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,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Dynamixel.h"
+#include "mbed.h"
+
+AX12::AX12(PinName tx, PinName rx, int ID) : _ax12(tx,rx) {
+
+    _ax12.baud(1000000);
+    _ID = ID;
+}
+
+// Set the mode of the servo
+//  0 = Positional (0-300 degrees)
+//  1 = Rotational -1 to 1 speed
+int AX12::SetMode(int mode) {
+
+    if (mode == 1) { // set CR
+        SetCWLimit(0);
+        SetCCWLimit(0);
+        SetCRSpeed(0.0);
+    } else {
+        SetCWLimit(0);
+        SetCCWLimit(300);
+        SetCRSpeed(0.0);
+    }
+    return 0;
+}
+
+
+// if flag[0] is set, we're blocking
+// if flag[1] is set, we're registering
+// they are mutually exclusive operations
+int AX12::SetGoal(int degrees, int flags) {
+
+    char reg_flag = 0;
+    char data[2];
+
+    // set the flag is only the register bit is set in the flag
+    if (flags == 0x2) {
+        reg_flag = 1;
+    }
+
+    // 1023 / 300 * degrees
+    short goal = (1023 * degrees) / 300;
+    if (AX12_DEBUG) {
+        printf("SetGoal to 0x%x\n",goal);
+    }
+
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
+
+    if (flags == 1) {
+        // block until it comes to a halt
+        while (isMoving()) {}
+    }
+    return(rVal);
+}
+
+
+// Set continuous rotation speed from -1 to 1
+int AX12::SetCRSpeed (float speed) {
+
+    // bit 10     = direction, 0 = CCW, 1=CW
+    // bits 9-0   = Speed
+    char data[2];
+
+    int goal = (0x3ff * abs(speed));
+
+    // Set direction CW if we have a negative speed
+    if (speed < 0) {
+        goal |= (0x1 << 10);
+    }
+
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    int rVal = write(_ID, 0x20, 2, data);
+
+    return(rVal);
+}
+
+
+int AX12::SetCWLimit (int degrees) {
+
+    char data[2];
+    
+    // 1023 / 300 * degrees
+    short limit = (1023 * degrees) / 300;
+
+    if (AX12_DEBUG) {
+        printf("SetCWLimit to 0x%x\n",limit);
+    }
+
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
+
+}
+
+
+int AX12::SetCCWLimit (int degrees) {
+
+    char data[2];
+
+    // 1023 / 300 * degrees
+    short limit = (1023 * degrees) / 300;
+
+    if (AX12_DEBUG) {
+        printf("SetCCWLimit to 0x%x\n",limit);
+    }
+
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
+}
+
+
+int AX12::SetID (int CurrentID, int NewID) {
+
+    char data[1];
+    data[0] = NewID;
+    if (AX12_DEBUG) {
+        printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
+    }
+    return (write(CurrentID, AX12_REG_ID, 1, data));
+
+}
+
+
+// return 1 is the servo is still in flight
+int AX12::isMoving(void) {
+
+    char data[1];
+    read(_ID,AX12_REG_MOVING,1,data);
+    return(data[0]);
+}
+
+
+void AX12::trigger(void) {
+
+    char TxBuf[16];
+    char sum = 0;
+
+    if (AX12_TRIGGER_DEBUG) {
+        printf("\nTriggered\n");
+    }
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    if (AX12_TRIGGER_DEBUG) {
+        printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
+    }
+
+    TxBuf[0] = 0xFF;
+    TxBuf[1] = 0xFF;
+
+    // ID - Broadcast
+    TxBuf[2] = 0xFE;
+    sum += TxBuf[2];
+
+    if (AX12_TRIGGER_DEBUG) {
+        printf("  ID : %d\n",TxBuf[2]);
+    }
+
+    // Length
+    TxBuf[3] = 0x02;
+    sum += TxBuf[3];
+    if (AX12_TRIGGER_DEBUG) {
+        printf("  Length %d\n",TxBuf[3]);
+    }
+
+    // Instruction - ACTION
+    TxBuf[4] = 0x04;
+    sum += TxBuf[4];
+    if (AX12_TRIGGER_DEBUG) {
+        printf("  Instruction 0x%X\n",TxBuf[5]);
+    }
+
+    // Checksum
+    TxBuf[5] = 0xFF - sum;
+    if (AX12_TRIGGER_DEBUG) {
+        printf("  Checksum 0x%X\n",TxBuf[5]);
+    }
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i < 6 ; i++) {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // This is a broadcast packet, so there will be no reply
+
+    return;
+}
+
+
+float AX12::GetPosition(void) {
+    if (AX12_DEBUG) {
+        printf("\nGetPosition(%d)",_ID);
+    }
+
+    char data[2];
+
+    int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
+    short position = data[0] + (data[1] << 8);
+    float angle = (position * 300)/1024;
+    
+    return (angle);
+}
+
+
+float AX12::GetTemp (void) {
+
+    if (AX12_DEBUG) {
+        printf("\nGetTemp(%d)",_ID);
+    }
+    char data[1];
+    int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
+    float temp = data[0];
+    return(temp);
+}
+
+
+float AX12::GetVolts (void) {
+    if (AX12_DEBUG) {
+        printf("\nGetVolts(%d)",_ID);
+    }
+    char data[1];
+    int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
+    float volts = data[0]/10.0;
+    return(volts);
+}
+
+float AX12::GetCurrent (void) {
+    return 0;
+}
+
+
+int AX12::TorqueEnable (int mode) {
+
+    char data[1];
+    data[0] = mode;
+
+    return (write(_ID, AX12_REG_TORQUE_ENABLE, 1, data));
+}
+
+
+int AX12::SetTorqueLimit (float torque_lim) {
+    
+    short limit = torque_lim * 1023;
+    char data[2];
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+
+    return (write(_ID, AX12_REG_TORQUE_LIMIT, 2, data));
+}
+
+
+int AX12::read(int ID, int start, int bytes, char* data) {
+
+    char PacketLength = 0x4;
+    char TxBuf[16];
+    char sum = 0;
+    char Status[16];
+
+    Status[4] = 0xFE; // return code
+
+    if (AX12_READ_DEBUG) {
+        printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
+    }
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    if (AX12_READ_DEBUG) {
+        printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
+    }
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+    if (AX12_READ_DEBUG) {
+        printf("  ID : %d\n",TxBuf[2]);
+    }
+
+    // Packet Length
+    TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
+    sum += TxBuf[3];            // Accululate the packet sum
+    if (AX12_READ_DEBUG) {
+        printf("  Length : 0x%x\n",TxBuf[3]);
+    }
+
+    // Instruction - Read
+    TxBuf[4] = 0x2;
+    sum += TxBuf[4];
+    if (AX12_READ_DEBUG) {
+        printf("  Instruction : 0x%x\n",TxBuf[4]);
+    }
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+    if (AX12_READ_DEBUG) {
+        printf("  Start Address : 0x%x\n",TxBuf[5]);
+    }
+
+    // Bytes to read
+    TxBuf[6] = bytes;
+    sum += TxBuf[6];
+    if (AX12_READ_DEBUG) {
+        printf("  No bytes : 0x%x\n",TxBuf[6]);
+    }
+
+    // Checksum
+    TxBuf[7] = 0xFF - sum;
+    if (AX12_READ_DEBUG) {
+        printf("  Checksum : 0x%x\n",TxBuf[7]);
+    }
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i<8 ; i++) {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // Wait for the bytes to be transmitted
+    wait (0.00002);
+
+    // Skip if the read was to the broadcast address
+    if (_ID != 0xFE) {
+
+        // Receive the Status packet 6+ number of bytes read
+        for (int i=0; i<(6+bytes) ; i++) {
+            Status[i] = _ax12.getc();
+        }
+
+        // Copy the data from Status into data for return
+        for (int i=0; i < Status[3]-2 ; i++) {
+            data[i] = Status[5+i];
+        }
+
+        if (AX12_READ_DEBUG) {
+            printf("\nStatus Packet\n");
+            printf("  Header : 0x%x\n",Status[0]);
+            printf("  Header : 0x%x\n",Status[1]);
+            printf("  ID : 0x%x\n",Status[2]);
+            printf("  Length : 0x%x\n",Status[3]);
+
+            for (int i=0; i < Status[3]-2 ; i++) {
+                printf("  Data : 0x%x\n",Status[5+i]);
+            }
+
+            printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
+        }
+
+    } // if (ID!=0xFE)
+
+    return(Status[4]);
+}
+
+
+int AX12:: write(int ID, int start, int bytes, char* data, int flag) {
+// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
+
+    char TxBuf[16];
+    char sum = 0;
+    char Status[6];
+
+    if (AX12_WRITE_DEBUG) {
+        printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
+    }
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    if (AX12_WRITE_DEBUG) {
+        printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
+    }
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+
+    if (AX12_WRITE_DEBUG) {
+        printf("  ID : %d\n",TxBuf[2]);
+    }
+
+    // packet Length
+    TxBuf[3] = 3+bytes;
+    sum += TxBuf[3];
+
+    if (AX12_WRITE_DEBUG) {
+        printf("  Length : %d\n",TxBuf[3]);
+    }
+
+    // Instruction
+    if (flag == 1) {
+        TxBuf[4]=0x04;
+        sum += TxBuf[4];
+    } else {
+        TxBuf[4]=0x03;
+        sum += TxBuf[4];
+    }
+
+    if (AX12_WRITE_DEBUG) {
+        printf("  Instruction : 0x%x\n",TxBuf[4]);
+    }
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+    if (AX12_WRITE_DEBUG) {
+        printf("  Start : 0x%x\n",TxBuf[5]);
+    }
+
+    // data
+    for (char i=0; i<bytes ; i++) {
+        TxBuf[6+i] = data[i];
+        sum += TxBuf[6+i];
+        if (AX12_WRITE_DEBUG) {
+            printf("  Data : 0x%x\n",TxBuf[6+i]);
+        }
+    }
+
+    // checksum
+    TxBuf[6+bytes] = 0xFF - sum;
+    if (AX12_WRITE_DEBUG) {
+        printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
+    }
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i < (7 + bytes) ; i++) {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // Wait for data to transmit
+    wait (0.00002);
+
+    // make sure we have a valid return
+    Status[4]=0x00;
+
+    // we'll only get a reply if it was not broadcast
+    if (_ID!=0xFE) {
+
+        // response is always 6 bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        for (int i=0; i < 6 ; i++) {
+            Status[i] = _ax12.getc();
+        }
+
+        // Build the TxPacket first in RAM, then we'll send in one go
+        if (AX12_WRITE_DEBUG) {
+            printf("\nStatus Packet\n  Header : 0x%X, 0x%X\n",Status[0],Status[1]);
+            printf("  ID : %d\n",Status[2]);
+            printf("  Length : %d\n",Status[3]);
+            printf("  Error : 0x%x\n",Status[4]);
+            printf("  Checksum : 0x%x\n",Status[5]);
+        }
+
+
+    }
+
+    return(Status[4]); // return error code
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamixel/Dynamixel.h	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,266 @@
+/* mbed Dynamixel Servo Library
+ *
+ * Copyright (c) 2015, Jean Nassar
+ *
+ * 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 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,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef MBED_DYNAMIXEL_H
+#define MBED_DYNAMIXEL_H
+
+#include "mbed.h"
+#include "SerialHalfDuplex.h"
+
+#define AX12_WRITE_DEBUG 0
+#define AX12_READ_DEBUG 0
+#define AX12_TRIGGER_DEBUG 0
+#define AX12_DEBUG 0
+
+#define AX12_REG_ID 0x3
+#define AX12_REG_CW_LIMIT 0x06
+#define AX12_REG_CCW_LIMIT 0x08
+#define AX12_REG_TORQUE_ENABLE 0x18
+#define AX12_REG_GOAL_POSITION 0x1E
+#define AX12_REG_MOVING_SPEED 0x20
+#define AX12_REG_VOLTS 0x2A
+#define AX12_REG_TEMP 0x2B
+#define AX12_REG_MOVING 0x2E
+#define AX12_REG_TORQUE_LIMIT 0x22
+#define AX12_REG_POSITION 0x24
+
+#define AX12_MODE_POSITION  0
+#define AX12_MODE_ROTATION  1
+
+#define AX12_CW 1
+#define AX12_CCW 0
+
+#define MX28_WRITE_DEBUG 0
+#define MX28_READ_DEBUG 0
+#define MX28_TRIGGER_DEBUG 0
+#define MX28_DEBUG 0
+
+#define MX28_REG_ID 0x3
+#define MX28_REG_CW_LIMIT 0x06
+#define MX28_REG_CCW_LIMIT 0x08
+#define MX28_REG_TORQUE_ENABLE 0x18
+#define MX28_REG_GOAL_POSITION 0x1E
+#define MX28_REG_MOVING_SPEED 0x20
+#define MX28_REG_VOLTS 0x2A
+#define MX28_REG_TEMP 0x2B
+#define MX28_REG_MOVING 0x2E
+#define MX28_REG_POSITION 0x24
+#define MX28_REG_TORQUE_LIMIT 0x22
+#define MX28_REG_CURRENT 0x44
+
+#define MX28_MODE_POSITION  0
+#define MX28_MODE_ROTATION  1
+
+#define MX28_CW 1
+#define MX28_CCW 0
+
+/** Servo control class, based on a PwmOut
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include "AX12.h"
+ * 
+ * int main() {
+ * 
+ *   AX12 myax12 (p9, p10, 1);
+ *
+ *   while (1) {
+ *       myax12.SetGoal(0);    // go to 0 degrees
+ *       wait (2.0);
+ *       myax12.SetGoal(300);  // go to 300 degrees
+ *       wait (2.0);
+ *   }
+ * }
+ * @endcode
+ */
+class Dynamixel {
+
+public:
+
+
+
+    /** Set the mode of the servo
+     * @param mode
+     *    0 = Positional, default
+     *    1 = Continuous rotation
+     */
+    virtual int SetMode(int mode) {return 0;}
+
+    /** Set goal angle in integer degrees, in positional mode
+     *
+     * @param degrees 0-300
+     * @param flags, defaults to 0
+     *    flags[0] = blocking, return when goal position reached 
+     *    flags[1] = register, activate with a broadcast trigger
+     *
+     */
+    virtual int SetGoal(int degrees, int flags = 0) {return 0;}
+
+
+    /** Set the speed of the servo in continuous rotation mode
+     *
+     * @param speed, -1.0 to 1.0
+     *   -1.0 = full speed counter clock wise
+     *    1.0 = full speed clock wise
+     */
+    virtual int SetCRSpeed(float speed) {return 0;}
+
+
+    /** Set the clockwise limit of the servo
+     *
+     * @param degrees, 0-300
+     */
+    virtual int SetCWLimit(int degrees) {return 0;}
+    
+    /** Set the counter-clockwise limit of the servo
+     *
+     * @param degrees, 0-300
+     */
+    virtual int SetCCWLimit(int degrees) {return 0;}
+
+    // Change the ID
+
+    /** Change the ID of a servo
+     *
+     * @param CurentID 1-255
+     * @param NewID 1-255
+     *
+     * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
+     * In this situation, only one servo should be connected to the bus
+     */
+    virtual int SetID(int CurrentID, int NewID) {return 0;}
+
+
+    /** Poll to see if the servo is moving
+     *
+     * @returns true is the servo is moving
+     */
+    virtual int isMoving(void) {return 0;}
+
+    /** Send the broadcast "trigger" command, to activate any outstanding registered commands
+     */
+    virtual void trigger(void) {}
+
+    /** Read the current angle of the servo
+     *
+     * @returns float in the range 0.0-300.0
+     */
+    virtual float GetPosition() {return 0;}
+
+    /** Read the temperature of the servo
+     *
+     * @returns float temperature 
+     */
+    virtual float GetTemp(void) {return 0;}
+
+    /** Read the supply voltage of the servo
+     *
+     * @returns float voltage
+     */
+    virtual float GetVolts(void) {return 0;}
+    virtual float GetCurrent(void) {return 0;}
+    
+    virtual int TorqueEnable(int mode) {return 0;}
+    
+    virtual int SetTorqueLimit(float torque_limit) {return 0;}
+
+protected :
+    int _ID;
+
+    virtual int read(int ID, int start, int length, char* data) {return 0;}
+    virtual int write(int ID, int start, int length, char* data, int flag=0) {return 0;}
+
+};
+
+class AX12 : public Dynamixel {
+ public:
+  /** Create a Dynamixel servo object connected to the specified serial port, with the specified ID
+  *
+  * @param pin tx pin
+  * @param pin rx pin 
+  * @param int ID, the Bus ID of the servo 1-255 
+  */
+  AX12(PinName tx, PinName rx, int ID);
+  
+  virtual int SetMode(int mode);
+  virtual int SetGoal(int degrees, int flags = 0);
+  virtual int SetCRSpeed(float speed);
+  virtual int SetCWLimit(int degrees);
+  virtual int SetCCWLimit(int degrees);
+  virtual int SetID(int CurrentID, int NewID);
+  virtual int isMoving(void);
+  virtual void trigger(void);
+  virtual float GetPosition();
+  virtual float GetTemp(void);
+  virtual float GetVolts(void);
+  virtual int TorqueEnable(int mode);
+  virtual int SetTorqueLimit(float torque_limit);
+  virtual float GetCurrent(void);
+ 
+ protected:
+  virtual int read(int ID, int start, int length, char* data);
+  virtual int write(int ID, int start, int length, char* data, int flag=0);
+ 
+ private:
+  SerialHalfDuplex _ax12;
+};
+
+class MX28 : public Dynamixel {
+ public:
+  /** Read the supply current of the servo
+   *
+   * @returns float current
+   */
+  MX28(PinName tx, PinName rx, int ID);
+  
+  virtual int SetMode(int mode);
+  virtual int SetGoal(int degrees, int flags = 0);
+  virtual int SetCRSpeed(float speed);
+  virtual int SetCWLimit(int degrees);
+  virtual int SetCCWLimit(int degrees);
+  virtual int SetID(int CurrentID, int NewID);
+  virtual int isMoving(void);
+  virtual void trigger(void);
+  virtual float GetPosition();
+  virtual float GetTemp(void);
+  virtual float GetVolts(void);
+  virtual int TorqueEnable(int mode);
+  virtual int SetTorqueLimit(float torque_limit);
+  
+  /** Create a Dynamixel servo object connected to the specified serial port, with the specified ID
+  *
+  * @param pin tx pin
+  * @param pin rx pin 
+  * @param int ID, the Bus ID of the servo 1-255 
+  */
+  virtual float GetCurrent(void);
+
+ protected:
+  virtual int read(int ID, int start, int length, char* data);
+  virtual int write(int ID, int start, int length, char* data, int flag=0);
+ 
+ private:
+  SerialHalfDuplex _mx28;
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamixel/MX28.cpp	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,502 @@
+/* mbed AX-12+ Servo Library
+ *
+ * Copyright (c) 2010, cstyles (http://mbed.org)
+ *
+ * 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 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,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "Dynamixel.h"
+#include "mbed.h"
+
+MX28::MX28(PinName tx, PinName rx, int ID) : _mx28(tx,rx) {
+
+    _mx28.baud(1000000);
+    _ID = ID;
+}
+
+// Set the mode of the servo
+//  0 = Positional (0-300 degrees)
+//  1 = Rotational -1 to 1 speed
+int MX28::SetMode(int mode) {
+
+    if (mode == 1) { // set CR
+        SetCWLimit(0);
+        SetCCWLimit(0);
+        SetCRSpeed(0.0);
+    } else {
+        SetCWLimit(0);
+        SetCCWLimit(360);
+        SetCRSpeed(0.0);
+    }
+    return(0);
+}
+
+
+// if flag[0] is set, we're blocking
+// if flag[1] is set, we're registering
+// they are mutually exclusive operations
+int MX28::SetGoal(int degrees, int flags) {
+
+    char reg_flag = 0;
+    char data[2];
+
+    // set the flag is only the register bit is set in the flag
+    if (flags == 0x2) {
+        reg_flag = 1;
+    }
+
+    // 4095 / 360 * degrees
+    short goal = (4095 * degrees) / 360;
+    if (MX28_DEBUG) {
+        printf("SetGoal to 0x%x\n",goal);
+    }
+
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    int rVal = write(_ID, MX28_REG_GOAL_POSITION, 2, data, reg_flag);
+
+    if (flags == 1) {
+        // block until it comes to a halt
+        while (isMoving()) {}
+    }
+    return(rVal);
+}
+
+
+// Set continuous rotation speed from -1 to 1
+int MX28::SetCRSpeed (float speed) {
+
+    // bit 10     = direction, 0 = CCW, 1=CW
+    // bits 9-0   = Speed
+    char data[2];
+
+    int goal = (0x3ff * abs(speed));
+
+    // Set direction CW if we have a negative speed
+    if (speed < 0) {
+        goal |= (0x1 << 10);
+    }
+
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    int rVal = write(_ID, 0x20, 2, data);
+
+    return(rVal);
+}
+
+
+int MX28::SetCWLimit (int degrees) {
+
+    char data[2];
+    
+    // 4095 / 360 * degrees
+    short limit = (4095 * degrees) / 360;
+
+    if (MX28_DEBUG) {
+        printf("SetCWLimit to 0x%x\n",limit);
+    }
+
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    return (write(_ID, MX28_REG_CW_LIMIT, 2, data));
+
+}
+
+
+int MX28::SetCCWLimit (int degrees) {
+
+    char data[2];
+
+    // 4095 / 360 * degrees
+    short limit = (4095 * degrees) / 360;
+
+    if (MX28_DEBUG) {
+        printf("SetCCWLimit to 0x%x\n",limit);
+    }
+
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    return (write(_ID, MX28_REG_CCW_LIMIT, 2, data));
+}
+
+
+int MX28::SetID (int CurrentID, int NewID) {
+
+    char data[1];
+    data[0] = NewID;
+    if (MX28_DEBUG) {
+        printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
+    }
+    return (write(CurrentID, MX28_REG_ID, 1, data));
+
+}
+
+
+// return 1 is the servo is still in flight
+int MX28::isMoving(void) {
+
+    char data[1];
+    read(_ID,MX28_REG_MOVING,1,data);
+    return(data[0]);
+}
+
+
+void MX28::trigger(void) {
+
+    char TxBuf[16];
+    char sum = 0;
+
+    if (MX28_TRIGGER_DEBUG) {
+        printf("\nTriggered\n");
+    }
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    if (MX28_TRIGGER_DEBUG) {
+        printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
+    }
+
+    TxBuf[0] = 0xFF;
+    TxBuf[1] = 0xFF;
+
+    // ID - Broadcast
+    TxBuf[2] = 0xFE;
+    sum += TxBuf[2];
+
+    if (MX28_TRIGGER_DEBUG) {
+        printf("  ID : %d\n",TxBuf[2]);
+    }
+
+    // Length
+    TxBuf[3] = 0x02;
+    sum += TxBuf[3];
+    if (MX28_TRIGGER_DEBUG) {
+        printf("  Length %d\n",TxBuf[3]);
+    }
+
+    // Instruction - ACTION
+    TxBuf[4] = 0x04;
+    sum += TxBuf[4];
+    if (MX28_TRIGGER_DEBUG) {
+        printf("  Instruction 0x%X\n",TxBuf[5]);
+    }
+
+    // Checksum
+    TxBuf[5] = 0xFF - sum;
+    if (MX28_TRIGGER_DEBUG) {
+        printf("  Checksum 0x%X\n",TxBuf[5]);
+    }
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i < 6 ; i++) {
+        _mx28.putc(TxBuf[i]);
+    }
+
+    // This is a broadcast packet, so there will be no reply
+
+    return;
+}
+
+
+float MX28::GetPosition(void) {
+
+    if (MX28_DEBUG) {
+        printf("\nGetPosition(%d)",_ID);
+    }
+
+    char data[2];
+
+    int ErrorCode = read(_ID, MX28_REG_POSITION, 2, data);
+    short position = data[0] + (data[1] << 8);
+    float angle = (position * 300)/1024;
+
+    return (angle);
+}
+
+
+float MX28::GetTemp (void) {
+
+    if (MX28_DEBUG) {
+        printf("\nGetTemp(%d)",_ID);
+    }
+    char data[1];
+    int ErrorCode = read(_ID, MX28_REG_TEMP, 1, data);
+    float temp = data[0];
+    return(temp);
+}
+
+
+float MX28::GetVolts (void) {
+
+    if (MX28_DEBUG) {
+        printf("\nGetVolts(%d)",_ID);
+    }
+    char data[1];
+    int ErrorCode = read(_ID, MX28_REG_VOLTS, 1, data);
+    float volts = data[0]/10.0;
+    return(volts);
+}
+
+
+float MX28::GetCurrent (void) {
+
+    if (MX28_DEBUG) {
+        printf("\nGetCurrent(%d)",_ID);
+    }
+    char data[2];
+    int ErrorCode = read(_ID, MX28_REG_CURRENT, 2, data);
+    float current = ((data[0]+(data[1] << 8))-0x8FF)*0.0045;
+    return(current);
+}
+
+
+int MX28::TorqueEnable (int mode) {
+
+    char data[1];
+    data[0] = mode;
+
+    return (write(_ID, MX28_REG_TORQUE_ENABLE, 1, data));
+}
+
+
+int MX28::SetTorqueLimit (float torque_lim) {
+    
+    short limit = torque_lim * 1023;
+    char data[2];
+    data[0] = limit & 0xff; // bottom 8 bits
+    data[1] = limit >> 8;   // top 8 bits
+
+    return (write(_ID, MX28_REG_TORQUE_LIMIT, 2, data));
+}
+
+int MX28::read(int ID, int start, int bytes, char* data) {
+
+    char PacketLength = 0x4;
+    char TxBuf[16];
+    char sum = 0;
+    char Status[16];
+
+    Status[4] = 0xFE; // return code
+
+    if (MX28_READ_DEBUG) {
+        printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
+    }
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    if (MX28_READ_DEBUG) {
+        printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
+    }
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+    if (MX28_READ_DEBUG) {
+        printf("  ID : %d\n",TxBuf[2]);
+    }
+
+    // Packet Length
+    TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
+    sum += TxBuf[3];            // Accululate the packet sum
+    if (MX28_READ_DEBUG) {
+        printf("  Length : 0x%x\n",TxBuf[3]);
+    }
+
+    // Instruction - Read
+    TxBuf[4] = 0x2;
+    sum += TxBuf[4];
+    if (MX28_READ_DEBUG) {
+        printf("  Instruction : 0x%x\n",TxBuf[4]);
+    }
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+    if (MX28_READ_DEBUG) {
+        printf("  Start Address : 0x%x\n",TxBuf[5]);
+    }
+
+    // Bytes to read
+    TxBuf[6] = bytes;
+    sum += TxBuf[6];
+    if (MX28_READ_DEBUG) {
+        printf("  No bytes : 0x%x\n",TxBuf[6]);
+    }
+
+    // Checksum
+    TxBuf[7] = 0xFF - sum;
+    if (MX28_READ_DEBUG) {
+        printf("  Checksum : 0x%x\n",TxBuf[7]);
+    }
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i<8 ; i++) {
+        _mx28.putc(TxBuf[i]);
+    }
+
+    // Wait for the bytes to be transmitted
+    wait (0.00002);
+
+    // Skip if the read was to the broadcast address
+    if (_ID != 0xFE) {
+
+        // Receive the Status packet 6+ number of bytes read
+        for (int i=0; i<(6+bytes) ; i++) {
+            Status[i] = _mx28.getc();
+        }
+
+        // Copy the data from Status into data for return
+        for (int i=0; i < Status[3]-2 ; i++) {
+            data[i] = Status[5+i];
+        }
+
+        if (MX28_READ_DEBUG) {
+            printf("\nStatus Packet\n");
+            printf("  Header : 0x%x\n",Status[0]);
+            printf("  Header : 0x%x\n",Status[1]);
+            printf("  ID : 0x%x\n",Status[2]);
+            printf("  Length : 0x%x\n",Status[3]);
+            printf("  Error Code : 0x%x\n",Status[4]);
+
+            for (int i=0; i < Status[3]-2 ; i++) {
+                printf("  Data : 0x%x\n",Status[5+i]);
+            }
+
+            printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
+        }
+
+    } // if (ID!=0xFE)
+
+    return(Status[4]);
+}
+
+
+int MX28:: write(int ID, int start, int bytes, char* data, int flag) {
+// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
+
+    char TxBuf[16];
+    char sum = 0;
+    char Status[6];
+
+    if (MX28_WRITE_DEBUG) {
+        printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
+    }
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    if (MX28_WRITE_DEBUG) {
+        printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
+    }
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+
+    if (MX28_WRITE_DEBUG) {
+        printf("  ID : %d\n",TxBuf[2]);
+    }
+
+    // packet Length
+    TxBuf[3] = 3+bytes;
+    sum += TxBuf[3];
+
+    if (MX28_WRITE_DEBUG) {
+        printf("  Length : %d\n",TxBuf[3]);
+    }
+
+    // Instruction
+    if (flag == 1) {
+        TxBuf[4]=0x04;
+        sum += TxBuf[4];
+    } else {
+        TxBuf[4]=0x03;
+        sum += TxBuf[4];
+    }
+
+    if (MX28_WRITE_DEBUG) {
+        printf("  Instruction : 0x%x\n",TxBuf[4]);
+    }
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+    if (MX28_WRITE_DEBUG) {
+        printf("  Start : 0x%x\n",TxBuf[5]);
+    }
+
+    // data
+    for (char i=0; i<bytes ; i++) {
+        TxBuf[6+i] = data[i];
+        sum += TxBuf[6+i];
+        if (MX28_WRITE_DEBUG) {
+            printf("  Data : 0x%x\n",TxBuf[6+i]);
+        }
+    }
+
+    // checksum
+    TxBuf[6+bytes] = 0xFF - sum;
+    if (MX28_WRITE_DEBUG) {
+        printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
+    }
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i < (7 + bytes) ; i++) {
+        _mx28.putc(TxBuf[i]);
+    }
+
+    // Wait for data to transmit
+    wait (0.00002);
+
+    // make sure we have a valid return
+    Status[4]=0x00;
+
+    // we'll only get a reply if it was not broadcast
+    if (_ID!=0xFE) {
+
+        // response is always 6 bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        for (int i=0; i < 6 ; i++) {
+            Status[i] = _mx28.getc();
+        }
+
+        // Build the TxPacket first in RAM, then we'll send in one go
+        if (MX28_WRITE_DEBUG) {
+            printf("\nStatus Packet\n  Header : 0x%X, 0x%X\n",Status[0],Status[1]);
+            printf("  ID : %d\n",Status[2]);
+            printf("  Length : %d\n",Status[3]);
+            printf("  Error : 0x%x\n",Status[4]);
+            printf("  Checksum : 0x%x\n",Status[5]);
+        }
+
+
+    }
+
+    return(Status[4]); // return error code
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MEMS/MEMS.cpp	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,36 @@
+#include "MEMS.h"
+#include "mbed.h"
+
+MEMS::MEMS(PinName sda, PinName scl)
+        : _mems(sda, scl) {
+
+}
+
+void MEMS::temp(float* dt) {
+        
+    char  I2C_rd[64]; // 生データ
+    short  datr[16]; // 16点 温度データ（10倍整数）
+    short  PTAT; // センサ内部PTAT温度データ（10倍整数）
+//    double   dt[16]; // 16点 温度データ
+    short   d_PTAT; // センサ内部PTAT温度データ
+    int  i,j;
+    int  itemp;
+    
+    //// measure
+    _mems.start();
+    _mems.write(D6T_addr);
+    _mems.write(D6T_cmd);
+    // Repeated Start condition
+    _mems.read(D6T_addr,I2C_rd,35);
+//        if(check_PEC(I2C_rd) == -1) continue; // error
+    for(i=0,j=0;i<17;i++){
+        itemp = (I2C_rd[j++] & 0xff);
+        itemp += I2C_rd[j++] * 256;
+        if(i == 0) PTAT = itemp;
+        else datr[i-1] = itemp;
+    }
+    for(i=0;i<16;i++){
+        dt[i] = 0.1 * datr[i];
+    }
+    d_PTAT = 0.1 * PTAT;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MEMS/MEMS.h	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,25 @@
+#ifndef MBED_MEMS_H
+#define MBED_MEMS_H
+
+#include "mbed.h"
+
+#define D6T_addr 0x14
+#define D6T_cmd  0x4c
+
+class MEMS {
+ public:
+  /* @param sda : I2C pin
+     @param scl : I2C pin
+  */
+  MEMS(PinName sda, PinName scl);
+
+  /*
+     @param datr : temperature data[16]
+  */    
+  void temp(float* datr);
+
+ private:
+  I2C _mems;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialHalfDuplex/SerialHalfDuplex.cpp	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,78 @@
+ /* mbed Microcontroller Library
+ * Copyright (c) 2006-2012 ARM Limited
+ *
+ * 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 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,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * NOTE: This is an unsupported legacy untested library.
+ */
+#include "SerialHalfDuplex.h"
+ 
+#if DEVICE_SERIAL
+ 
+#include "pinmap.h"
+#include "serial_api.h"
+#include "gpio_api.h"
+//#include "RawSerial.h"
+ 
+namespace mbed {
+ 
+SerialHalfDuplex::SerialHalfDuplex(PinName tx, PinName rx, const char *name)
+    : Serial(tx, rx, name) {
+    _txpin = tx;
+    
+    // set as input 
+    gpio_set(_txpin); 
+    pin_mode(_txpin, PullNone); // no pull
+    pin_function(_txpin, 0);    // set as gpio
+}
+ 
+// To transmit a byte in half duplex mode:
+// 1. Disable interrupts, so we don't trigger on loopback byte
+// 2. Set tx pin to UART out
+// 3. Transmit byte as normal
+// 4. Read back byte from looped back tx pin - this both confirms that the
+//    transmit has occurred, and also clears the byte from the buffer.
+// 5. Return pin to input mode
+// 6. Re-enable interrupts
+ 
+int SerialHalfDuplex::_putc(int c) {
+    int retc;
+    
+    // TODO: We should not disable all interrupts
+    __disable_irq();
+    
+    serial_pinout_tx(_txpin);
+    
+    Serial::_putc(c);
+    retc = Serial::getc();       // reading also clears any interrupt
+    
+    pin_function(_txpin, 0);
+    
+    __enable_irq();
+    
+    return retc;
+}
+ 
+int SerialHalfDuplex::_getc(void) {
+    return Serial::_getc();
+}
+ 
+} // End namespace
+ 
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialHalfDuplex/SerialHalfDuplex.h	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,208 @@
+
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2012 ARM Limited
+ *
+ * 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 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,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * NOTE: This is an unsupported legacy untested library.
+ */
+#ifndef MBED_SERIALHALFDUPLEX_H
+#define MBED_SERIALHALFDUPLEX_H
+
+#include "device.h"
+ 
+#if DEVICE_SERIAL
+ 
+#include "Serial.h"
+#include "PinNames.h"
+#include "PeripheralNames.h"
+ 
+namespace mbed {
+ 
+/* Class: SerialHalfDuplex
+ * A serial port (UART) for communication with other devices using  
+ * Half-Duplex, allowing transmit and receive on a single
+ * shared transmit and receive line. Only one end should be transmitting 
+ * at a time.
+ * 
+ * Both the tx and rx pin should be defined, and wired together. 
+ * This is in addition to them being wired to the other serial 
+ * device to allow both read and write functions to operate.
+ *
+ *  Example:
+ *  > // Send a byte to a second HalfDuplex device, and read the response
+ *  >
+ *  > #include "mbed.h"
+ *  >
+ *  > // p9 and p10 should be wired together to form "a"
+ *  > // p28 and p27 should be wired together to form "b"
+ *  > // p9/p10 should be wired to p28/p27 as the Half Duplex connection
+ *  >
+ *  > SerialHalfDuplex a(p9, p10);
+ *  > SerialHalfDuplex b(p28, p27);
+ *  >
+ *  > void b_rx() { // second device response
+ *  >     b.putc(b.getc() + 4);
+ *  > }
+ *  >   
+ *  > int main() {
+ *  >     b.attach(&b_rx);
+ *  >     for(int c = 'A'; c < 'Z'; c++) {
+ *  >         a.putc(c);
+ *  >         printf("sent [%c]\n", c);
+ *  >         wait(0.5);   // b should respond
+ *  >         if(a.readable()) {
+ *  >             printf("received [%c]\n", a.getc());
+ *  >         }
+ *  >     }
+ *  > }
+ * 
+ * For Simplex and Full-Duplex Serial communication, see <Serial>
+ */
+class SerialHalfDuplex : public Serial {
+ 
+public:
+    /* Constructor: SerialHalfDuplex
+     * Create a half-duplex serial port, connected to the specified transmit
+     * and receive pins.
+     *
+     * These pins should be wired together, as well as to the target device
+     *
+     * Variables:
+     *  tx - Transmit pin
+     *  rx - Receive pin
+     */
+    SerialHalfDuplex(PinName tx, PinName rx, const char *name = NULL);
+ 
+#if 0       // Inherited from Serial class, for documentation
+    /* Function: baud
+     *  Set the baud rate of the serial port
+     *
+     * Variables:
+     *  baudrate - The baudrate of the serial port (default = 9600).
+     */
+    void baud(int baudrate);
+ 
+    enum Parity {
+        None = 0
+        , Odd
+        , Even
+        , Forced1
+        , Forced0
+    };
+ 
+    /* Function: format
+     *  Set the transmission format used by the Serial port
+     *
+     * Variables:
+     *  bits - The number of bits in a word (5-8; default = 8)
+     *  parity - The parity used (Serial::None, Serial::Odd, 
+Serial::Even, Serial::Forced1, Serial::Forced0; default = Serial::None)
+     *  stop - The number of stop bits (1 or 2; default = 1)
+     */
+    void format(int bits = 8, Parity parity = Serial::None, int stop_bits 
+= 1);
+ 
+    /* Function: putc
+     *  Write a character
+     *
+     * Variables:
+     *  c - The character to write to the serial port
+     */
+    int putc(int c);
+ 
+    /* Function: getc
+     *  Read a character
+     *
+     * Read a character from the serial port. This call will block
+     * until a character is available. For testing if a character is
+     * available for reading, see <readable>.
+     *
+     * Variables:
+     *  returns - The character read from the serial port
+     */
+    int getc();
+ 
+    /* Function: printf
+     *  Write a formated string
+     *
+     * Variables:
+     *  format - A printf-style format string, followed by the
+     *      variables to use in formating the string.
+     */
+    int printf(const char* format, ...);
+ 
+    /* Function: scanf
+     *  Read a formated string
+     *
+     * Variables:
+     *  format - A scanf-style format string,
+     *      followed by the pointers to variables to store the results.
+     */
+    int scanf(const char* format, ...);
+ 
+    /* Function: readable
+     *  Determine if there is a character available to read
+     *
+     * Variables:
+     *  returns - 1 if there is a character available to read, else 0
+     */
+    int readable();
+ 
+    /* Function: writeable
+     *  Determine if there is space available to write a character
+     *
+     * Variables:
+     *  returns - 1 if there is space to write a character, else 0
+     */
+    int writeable();
+ 
+    /* Function: attach
+     *  Attach a function to call whenever a serial interrupt is generated
+     *
+     * Variables:
+     *  fptr - A pointer to a void function, or 0 to set as none
+     */
+    void attach(void (*fptr)(void));
+ 
+    /* Function: attach
+     *  Attach a member function to call whenever a serial interrupt is generated
+     *
+     * Variables:
+     *  tptr - pointer to the object to call the member function on
+     *  mptr - pointer to the member function to be called
+     */
+    template<typename T>
+    void attach(T* tptr, void (T::*mptr)(void));
+ 
+#endif
+ 
+protected:
+    PinName     _txpin;
+ 
+    virtual int _putc(int c);
+    virtual int _getc(void);
+ 
+}; // End class SerialHalfDuplex
+ 
+} // End namespace
+ 
+#endif
+ 
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,234 @@
+#include <vector>
+#include "mbed.h"
+#include "Dynamixel.h"
+#include "MEMS.h"
+
+struct ArmPacketBits {
+                      unsigned int mode : 2;
+                      unsigned int linear : 2;
+                      unsigned int pitch : 2;
+                      unsigned int yaw : 2;
+                     };
+
+union ArmPacket {
+                 struct ArmPacketBits b;
+                 unsigned char as_byte;
+                };
+
+Serial rpi(USBTX, USBRX);
+
+AnalogIn co2(p20);
+
+DigitalOut dx_low(p16);
+DigitalOut dx_relay(p18);
+
+DigitalOut led_1(LED1);
+DigitalOut led_2(LED2);
+DigitalOut led_3(LED3);
+DigitalOut led_4(LED4);
+
+std::vector<Dynamixel*> servos;
+AX12 *linear = new AX12(p13, p14, 0);
+MX28 *pitch = new MX28(p13, p14, 1);
+MX28 *yaw = new MX28(p13, p14, 2);
+
+MEMS thermo_sensors[] = { MEMS(p9, p10),
+                          MEMS(p28, p27) };
+
+int minima[] = {100, 172, 360};
+int maxima[] = {300, 334, 360};
+float speeds[] = {0.1, 0.2, 0.2};
+int inits[] = {maxima[0], maxima[1], 0};
+int goals[] = {maxima[0], maxima[1], 0};
+
+void DxGoHome() {
+  led_2 = 1;
+  if (abs(linear->GetPosition() - inits[0]) < 2) {
+    pitch->SetGoal(inits[1]);
+    yaw->SetGoal(inits[2]);
+  } else {
+    linear->SetGoal(inits[0]);
+  }
+  led_2 = 0;
+}
+
+void DxGoHomeLoop() {
+  led_4 = 1;
+  led_2 = 1;
+  led_1 = 1;
+  linear->SetGoal(inits[0]);
+  wait(2);
+  led_1 = 0;
+  pitch->SetGoal(inits[1]);
+  yaw->SetGoal(inits[2]);
+  led_2 = 1;
+  led_3 = 1;
+  wait(5);
+  led_2 = 0;
+  led_3 = 0;
+  led_4 = 0;
+}
+
+void DxInitialize() {
+  led_1 = 1;
+  dx_low = 0;
+  dx_relay = 1;
+  
+  for (int i=0; i < 3; i++) {
+    servos[i]->SetCWLimit(minima[i]);
+    servos[i]->SetCCWLimit(maxima[i]);
+    servos[i]->SetCRSpeed(speeds[i]);
+  }
+  led_1 = 0;
+}
+
+void DxReset() {
+  led_4 = 0;
+  led_3 = 1;
+  dx_relay = 0;
+  wait_ms(10);
+  DxInitialize();
+  led_3 = 0;
+}
+
+void DxEnd() {
+  DxGoHomeLoop();
+  dx_relay = 0;
+  led_4 = 1;
+}
+
+float GetCO2() {
+  return co2.read() * 5000 + 400;  // ppm
+}
+
+int main() {
+  led_1 = 1;
+  led_2 = 1;
+  led_3 = 1;
+  led_4 = 1;
+  
+  servos.push_back(linear);
+  servos.push_back(pitch);
+  servos.push_back(yaw);
+  
+  float positions[3];
+  float values[3];
+  float thermo_data[2][16];
+  float co2_data;
+  int commands[3];
+
+  union ArmPacket packet;
+
+  rpi.baud(38400);  // Match this in the RPi settings.
+  led_1 = 0;
+  led_2 = 0;
+  led_3 = 0;
+  led_4 = 0;
+  
+  DxInitialize();  // Comment this out when testing without the arm.
+  DxGoHomeLoop();
+  
+  while (1) {
+    led_3 = 1;
+    packet.as_byte = rpi.getc();
+    led_3 = 0;
+
+    for (int i = 0; i < 3; i++) {
+      positions[i] = -1;
+      values[i] = -1;
+    }
+
+    commands[0] = packet.b.linear;
+    commands[1] = packet.b.pitch;
+    commands[2] = packet.b.yaw;
+
+    rpi.printf("%d %d %d %d\n", packet.b.mode, packet.b.linear, packet.b.pitch, packet.b.yaw);
+    
+    switch (packet.b.mode) {
+      case 0: {
+        if (not dx_relay) {
+          break;
+        }
+        
+        linear->SetTorqueLimit(1);  // Reset torque limit
+        led_3 = 1;
+        led_4 = 1;
+        for (int i=0; i < 3; i++) {
+          positions[i] = servos[i]->GetPosition();
+          if (i == 0) {
+            values[i] = servos[i]->GetVolts();
+          } else {
+            values[i] = servos[i]->GetCurrent();
+          }
+          
+          if (commands[i] == 1) {
+            servos[i]->SetGoal(positions[i] + 1);
+          } else if (commands[i] == 2) {
+            servos[i]->SetGoal(positions[i] - 1);
+          }
+        }
+        led_3 = 0;
+        led_4 = 0;
+        break;
+      } case 1: {
+        if (dx_relay) {
+          DxGoHomeLoop();
+        }
+        break;
+      } case 2: {
+        DxReset();
+        break;
+      } case 3: {
+        if (packet.b.linear) {
+          led_1 = 1;
+          led_2 = 1;
+          rpi.printf("arm\n");
+          led_1 = 0;
+          led_2 = 0;
+        } else {
+          if (dx_relay) {
+            DxEnd();
+          }
+        }
+        break;
+      }
+    }
+    
+    led_1 = 1;
+    led_2 = 1;
+    for (int i=0; i < 2; i++) {
+      thermo_sensors[i].temp(thermo_data[i]);
+    }
+    
+    co2_data = GetCO2();
+    led_1 = 0;
+    led_2 = 0;
+    
+    led_2 = 1;
+    led_3 = 1;
+    // Send Dynamixel position
+    for (int i=0; i < 3; i++) {
+      rpi.printf("%4.1f ", positions[i]);
+    }
+    
+    // Send Dynamixel values
+    for (int i=0; i < 3; i++) {
+      rpi.printf("%4.1f ", values[i]);
+    }
+    
+    // Send thermo data
+    for (int i=0; i < 2; i++) {
+      for (int j=0; j < 16; j++) {
+        rpi.printf("%4.1f ", thermo_data[i][j]);
+      }
+    }
+    
+    // Send CO2 data
+    rpi.printf("%4.1f", co2_data);
+    
+    // End transmission
+    rpi.printf("\n");
+    led_2 = 0;
+    led_3 = 0;
+  }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Apr 24 01:55:32 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/487b796308b0
\ No newline at end of file