Arm control program for Yozakura

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
masasin
Date:
Fri Apr 24 01:55:32 2015 +0000
Commit message:
Initial commit

Changed in this revision

Dynamixel/AX12.cpp Show annotated file Show diff for this revision Revisions of this file
Dynamixel/Dynamixel.h Show annotated file Show diff for this revision Revisions of this file
Dynamixel/MX28.cpp Show annotated file Show diff for this revision Revisions of this file
MEMS/MEMS.cpp Show annotated file Show diff for this revision Revisions of this file
MEMS/MEMS.h Show annotated file Show diff for this revision Revisions of this file
SerialHalfDuplex/SerialHalfDuplex.cpp Show annotated file Show diff for this revision Revisions of this file
SerialHalfDuplex/SerialHalfDuplex.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 6b3497b2f2ec Dynamixel/AX12.cpp
--- /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
diff -r 000000000000 -r 6b3497b2f2ec Dynamixel/Dynamixel.h
--- /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
diff -r 000000000000 -r 6b3497b2f2ec Dynamixel/MX28.cpp
--- /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
+}
diff -r 000000000000 -r 6b3497b2f2ec MEMS/MEMS.cpp
--- /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;
+}
diff -r 000000000000 -r 6b3497b2f2ec MEMS/MEMS.h
--- /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
diff -r 000000000000 -r 6b3497b2f2ec SerialHalfDuplex/SerialHalfDuplex.cpp
--- /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
diff -r 000000000000 -r 6b3497b2f2ec SerialHalfDuplex/SerialHalfDuplex.h
--- /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
diff -r 000000000000 -r 6b3497b2f2ec main.cpp
--- /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;
+  }
+}
diff -r 000000000000 -r 6b3497b2f2ec mbed.bld
--- /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