2018/06/08

Dependencies:   LSM9DS0 mbed

Files at this revision

API Documentation at this revision

Comitter:
cpul5338
Date:
Fri Jun 08 14:11:49 2018 +0000
Commit message:
2018/06/08

Changed in this revision

Controller.cpp Show annotated file Show diff for this revision Revisions of this file
Controller.h Show annotated file Show diff for this revision Revisions of this file
LSM9DS0.lib Show annotated file Show diff for this revision Revisions of this file
Mx28.cpp Show annotated file Show diff for this revision Revisions of this file
Mx28.h Show annotated file Show diff for this revision Revisions of this file
SensorFusion.cpp Show annotated file Show diff for this revision Revisions of this file
SensorFusion.h Show annotated file Show diff for this revision Revisions of this file
SystemConstant.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 bf9bf4b7625f Controller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,72 @@
+#include "Controller.h"
+#include "SystemConstant.h"
+#include "SensorFusion.h"
+
+bool test1 = 0;
+float sigma = 0.0;
+float alpha_1 = 0.0;
+float alpha_2 = 0.0;
+float K_1[3] = {1175.5, 302.2, -27.7};
+float K_2[3] = {193.573, 50.0229, -2.8192};
+float K_LQR01[3] = {39.9682, 9.2497, -0.0712};          // R = 1
+float K_LQR11[3] = {107.3029, 26.5432, -2.1435};        // R = 0.01
+float K_LQR21[3] = {285.88, 72.6362, -7.5105};          // R = 0.001
+float K_LQR31[3] = {444.0651, 106.7309, -12.7130};       // R = 0.0001
+float K_LQR75[3] = {789.0790, 190.093 -23.4674};        // R = 0.0003
+float K_LQR755[3]= {981.915, 252.3768, -28.3543};       // R = 0.000075
+float K_LQR655[3]= {1053.0382, 270.7444, -30.4835};     // R = 0.00065
+float K_LQR55[3] = {1197.4, 308.0321, -34.8059};        // R = 0.00005
+float K_LQR65[3] = {1095.0974, 281.6061, -31.7426};     // R = 0.00006
+float K_LQR35[3] = {1539.18735, 396.2928, -45.0369};    // R = 0.00006
+float K_LQR15[3] = {2649.2, 682.9531, -78.2644};        // R = 0.00001
+float K_LQR85[3] = {951.4667, 244.5136, -27.4427};      // R = 0.00008
+float K_LQR95[3] = {898.3697, 230.8014, -25.8531};      // R = 0.00009
+float u_1 = 0.0;
+float u_2 = 0.0;
+float u_3 = 0.0;
+float u_d = 0.0;
+float u = 0.0;
+float roll_ref = 0.0;
+float roll_err = 0.0;
+float steer_ref = 0.0;
+float steer_ref_old = 0.0;
+float steer_rad = 0.0;
+float steer_rad_old = 0.0;
+float steer_degree = 0.0;
+float steering_angle = 0.0f;
+
+
+//*************************************************************************************************************************************************************************************************************************
+//                                                                              Controller
+void controller(float velocity)
+{
+    roll_err = roll_angle - roll_ref;
+    
+    // Controller from Linear Quadratic Regulator
+//    u_1 = - K_LQR75[0]*roll_err;
+//    u_2 = - K_LQR655[1]*droll_angle;
+//    u_3 = - K_LQR75[2]*(steer_rad-steer_ref);
+//    u   = u_1 + u_2 + u_3 - 18.8803f*roll_ref;
+    
+    
+    // Controller from Linear Matrice Inequality
+    sigma = (velocity - Vmin) / (Vmax - Vmin);
+    alpha_1 = (1 - sigma);
+    alpha_2 = sigma;
+    u_1 = -K_1[0]*roll_err*alpha_1 - K_2[0]*roll_err*alpha_2;
+    u_2 = -K_1[1]*droll_angle*alpha_1 - K_2[1]*droll_angle*alpha_2;
+    u_3 = -K_1[2]*(steer_rad-steer_ref)*alpha_1 - K_2[2]*(steer_rad-steer_ref)*alpha_2;
+    u = u_1 + u_2 + u_3  - 18.8803f*roll_ref;
+    
+}// controller
+
+//*************************************************************************************************************************************************************************************************************************
+//                                                                              steer_angle
+void steer_angle(float u_in, float velocity)
+{
+    steer_rad = lpf(u_in, steer_rad_old, u2steer_lpf_freq * velocity) * u2steer_gain / velocity; 
+    steer_rad_old = steer_rad;
+    steering_angle = steer_rad/3.14f*180.0f;
+    if(steering_angle > 60) steering_angle =  60;
+    if(steering_angle <-60) steering_angle = -60;
+}// steer_angle
\ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f Controller.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,43 @@
+#include "mbed.h"
+#include "SystemConstant.h"
+#include "SensorFusion.h"
+
+#ifndef CONTROLLER_H_INCLUDE
+#define CONTROLLER_H_INCLUDE
+extern bool test1;
+extern float sigma;
+extern float alpha_1;
+extern float alpha_2;
+extern float roll_err;
+extern float K_1[3];
+extern float K_2[3];
+extern float K_LQR01[3];
+extern float K_LQR11[3];
+extern float K_LQR21[3];
+extern float K_LQR31[3];
+extern float K_LQR75[3];
+extern float K_LQR55[3];
+extern float K_LQR65[3];
+extern float K_LQR35[3];
+extern float K_LQR15[3];
+extern float K_LQR85[3];
+extern float K_LQR95[3];
+extern float K_LQR655[3];
+extern float K_LQR755[3];
+extern float u_1;
+extern float u_2;
+extern float u_3;
+extern float u_d;
+extern float u;
+extern float roll_ref;
+extern float steer_ref;
+extern float steer_ref_old;
+extern float steer_rad;
+extern float steering_angle;
+extern float steer_rad_old;
+extern float steer_degree;
+
+extern void controller(float velocity);
+extern void steer_angle(float u_in, float velocity);
+extern void anti_widup(void);
+#endif// CONTROLLER_H_INCLUDE
diff -r 000000000000 -r bf9bf4b7625f LSM9DS0.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS0.lib	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/cpul5338/code/LSM9DS0/#b9e5363b8c38
diff -r 000000000000 -r bf9bf4b7625f Mx28.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mx28.cpp	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,1024 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include "mbed.h"
+#include "Mx28.h"
+
+
+    unsigned char   Instruction_Packet_Array[15];   // Array to hold instruction packet data 
+    unsigned char   Status_Packet_Array[8];         // Array to hold returned status packet data
+    unsigned int    Time_Counter;                   // Timer for time out watchers
+    unsigned char   Direction_Pin;                  // Pin to control TX/RX buffer chip
+    unsigned char   Status_Return_Value = READ;     // Status packet return states ( NON , READ , ALL )
+
+//-------------------------------------------------------------------------------------------------------------------------------
+//            Private Methods 
+//-------------------------------------------------------------------------------------------------------------------------------
+void DynamixelClass::debugframe(void) {
+ for (int i=0; i<10; i++)
+  printf("%x,",Instruction_Packet_Array[i]);
+ printf("\r\n"); 
+}
+
+void DynamixelClass::debugStatusframe(void) {
+ for (int i=0; i<10; i++)
+  printf("%x,",Status_Packet_Array[i]);
+ printf("\r\n"); 
+}
+
+
+void DynamixelClass::transmitInstructionPacket(void){                                   // Transmit instruction packet to Dynamixel
+
+    unsigned char Counter;
+    Counter = 0;    
+    servoSerialDir->write(1);                                                          // Set TX Buffer pin to HIGH    
+
+    servoSerial->putc(HEADER);                                                              // Write Header (0xFF) data 1 to serial                     
+    servoSerial->putc(HEADER);                                                              // Write Header (0xFF) data 2 to serial
+    servoSerial->putc(Instruction_Packet_Array[0]);                                         // Write Dynamixal ID to serial 
+    servoSerial->putc(Instruction_Packet_Array[1]);                                         // Write packet length to serial    
+    
+    do{                                                                                 
+        servoSerial->putc(Instruction_Packet_Array[Counter + 2]);                           // Write Instuction & Parameters (if there is any) to serial
+        Counter++;
+    }while((Instruction_Packet_Array[1] - 2) >= Counter);
+    
+    servoSerial->putc(Instruction_Packet_Array[Counter + 2]);                               // Write check sum to serial
+    wait_us((Counter + 4)*11);
+    servoSerialDir->write(0);                                                          // Set TX Buffer pin to LOW after data has been sent
+
+}
+
+
+unsigned int DynamixelClass::readStatusPacket(void){
+
+    unsigned char Counter = 0x00;
+
+    static unsigned char InBuff[20];
+    unsigned char i, j, RxState;
+
+    
+    Status_Packet_Array[0] = 0x00;
+    Status_Packet_Array[1] = 0x00;
+    Status_Packet_Array[2] = 0x00;                                                      
+    Status_Packet_Array[3] = 0x00;
+    i=0; RxState=0; j=0; InBuff[0]=0;
+    Timer timer;
+    timer.start();
+
+
+while (RxState<3){                              // Wait for " header + header + frame length + error " RX data
+ if (timer.read_ms() >= STATUS_PACKET_TIMEOUT){
+        return Status_Packet_Array[2] = 0x80;                                      // Return with Error if Serial data not received with in time limit
+
+ }
+
+ if (servoSerial->readable()) {
+   InBuff[i]=servoSerial->getc();
+   if (InBuff[0]==0xff) i++;                                                            // When we have the first header we starts to inc data to inbuffer
+
+   
+   if ((i>=(STATUS_FRAME_BUFFER-1)) &&(RxState==0)) RxState++; //read header
+   
+   switch (RxState) {
+    case 1: {//Read header 
+            if ((InBuff[j] == 0xFF) &&  (InBuff[j+1]== 0xFF)){                                    // checkes that we have got the buffer
+                    j=2;
+                    Status_Packet_Array[0] = InBuff[j++];                                    // ID sent from Dynamixel
+                    Status_Packet_Array[1] = InBuff[j++];                                    // Frame Length of status packet
+                    Status_Packet_Array[2] = InBuff[j++];                                    // Error (char) 
+                    RxState++; led2->write(0);
+            }
+    } break;
+    case 2: {//Read data
+      if (i>Status_Packet_Array[1]+3) { //We have recieved all data
+            do{
+                Status_Packet_Array[3 + Counter] = InBuff[j++];
+                Counter++;              
+            }while(Status_Packet_Array[1] > Counter);                           // Read Parameter(s) into array
+            
+            Status_Packet_Array[Counter + 4] = InBuff[j++];                          // Read Check sum   
+           RxState++; 
+        }    
+      } break;      
+   } //switch 
+   }
+  }//while..
+  timer.stop();
+//  debugStatusframe();
+
+  return 0x00;
+}
+
+
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Public Methods 
+//-------------------------------------------------------------------------------------------------------------------------------
+
+ DynamixelClass::DynamixelClass(int baud, PinName D_Pin, PinName tx, PinName rx){ 
+    servoSerial=new Serial(tx, rx);
+    servoSerial->baud(baud);
+    servoSerialDir= new DigitalOut(D_Pin);     
+    servoSerialDir->write(0);
+    led2=new DigitalOut(LED2); 
+
+}   
+
+ DynamixelClass::~DynamixelClass(){
+    if(servoSerial != NULL)
+        delete servoSerial;
+}
+
+unsigned int DynamixelClass::reset(unsigned char ID){
+
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = RESET_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_RESET;
+    Instruction_Packet_Array[3] = ~(ID + RESET_LENGTH + COMMAND_RESET); //Checksum;
+ 
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }   
+}
+
+void DynamixelClass::NewBaudRate(int baud) {
+//Change the baudrate on then MBED
+    int Baudrate_BPS = 0;
+    Baudrate_BPS  =(int) 2000000 / (baud + 1);                        // Calculate Baudrate as ber "Robotis e-manual"
+    servoSerial->baud(Baudrate_BPS);
+}
+
+unsigned int DynamixelClass::ping(unsigned char ID){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = PING_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_PING;
+    Instruction_Packet_Array[3] = ~(ID + PING_LENGTH + COMMAND_PING);
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+        return (Status_Packet_Array[0]);            // Return SERVO ID
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+    }            
+}
+
+unsigned int DynamixelClass::setStatusPaketReturnDelay(unsigned char ID,unsigned char ReturnDelay){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_RETURN_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_RETURN_DELAY_TIME;
+    Instruction_Packet_Array[4] = (char) (ReturnDelay/2);
+    Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_DELAY_TIME + (char)(ReturnDelay/2));  
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }       
+
+}
+
+unsigned int DynamixelClass::setID(unsigned char ID, unsigned char New_ID){    
+
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_ID_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_ID;
+    Instruction_Packet_Array[4] = New_ID;
+    Instruction_Packet_Array[5] = ~(ID + SET_ID_LENGTH + COMMAND_WRITE_DATA+ EEPROM_ID + New_ID);  
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }       
+}
+
+unsigned int DynamixelClass::setBaudRate(unsigned char ID, long Baud){                      
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_BD_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_BAUD_RATE;
+//  if (Baud > 2250000){
+if (Baud >= 1000000){
+    switch (Baud){
+        case 2250000:
+        Instruction_Packet_Array[4] = 0xFA;
+        break;
+        case 2500000:
+        Instruction_Packet_Array[4] = 0xFB;
+        break;
+        case 3000000:
+        Instruction_Packet_Array[4] = 0xFC;
+        break;
+        case 1000000:
+        Instruction_Packet_Array[4] = 0x01;
+        }
+    }else{
+    Instruction_Packet_Array[4] = (char)((2000000/Baud) - 1);
+    }
+    Instruction_Packet_Array[5] = ~(ID + SET_BD_LENGTH + COMMAND_WRITE_DATA + EEPROM_BAUD_RATE + (char)((2000000/Baud) - 1) ); 
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }   
+}
+
+unsigned int DynamixelClass::setMaxTorque( unsigned char ID, int Torque){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_MAX_TORQUE_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_MAX_TORQUE_L ;
+    Instruction_Packet_Array[4] = (char)(Torque);
+    Instruction_Packet_Array[5] = (char)(Torque >> 8);
+    Instruction_Packet_Array[6] = ~(ID + SET_MAX_TORQUE_LENGTH + COMMAND_WRITE_DATA + EEPROM_MAX_TORQUE_L + (char)(Torque) + (char)(Torque >> 8));
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }       
+}
+
+unsigned int DynamixelClass::setHoldingTorque(unsigned char ID, bool Set){
+  
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_HOLDING_TORQUE_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_TORQUE_ENABLE;
+    Instruction_Packet_Array[4] = Set;
+    Instruction_Packet_Array[5] = ~(ID + SET_HOLDING_TORQUE_LENGTH + COMMAND_WRITE_DATA + RAM_TORQUE_ENABLE + Set); 
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }       
+}
+
+unsigned int DynamixelClass::setAlarmShutdown(unsigned char  ID,unsigned char Set){
+
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_ALARM_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_ALARM_SHUTDOWN;
+    Instruction_Packet_Array[4] = Set;
+    Instruction_Packet_Array[5] = ~(ID + SET_ALARM_LENGTH + COMMAND_WRITE_DATA + EEPROM_ALARM_SHUTDOWN + Set);  
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }           
+
+}
+
+unsigned int DynamixelClass::setStatusPaket(unsigned char  ID,unsigned char Set){
+
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_RETURN_LEVEL_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_RETURN_LEVEL;
+    Instruction_Packet_Array[4] = Set;
+    Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LEVEL_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_LEVEL + Set);
+    
+    Status_Return_Value = Set;
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }       
+
+}
+
+
+unsigned int DynamixelClass::setMode(unsigned char ID, bool Dynamixel_Mode, unsigned int Dynamixel_CW_Limit,unsigned int Dynamixel_CCW_Limit){
+
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_MODE_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_CW_ANGLE_LIMIT_L;
+     if ( Dynamixel_Mode == WHEEL) {                                    // Set WHEEL mode, this is done by setting both the clockwise and anti-clockwise angle limits to ZERO
+        Instruction_Packet_Array[4] = 0x00;
+        Instruction_Packet_Array[5] = 0x00;
+        Instruction_Packet_Array[6] = 0x00;
+        Instruction_Packet_Array[7] = 0x00;
+        Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L);
+    }else {                                                             // Else set SERVO mode
+        Instruction_Packet_Array[4] = (char)(Dynamixel_CW_Limit);
+        Instruction_Packet_Array[5] = (char)((Dynamixel_CW_Limit & 0x0F00) >> 8);
+        Instruction_Packet_Array[6] = (char)(Dynamixel_CCW_Limit);
+        Instruction_Packet_Array[7] = (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8);
+        Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L + (char)(Dynamixel_CW_Limit) + (char)((Dynamixel_CW_Limit & 0x0F00) >> 8) + (char)(Dynamixel_CCW_Limit) + (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8));
+    }   
+        
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    
+        if (Status_Return_Value == ALL){
+        readStatusPacket();
+            if (Status_Packet_Array[2] != 0){
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+            }
+            
+        }
+   return 0x00; //if no errors
+ } 
+ 
+ unsigned int DynamixelClass::setPunch(unsigned char ID,unsigned int Punch){
+ 
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_PUNCH_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_PUNCH_L;
+    Instruction_Packet_Array[4] = (char)(Punch);
+    Instruction_Packet_Array[5] = (char)(Punch >> 8);
+    Instruction_Packet_Array[6] = ~(ID + SET_PUNCH_LENGTH + COMMAND_WRITE_DATA + RAM_PUNCH_L + (char)(Punch) + (char)(Punch >> 8) );
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }   
+ 
+ }
+ 
+ unsigned int DynamixelClass::setPID(unsigned char ID ,unsigned char P,unsigned char I,unsigned char D){
+ 
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_PID_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_PROPORTIONAL_GAIN;
+    Instruction_Packet_Array[4] = P;
+    Instruction_Packet_Array[5] = I;
+    Instruction_Packet_Array[6] = D;
+    Instruction_Packet_Array[7] = ~(ID + SET_PID_LENGTH + COMMAND_WRITE_DATA + RAM_PROPORTIONAL_GAIN + P + I + D );
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }
+ }
+
+unsigned int DynamixelClass::setTemp(unsigned char ID,unsigned char temp){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_TEMP_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_LIMIT_TEMPERATURE;
+    Instruction_Packet_Array[4] = temp;
+    Instruction_Packet_Array[5] = ~(ID + SET_TEMP_LENGTH + COMMAND_WRITE_DATA + EEPROM_LIMIT_TEMPERATURE + temp);   
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }   
+}
+
+unsigned int DynamixelClass::setVoltage(unsigned char ID,unsigned char Volt_L, unsigned char Volt_H){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SET_VOLT_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = EEPROM_LOW_LIMIT_VOLTAGE;
+    Instruction_Packet_Array[4] = Volt_L;
+    Instruction_Packet_Array[5] = Volt_H;
+    Instruction_Packet_Array[6] = ~(ID + SET_VOLT_LENGTH + COMMAND_WRITE_DATA + EEPROM_LOW_LIMIT_VOLTAGE + Volt_L + Volt_H);    
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }
+}
+ 
+unsigned int DynamixelClass::servo(unsigned char ID,unsigned int Position,unsigned int Speed){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
+    Instruction_Packet_Array[4] = (char)(Position);
+    Instruction_Packet_Array[5] = (char)((Position & 0x0F00) >> 8);
+    Instruction_Packet_Array[6] = (char)(Speed);
+    Instruction_Packet_Array[7] = (char)((Speed & 0x0F00) >> 8);
+    Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + Position + (char)((Position & 0x0F00) >> 8) + Speed + (char)((Speed & 0x0F00) >> 8));   
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }
+}
+
+unsigned int DynamixelClass::servoPreload(unsigned char ID,unsigned int Position,unsigned int Speed){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
+    Instruction_Packet_Array[4] = (char)(Position);
+    Instruction_Packet_Array[5] = (char)(Position >> 8);
+    Instruction_Packet_Array[6] = (char)(Speed);
+    Instruction_Packet_Array[7] = (char)(Speed >> 8);
+    Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_POSITION_L + (char)(Position) + (char)(Position >> 8) + (char)(Speed) + (char)(Speed >> 8)); 
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }
+}
+ 
+unsigned int DynamixelClass::wheel(unsigned char ID, bool Rotation,unsigned int Speed){ 
+
+    char Speed_H,Speed_L;
+    Speed_L = Speed;    
+        if (Rotation == 0){                         // Move Left                     
+            Speed_H = Speed >> 8;
+            }
+        else if (Rotation == 1){                    // Move Right
+            Speed_H = (Speed >> 8)+4;   
+            }   
+            
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = WHEEL_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
+    Instruction_Packet_Array[4] = Speed_L;
+    Instruction_Packet_Array[5] = Speed_H;
+    Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_SPEED_L  + Speed_L + Speed_H);            
+            
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }           
+
+}
+
+void DynamixelClass::wheelSync(unsigned char ID1, bool Dir1, unsigned int Speed1, unsigned char ID2, bool Dir2, unsigned int Speed2, unsigned char ID3, bool Dir3, unsigned int Speed3){
+    
+    char Speed1_H,Speed1_L;
+    Speed1_L = Speed1; 
+        if (Dir1 == 0){                          // Move Left
+            Speed1_H = Speed1 >> 8;
+        }
+        else if (Dir1 == 1)                     // Move Right
+        {   
+            Speed1_H = (Speed1 >> 8)+4;
+        }   
+
+    char Speed2_H,Speed2_L;
+    Speed2_L = Speed2; 
+        if (Dir2 == 0){                          // Move Left
+            Speed2_H = Speed2 >> 8;
+        }
+        else if (Dir2 == 1)                     // Move Right
+        {   
+            Speed2_H = (Speed2 >> 8)+4;
+        }
+  
+    char Speed3_H,Speed3_L;
+    Speed3_L = Speed3; 
+        if (Dir3 == 0){                          // Move Left
+            Speed3_H = Speed3 >> 8;
+        }
+        else if (Dir3 == 1)                     // Move Right
+        {   
+            Speed3_H = (Speed3 >> 8)+4;
+        }       
+        
+    Instruction_Packet_Array[0] = 0xFE;         // When Writing a Sync comman you must address all(0xFE) servos
+    Instruction_Packet_Array[1] = SYNC_LOAD_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_SYNC_WRITE;
+    Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
+    Instruction_Packet_Array[4] = SYNC_DATA_LENGTH;
+    Instruction_Packet_Array[5] = ID1;
+    Instruction_Packet_Array[6] = Speed1_L;
+    Instruction_Packet_Array[7] = Speed1_H;
+    Instruction_Packet_Array[8] = ID2;
+    Instruction_Packet_Array[9] = Speed2_L;
+    Instruction_Packet_Array[10] = Speed2_H;
+    Instruction_Packet_Array[11] = ID3;
+    Instruction_Packet_Array[12] = Speed3_L;
+    Instruction_Packet_Array[13] = Speed3_H;    
+    Instruction_Packet_Array[14] = (char)(~(0xFE + SYNC_LOAD_LENGTH + COMMAND_SYNC_WRITE + RAM_GOAL_SPEED_L + SYNC_DATA_LENGTH + ID1 + Speed1_L + Speed1_H + ID2 + Speed2_L + Speed2_H + ID3 + Speed3_L + Speed3_H));             
+    
+    transmitInstructionPacket();
+ 
+}
+
+unsigned int DynamixelClass::wheelPreload(unsigned char ID, bool Dir,unsigned int Speed){
+    
+    char Speed_H,Speed_L;
+    Speed_L = Speed; 
+        if (Dir == 0){                          // Move Left
+            Speed_H = Speed >> 8;
+        }
+        else if (Dir == 1)                      // Move Right
+        {   
+            Speed_H = (Speed >> 8)+4;
+        }   
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = WHEEL_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
+    Instruction_Packet_Array[4] = Speed_L;
+    Instruction_Packet_Array[5] = Speed_H;
+    Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H);             
+            
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }       
+
+}
+
+unsigned int DynamixelClass::action(unsigned char ID){
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = RESET_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_ACTION;
+    Instruction_Packet_Array[3] = ~(ID + ACTION_LENGTH + COMMAND_ACTION);
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }       
+}
+
+unsigned int DynamixelClass::ledState(unsigned char ID, bool Status){  
+  
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = LED_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_LED;
+    Instruction_Packet_Array[4] = Status;
+    Instruction_Packet_Array[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + Status);   
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }   
+}
+
+unsigned int DynamixelClass::readTemperature(unsigned char ID){ 
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_TEMP_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE;
+    Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH  + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH);
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket(); 
+
+    if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+        return Status_Packet_Array[3];
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+    }
+}
+
+unsigned int DynamixelClass::readPosition(unsigned char ID){    
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_POS_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L;
+    Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH);  
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value                                          // If there is no status packet error return value
+        return Status_Packet_Array[4] << 8 | Status_Packet_Array[3];    // Return present position value
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);                           // If there is a error Returns error value
+    }
+}
+
+unsigned int DynamixelClass::readLoad(unsigned char ID){    
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_LOAD_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_PRESENT_LOAD_L;
+    Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_LOAD_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_LOAD_L  + READ_TWO_BYTE_LENGTH);
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){                                           // If there is no status packet error return value
+        return ((Status_Packet_Array[4] << 8) | Status_Packet_Array[3]);    // Return present load value
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);                                   // If there is a error Returns error value
+    }
+}
+
+unsigned int DynamixelClass::readSpeed(unsigned char ID){   
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_SPEED_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_PRESENT_SPEED_L;
+    Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_SPEED_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_SPEED_L + READ_TWO_BYTE_LENGTH);
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){                                           // If there is no status packet error return value
+        return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3];  // Return present position value
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);                           // If there is a error Returns error value
+    }
+}
+
+
+unsigned int DynamixelClass::readVoltage(unsigned char ID){    
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_VOLT_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE;
+    Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_VOLT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH);    
+
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){                   // If there is no status packet error return value
+        return Status_Packet_Array[3];                  // Return voltage value (value retured by Dynamixel is 10 times actual voltage)
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+    }
+}
+
+unsigned int DynamixelClass::checkRegister(unsigned char ID){    
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_REGISTER_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_REGISTER;
+    Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_REGISTER_LENGTH + COMMAND_READ_DATA + RAM_REGISTER + READ_ONE_BYTE_LENGTH);
+
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){                   // If there is no status packet error return value
+        return (Status_Packet_Array[3]);            // Return register value
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+    }
+}
+
+unsigned int DynamixelClass::checkMovement(unsigned char ID){    
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_MOVING_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_MOVING;
+    Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_MOVING_LENGTH + COMMAND_READ_DATA + RAM_MOVING + READ_ONE_BYTE_LENGTH);
+
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){                   // If there is no status packet error return value
+        return (Status_Packet_Array[3]);            // Return movement value
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);            // If there is a error Returns error value
+    }
+}
+
+unsigned int DynamixelClass::checkLock(unsigned char ID){    
+    
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = READ_LOCK_LENGTH;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = RAM_LOCK;
+    Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_LOCK_LENGTH + COMMAND_READ_DATA + RAM_LOCK + READ_ONE_BYTE_LENGTH);   
+
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket();
+    
+    if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+        return (Status_Packet_Array[3]);            // Return Lock value
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+    }
+}
+
+unsigned int DynamixelClass::torqueMode(unsigned char ID, bool Status){  
+  
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = 0x04;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_TORQUE_CONTROL_MODE_ENABLE;
+    Instruction_Packet_Array[4] = Status;
+    Instruction_Packet_Array[5] = ~(ID + 0x04 + COMMAND_WRITE_DATA + RAM_TORQUE_CONTROL_MODE_ENABLE + Status);   
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }   
+}
+
+unsigned int DynamixelClass::torque(unsigned char ID,unsigned int Torque){ 
+  
+            
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = 0x05;
+    Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
+    Instruction_Packet_Array[3] = RAM_GOAL_TORQUE_L;
+    Instruction_Packet_Array[4] = (char)(Torque);
+    Instruction_Packet_Array[5] = (char)((Torque & 0x0F00) >> 8);    
+    Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + RAM_GOAL_TORQUE_L  + (char)(Torque) + (char)((Torque & 0x0F00) >> 8) );            
+            
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();    
+    
+    if (ID == 0XFE || Status_Return_Value != ALL ){     // If ID of FE is used no status packets are returned so we do not need to check it
+        return (0x00);
+    }else{
+        readStatusPacket();
+        if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+            return (Status_Packet_Array[0]);            // Return SERVO ID
+        }else{
+            return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+        }
+    }           
+
+}
+
+unsigned int DynamixelClass::readRegister(unsigned char ID,unsigned char Register){ 
+        
+    Instruction_Packet_Array[0] = ID;
+    Instruction_Packet_Array[1] = 0x04;
+    Instruction_Packet_Array[2] = COMMAND_READ_DATA;
+    Instruction_Packet_Array[3] = Register;
+    Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
+    Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH  + COMMAND_READ_DATA + Register + READ_ONE_BYTE_LENGTH);
+    
+    if (servoSerial->readable()) 
+      while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
+
+    transmitInstructionPacket();
+    readStatusPacket(); 
+
+    if (Status_Packet_Array[2] == 0){               // If there is no status packet error return value
+        return Status_Packet_Array[3];
+    }else{
+        return (Status_Packet_Array[2] | 0xF000);   // If there is a error Returns error value
+    }
+}
+
+
diff -r 000000000000 -r bf9bf4b7625f Mx28.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mx28.h	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,206 @@
+/*
+How Dynamixel work can be found
+--------------------------------
+Robotis e-Manual  
+http://support.robotis.com
+
+Overview of Communication 
+http://support.robotis.com/en/product/dynamixel/dxl_communication.htm
+
+Kind of Instruction 
+http://support.robotis.com/en/product/dynamixel/communication/dxl_instruction.htm 
+
+Instruction Packet & Status Packet (Return Packet)
+http://support.robotis.com/en/product/dynamixel/communication/dxl_packet.htm
+ 
+Control Table
+http://support.robotis.com/en/product/dynamixel/mx_series/mx-28.htm
+
+ */
+
+#ifndef Mx28_h
+#define Mx28_h
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// define - Dynamixel Hex code table 
+//-------------------------------------------------------------------------------------------------------------------------------
+// EEPROM AREA  
+#define EEPROM_MODEL_NUMBER_L           0x00
+#define EEPROM_MODEL_NUMBER_H           0x01
+#define EEPROM_VERSION                  0x02
+#define EEPROM_ID                       0x03
+#define EEPROM_BAUD_RATE                0x04
+#define EEPROM_RETURN_DELAY_TIME        0x05
+#define EEPROM_CW_ANGLE_LIMIT_L         0x06
+#define EEPROM_CW_ANGLE_LIMIT_H         0x07
+#define EEPROM_CCW_ANGLE_LIMIT_L        0x08
+#define EEPROM_CCW_ANGLE_LIMIT_H        0x09
+#define EEPROM_LIMIT_TEMPERATURE        0x0B
+#define EEPROM_LOW_LIMIT_VOLTAGE        0x0C
+#define EEPROM_HIGN_LIMIT_VOLTAGE       0x0D
+#define EEPROM_MAX_TORQUE_L             0x0E
+#define EEPROM_MAX_TORQUE_H             0x0F
+#define EEPROM_RETURN_LEVEL             0x10
+#define EEPROM_ALARM_LED                0x11
+#define EEPROM_ALARM_SHUTDOWN           0x12
+// RAM AREA  
+#define RAM_TORQUE_ENABLE               0x18
+#define RAM_LED                         0x19
+#define RAM_PROPORTIONAL_GAIN           0x1A
+#define RAM_INTERGRAL_GAIN              0x1B
+#define RAM_DERIVATIVE_GAIN             0x1C
+#define RAM_GOAL_POSITION_L             0x1E
+#define RAM_GOAL_POSITION_H             0x1F
+#define RAM_GOAL_SPEED_L                0x20
+#define RAM_GOAL_SPEED_H                0x21
+#define RAM_TORQUE_LIMIT_L              0x22
+#define RAM_TORQUE_LIMIT_H              0x23
+#define RAM_PRESENT_POSITION_L          0x24
+#define RAM_PRESENT_POSITION_H          0x25
+#define RAM_PRESENT_SPEED_L             0x26
+#define RAM_PRESENT_SPEED_H             0x27
+#define RAM_PRESENT_LOAD_L              0x28
+#define RAM_PRESENT_LOAD_H              0x29
+#define RAM_PRESENT_VOLTAGE             0x2A
+#define RAM_PRESENT_TEMPERATURE         0x2B
+#define RAM_REGISTER                    0x2C
+#define RAM_MOVING                      0x2E
+#define RAM_LOCK                        0x2F
+#define RAM_PUNCH_L                     0x30
+#define RAM_PUNCH_H                     0x31
+
+#define RAM_TORQUE_CONTROL_MODE_ENABLE  0X46
+#define RAM_GOAL_TORQUE_L               0X47
+#define RAM_GOAL_TORQUE_H               0X48
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Instruction commands Set 
+//-------------------------------------------------------------------------------------------------------------------------------
+#define COMMAND_PING                    0x01
+#define COMMAND_READ_DATA               0x02
+#define COMMAND_WRITE_DATA              0x03
+#define COMMAND_REG_WRITE_DATA          0x04
+#define COMMAND_ACTION                  0x05
+#define COMMAND_RESET                   0x06
+#define COMMAND_SYNC_WRITE              0x83
+
+
+//-------------------------------------------------------------------------------------------------------------------------------
+//Instruction packet lengths 
+#define READ_ONE_BYTE_LENGTH            0x01
+#define READ_TWO_BYTE_LENGTH            0x02
+#define RESET_LENGTH                    0x02
+#define PING_LENGTH                     0x02
+#define ACTION_LENGTH                   0x02
+#define SET_ID_LENGTH                   0x04
+#define SET_BD_LENGTH                   0x04
+#define SET_RETURN_LEVEL_LENGTH         0x04
+#define READ_TEMP_LENGTH                0x04
+#define READ_POS_LENGTH                 0x04
+#define READ_LOAD_LENGTH                0x04
+#define READ_SPEED_LENGTH               0x04
+#define READ_VOLT_LENGTH                0x04
+#define READ_REGISTER_LENGTH            0x04
+#define READ_MOVING_LENGTH              0x04
+#define READ_LOCK_LENGTH                0x04
+#define LED_LENGTH                      0x04
+#define SET_HOLDING_TORQUE_LENGTH       0x04
+#define SET_MAX_TORQUE_LENGTH           0x05
+#define SET_ALARM_LENGTH                0x04
+#define READ_LOAD_LENGTH                0x04
+#define SET_RETURN_LENGTH               0x04
+#define WHEEL_LENGTH                    0x05
+#define SERVO_GOAL_LENGTH               0x07
+#define SET_MODE_LENGTH                 0x07
+#define SET_PUNCH_LENGTH                0x04
+#define SET_PID_LENGTH                  0x06
+#define SET_TEMP_LENGTH                 0x04
+#define SET_VOLT_LENGTH                 0x05
+#define SYNC_LOAD_LENGTH                0x0D
+#define SYNC_DATA_LENGTH                0x02        
+
+
+//-------------------------------------------------------------------------------------------------------------------------------
+// Specials 
+//-------------------------------------------------------------------------------------------------------------------------------
+
+#define OFF                             0x00
+#define ON                              0x01
+
+#define SERVO                           0x01
+#define WHEEL                           0x00
+
+#define LEFT                            0x00
+#define RIGHT                           0x01
+
+#define NONE                            0x00
+#define READ                            0x01
+#define ALL                             0x02
+
+#define BROADCAST_ID                    0xFE
+
+#define HEADER                          0xFF
+
+#define STATUS_PACKET_TIMEOUT           50      // in millis()
+#define STATUS_FRAME_BUFFER             5
+
+
+
+class DynamixelClass {
+private:
+    DigitalOut *servoSerialDir;
+    Serial *servoSerial;                   
+    void transmitInstructionPacket(void);
+    unsigned int readStatusPacket(void);
+    bool overflow;
+    void debugframe(void);
+    void debugStatusframe(void);
+    DigitalOut *led2;   
+public:
+    
+     DynamixelClass(int baud,PinName D_Pin,PinName tx, PinName rx);    //Constructor
+     ~DynamixelClass(void);                 //destruktor
+
+    void NewBaudRate(int baud);
+    unsigned int reset(unsigned char);
+    unsigned int ping(unsigned char); 
+    
+    unsigned int setStatusPaketReturnDelay(unsigned char,unsigned char);    
+    unsigned int setID(unsigned char, unsigned char);
+    unsigned int setBaudRate(unsigned char, long);
+    unsigned int setMaxTorque(unsigned char, int);
+    unsigned int setHoldingTorque(unsigned char, bool);
+    unsigned int setAlarmShutdown(unsigned char,unsigned char);
+    unsigned int setStatusPaket(unsigned char,unsigned char);   
+    unsigned int setMode(unsigned char, bool, unsigned int, unsigned int);
+    unsigned int setPunch(unsigned char,unsigned int);
+    unsigned int setPID(unsigned char,unsigned char,unsigned char,unsigned char);
+    unsigned int setTemp(unsigned char,unsigned char);
+    unsigned int setVoltage(unsigned char,unsigned char,unsigned char);
+    
+    unsigned int servo(unsigned char, unsigned int, unsigned int);
+    unsigned int servoPreload(unsigned char, unsigned int, unsigned int);
+    unsigned int wheel(unsigned char, bool, unsigned int);
+    void wheelSync(unsigned char,bool,unsigned int,unsigned char, bool,unsigned int,unsigned char, bool,unsigned int);
+    unsigned int wheelPreload(unsigned char, bool, unsigned int);       
+    
+    unsigned int action(unsigned char);
+    
+    unsigned int readTemperature(unsigned char);
+    unsigned int readVoltage(unsigned char);
+    unsigned int readPosition(unsigned char);
+    unsigned int readLoad(unsigned char);
+    unsigned int readSpeed(unsigned char);
+    
+    unsigned int checkRegister(unsigned char);
+    unsigned int checkMovement(unsigned char);
+    unsigned int checkLock(unsigned char);
+    
+    unsigned int ledState(unsigned char, bool);
+
+    unsigned int torqueMode(unsigned char ID, bool Status);    
+    unsigned int torque(unsigned char ID,unsigned int Torque);
+    unsigned int readRegister(unsigned char ID,unsigned char Register);
+};
+
+#endif
diff -r 000000000000 -r bf9bf4b7625f SensorFusion.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorFusion.cpp	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,113 @@
+#include "SensorFusion.h"
+#include "SystemConstant.h"
+#include "math.h"
+
+///Gyro and Acce data
+float axm = 0.0f;
+float aym = 0.0f;
+float azm = 0.0f;
+float u1 = 0.0f;
+float u2 = 0.0f;
+float u3 = 0.0f;
+float mx = 0.0f;
+float my = 0.0f;
+float mz = 0.0f;
+float Ac[3] = {0.0f, 0.0f, 0.0f};
+
+float axm_f = 0.0f;
+float axm_f_old = 0.0f;
+float u3aym_f = 0.0f;
+float u3aym_f_old = 0.0f;
+float u2azm_f = 0.0f;
+float u2azm_f_old = 0.0f;
+
+float aym_f = 0.0f;
+float aym_f_old = 0.0f;
+float u3axm_f = 0.0f;
+float u3axm_f_old = 0.0f;
+float u1azm_f = 0.0f;
+float u1azm_f_old = 0.0f;
+
+float azm_f = 0.0f;
+float azm_f_old = 0.0f;
+float u2axm_f = 0.0f;
+float u2axm_f_old = 0.0f;
+float u1aym_f = 0.0f;
+float u1aym_f_old = 0.0f;
+
+float x1_hat = 0.0f;
+float x2_hat = 0.0f;
+float x3_hat = 0.0f;
+float sinroll = 0.0f;
+float cosroll = 0.0f;
+float roll_angle = 0.0f;
+float roll_angle_old = 0.0f;
+float droll_angle= 0.0f;
+float droll_angle_old= 0.0f;
+float sinpitch = 0.0f;
+float cospitch = 0.0f;
+float yaw_ref = 0.0f;
+float yaw_angle = 0.0f;
+float yaw_angle_old = 0.0f;
+float dyaw_angle = 0.0f;
+float dyaw_angle_old = 0.0f;
+// ***************************************************************************************************************************************************************************
+//                                                                              Low-Pass Filter
+float lpf(float input,float input_old,float frequency)
+{
+    float output = 0;
+    
+    //y[n] = a*y[n-1] + (1 - a)*x[n]
+    output = input*frequency*sample_time+input_old*(1-frequency*sample_time);
+    
+    return output;
+}// Low-Pass Filter
+
+// ***************************************************************************************************************************************************************************
+//                                                                              x2_hat_estimat with finding roll angle
+void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha)
+{
+    // x2_hat estimation
+    aym_f = lpf(a_ym,aym_f_old,alpha);
+    aym_f_old = aym_f;
+    u3axm_f = lpf(u_3*a_xm,u3axm_f_old,alpha); // w3axm
+    u3axm_f_old = u3axm_f;
+    u1azm_f = lpf(u_1*a_zm,u1azm_f_old,alpha); // w1azm
+    u1azm_f_old = u1azm_f;
+    
+    x2_hat = -u3axm_f/alpha + aym_f + u1azm_f/alpha; //THe Low-Pass Filter(alpha/(s+alpha)) have already been done  
+      
+   // Finding the roll angle and giving the limit for it
+    sinroll = x2_hat*(-0.1020); // This is for finding the roll angle, because x2_hat = -gsin(roll_angle). This is from paper Observer-Based Sensor Fusion.
+    if(sinroll >= 1.0f)
+    {
+        sinroll = 1.0;
+        cosroll = 0.0;
+    }
+    else if(sinroll <= -1.0f)
+    {
+        sinroll = -1.0;
+        cosroll = 0.0;
+    }
+    else cosroll = sqrt(1-(sinroll*sinroll));
+    roll_angle = asin(sinroll);///By 312 Rotation
+//    roll_angle = lpf(roll_angle, roll_angle_old, 3);
+//    roll_angle_old = roll_angle;
+}// roll_fusion
+
+// ***************************************************************************************************************************************************************************
+//                                                                              absolute
+float absolute(float value)
+{
+    if(value < 0)return -value;
+    else return value;
+}// absolute
+
+// ***************************************************************************************************************************************************************************
+//                                                                              Reset_data
+void Reset_data(void)
+{
+    axm_f = axm_f_old = u3aym_f = u3aym_f_old = u2azm_f = u2azm_f_old = 0.0;
+    aym_f = aym_f_old = u3axm_f = u3axm_f_old = u1azm_f = u1azm_f_old = 0.0;
+    azm_f = azm_f_old = u2axm_f = u2axm_f_old = u1aym_f = u1aym_f_old = 0.0;
+}// Reset_data
\ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f SensorFusion.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorFusion.h	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,28 @@
+#ifndef SENSORFUSION_H_INCLUDED
+#define SENSORFUSION_H_INCLUDED
+
+#include "math.h"
+#define FTimer          1000.0f             ///Hz
+#define sample_time     1.0f/FTimer
+#define Alpha           FTimer/25.0f
+
+extern float axm, aym, azm, u1, u2, u3, mx, my, mz;
+extern float Ac[3];
+
+extern float axm_f, axm_f_old, u3aym_f, u3aym_f_old, u2azm_f, u2azm_f_old;
+extern float aym_f, aym_f_old, u3axm_f, u3axm_f_old, u1azm_f, u1azm_f_old;
+extern float azm_f, azm_f_old, u2axm_f, u2axm_f_old, u1aym_f, u1aym_f_old;
+
+extern float x1_hat, x2_hat, x3_hat;
+extern float sinroll, cosroll, roll_angle, droll_angle, droll_angle_old;
+extern float sinpitch, cospitch, yaw_ref, yaw_angle, yaw_angle_old, dyaw_angle, dyaw_angle_old;
+
+//extern void CentrifugalAcce_Compensation(float velocity);
+extern float lpf(float input,float input_old,float frequency);
+extern void roll_fusion(float a_xm,float a_ym,float a_zm,float u_3,float u_1,float alpha);
+extern float Determinant(float x11, float x12, float x21, float x22);
+extern float absolute(float value);
+
+extern void Reset_data(void);
+
+#endif // SENSORFUSION_H_INCLUDED
\ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f SystemConstant.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SystemConstant.h	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,29 @@
+#ifndef SYSTEMCONST_H_INCLUDED
+#define SYSTEMCONST_H_INCLUDED
+
+#define pi      3.14f
+
+#define M       23.1f
+#define J       5.2067f
+#define g       9.81f
+#define Xt      0.3857f
+#define Zt      0.4338f
+#define L       1.053f
+#define lammda  20/180*pi
+
+#define R_wheel 0.0695f  ///radius of wheels
+#define l_rs    0.095f   ///length from rear wheel to sensor on XY plane
+#define l_rg    0.13f    ///length from rear wheel to center of mass on XY plane
+#define Vmin    0.5f
+#define Vmax    5.0f
+
+
+/*
+    steer      u2steer_lpf_freq*speed                1
+    ----- = ----------------------------- * --------------------
+      u      S + u2steer_lpf_freq*speed      u2steer_gain*speed
+*/
+#define u2steer_lpf_freq    2.52668f//2.52668f
+#define u2steer_gain        0.6251f
+
+#endif // ROBOTBICYCLECONST_H_INCLUDED
diff -r 000000000000 -r bf9bf4b7625f main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,403 @@
+#include "mbed.h"
+#include "math.h"
+#include "LSM9DS0.h"
+#include "Mx28.h"
+#include "Controller.h"
+#include "SensorFusion.h"
+#include "SystemConstant.h"
+
+// Dynamixel **************************************************************************************************************************************************
+#define Steering_SERVO_ID           3
+#define SERVO_ControlPin            A2       // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer.
+#define SERVO_SET_Baudrate          1000000  // Baud rate speed which the Dynamixel will be set too (1Mbps)
+#define TxPin                       A0
+#define RxPin                       A1
+#define CW_LIMIT_ANGLE              0x001    // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
+#define CCW_LIMIT_ANGLE             0xFFF    // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
+DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
+float deg_s = 0.0;
+float left_motor_angle = 0.0f;
+float right_motor_angle = 0.0f;
+
+// LSM9DS0 *****************************************************************************************************************************************************
+// sensor gain
+#define Gyro_gain_x   0.002422966362920f
+#define Gyro_gain_y   0.002422966362920f
+#define Gyro_gain_z   0.002422966362920f            // datasheet : 70 mdeg/digit
+#define Acce_gain_x   -0.002403878;                 // -9.81 / ( 3317.81 - (-764.05) )
+#define Acce_gain_y   -0.002375119;                 // -9.81 / ( 3476.34 - (-676.806))
+#define Acce_gain_z   -0.002454702;                 // -9.81 / ( 4423.63 - (285.406) )
+#define Magn_gain     0
+
+// raw data register
+int16_t Gyro_axis_data[3] = {0};     // X, Y, Z axis
+int16_t Acce_axis_data[3] = {0};     // X, Y, Z axis
+
+// sensor filter correction data
+float Gyro_axis_data_f[3];
+float Gyro_axis_data_f_old[3];
+float Gyro_scale[3];                          // scale = (data - zero) * gain
+float Gyro_axis_zero[3] = {51.38514174, 62.64907136, -20.82209189};//{52.6302,57.3614,-48.6806};
+//Gyro offset
+//**********************************************
+//41.5376344    -26.92864125    103.3949169
+//42.53079179   -27.71065494    97.0400782
+//43.54056696   -28.63734115    90.77419355
+//**********************************************
+
+
+float Acce_axis_data_f[3];
+float Acce_axis_data_f_old[3];
+float Acce_scale[3];                          // scale = (data - zero) * gain
+float Acce_axis_zero[3] = {-754.5894428, -677.0645161, 413.4472141};//{-818.8824, -714.3789, 326.2993};//{-721.0150,-748.3353,394.9194};
+//Acce offset
+//**********************************************
+//-618.8191593    -639.3255132    4676.384164
+//-604.7047898    3395.289345     375.0957967
+//4676.57478     -700.4506354   416.9599218
+//**********************************************
+LSM9DS0 sensor(SPI_MODE, PB_2, PB_1);    // PB_13, PB_14, PB_15
+
+// Useful constants  *******************************************************************************************************************************************
+#define T 0.001 //sampling time
+#define CNT2STR 1500 // 1500(ms) = 1.5(s)
+// mbed function ***********************************************************************************************************************************************
+Ticker timer1;
+Ticker timer2;
+
+Serial USB(USBTX, USBRX);
+Serial BT_Cmd(PC_12, PD_2);
+Serial BT_Data(PC_10, PC_11);
+Serial MCU(PB_6, PA_10);
+
+// Linear actuator *********************************************************************************************************************************************
+PwmOut pwm_l(D7);
+PwmOut pwmn_l(D11);
+
+PwmOut pwm_r(D8);
+PwmOut pwmn_r(A3);
+
+#define down_duty 1.0f
+#define up_duty   0.0f
+#define stop_duty 0.5f
+
+// ADC
+AnalogIn RightActuator(PC_0);
+AnalogIn LeftActuator(PC_1);
+
+// Functions ***************************************************************************************************************************************************
+void init_sensor(void);
+void read_sensor(void);
+void timer1_interrupt(void);
+void timer2_interrupt(void);
+void uart_send(void);
+void separate(void);
+
+// Variables ***************************************************************************************************************************************************
+int i = 0;
+int display[6] = {0,0,0,0,0,0};
+char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0};      // char num[0] , num[1] : 2 start byte
+char BTCmd = 0;
+char MCU_Data_get = 0;
+char MCU_Data_put = 0;
+float velocity = 0.0f;
+float velocity_old = 0.0f;
+float speed = 0.0f;
+float VelocityCmd = 0.0f;
+float VelocityCmdOld = 0.0f;
+float steer_out = 0.0f;
+float steer_out_old = 0.0f;
+bool start_control = 0;
+float roll_sum = 0.0f;
+float average = 0.0f;
+int temp = 0;
+// turning command
+int count2straight = 0;
+float roll_cmd = 0.0f;
+float roll_old = 0.0f;
+float roll = 0.0f;
+
+float steer_cmd = 0.0f;
+float steer_old = 0.0f;
+float steer = 0.0f;
+// test tool
+bool up = 0;
+bool down = 0;
+bool tim1 = 0;
+int counter = 0;
+
+// Code ********************************************************************************************************************************************************
+// main ********************************************************************************************************************************************************
+int main()
+{
+    dynamixelClass.setMode(Steering_SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE);    // set mode to SERVO and set angle limits
+    USB.baud(115200);
+    BT_Data.baud(57600);
+    BT_Cmd.baud(115200);
+    MCU.baud(115200);
+    
+    // actuator pwm initialize
+    pwm_l.period_us(50);    
+    pwm_r.period_us(50);    
+    // timer setting
+    timer1.attach_us(&timer1_interrupt, 1000);//4ms interrupt period
+    timer2.attach_us(&timer2_interrupt, 3000);//4.098ms interrupt period
+    
+    // initialize
+    sensor.begin();
+    start_control = 0;
+    steering_angle = 0;
+    
+    while(1) {
+        // command from tablet
+        if(BT_Cmd.readable()) {
+            BTCmd = BT_Cmd.getc();
+            switch(BTCmd) {
+                case 's':
+                    start_control = 1;
+                    roll_cmd = 2.0f/180.0f*pi;
+                    count2straight = 0;
+                    break;
+                case 'p':
+                    start_control = 0;
+                    steer_out = 0;
+                    steer_rad = 0;
+                    steer_rad_old = 0;
+                    steering_angle = 0.0f;
+                    u_1 = 0;
+                    u_2 = 0;
+                    u_3 = 0;
+                    u_d = 0;
+                    u = 0;
+                    alpha_1 = 0;
+                    alpha_2 = 0;
+                    roll_err = 0;
+                    VelocityCmd = 0;
+                    break;
+                case 'f':
+                    steer_out = 0;
+                    break;
+                case 'r':
+                    steer_out = steer_out + 2;
+//                    up = 1;
+//                    down = 0;
+                    break;
+                case 'l':
+                    steer_out = steer_out - 2;
+//                    up = 0;
+//                    down = 1;
+                    break;
+                case 'm': // command of turning right
+                    roll_cmd = 4.0f/180.0f*pi;
+                    count2straight = 0;
+                    break;
+                case 'n': // command of turning left
+                    roll_cmd = 0.0f/180.0f*pi;
+                    count2straight = 0;
+                    break;
+                case 'a':
+                    VelocityCmd = VelocityCmd + 0.0f;
+                    break;
+                case 'b':
+                    VelocityCmd = VelocityCmd - 0.0f;
+                    break;
+                case 'c':
+                    break;
+                case 'h':
+                    VelocityCmd = 0;
+                    steer_out = 0;
+                    break;
+                default:
+                    break;
+            }
+            BTCmd = 0;
+        }
+    }
+}// main
+
+// timer1_interrupt *********************************************************************************************************************************************
+void timer1_interrupt(void)
+{   
+    // MCU_UART ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    if(VelocityCmd != VelocityCmdOld) {
+        if(MCU.writeable()) {
+            MCU_Data_put = VelocityCmd / 0.01953125f;
+            MCU.putc(MCU_Data_put);
+        }
+        VelocityCmdOld = VelocityCmd;
+    }
+    // speed data from another MCU
+    if(MCU.readable())
+    {
+        MCU_Data_get = MCU.getc();
+        tim1=!tim1;
+    }
+    speed = (float)MCU_Data_get * 0.01953125f;
+    // velocity check for auxiliary wheels ///////////////////////////////////////////////////////////////////////////////////
+    if(speed < 0.3f){
+        counter = 0;
+        pwm_l.write(down_duty);
+        pwm_r.write(down_duty);
+    }
+    if(speed > 0.3f){
+        counter++;
+        pwm_l.write(up_duty);
+        pwm_r.write(up_duty);
+        if(counter >= 16000){
+            counter = 16000;
+            pwm_l.write(stop_duty);
+            pwm_r.write(stop_duty);
+        }
+    }
+   // if(up ==1){
+//        pwm_l.write(up_duty);
+//        pwm_r.write(up_duty);
+//    }
+//    if(down ==1){
+//        pwm_l.write(down_duty);
+//        pwm_r.write(down_duty);
+//    }
+    
+    TIM1->CCER |= 0x4;
+    TIM1->CCER |= 0x40;
+    velocity = 1;
+    // IMU Read ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    read_sensor();
+    Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
+    Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
+    Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
+    Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
+    Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
+    Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
+
+    Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
+    Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
+    Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
+    Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
+    Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
+    Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
+
+    Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
+    Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
+    Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
+
+    Gyro_scale[0] = ((Gyro_axis_data_f[0]-Gyro_axis_zero[0]))*Gyro_gain_x;
+    Gyro_scale[1] = ((Gyro_axis_data_f[1]-Gyro_axis_zero[1]))*Gyro_gain_y;
+    Gyro_scale[2] = ((Gyro_axis_data_f[2]-Gyro_axis_zero[2]))*Gyro_gain_z;
+
+    //Centrifugal Acceleration Compensation ////////////////////////////////////////////////////////////////////////////////////
+    /*
+                                Acx = Ac*sin(e) = YawRate*YawRate*l_rs
+                                Acx = Ac*sin(e)*cos(roll_angle) = YawRate*speed*cos(roll_angle)
+                                Acx = Ac*sin(e)*sin(roll_angle) = YawRate*speed*sin(roll_angle)
+    */
+    Ac[0] = l_rs * Gyro_scale[2] * Gyro_scale[2];
+    Ac[1] = (-1.0) * velocity * Gyro_scale[2] * cos(roll_angle);
+    Ac[2] = (-1.0) * velocity * Gyro_scale[2] * sin(roll_angle);
+    Acce_scale[0] = Acce_scale[0] - Ac[0];
+    Acce_scale[1] = Acce_scale[1] - Ac[1];
+    Acce_scale[2] = Acce_scale[2] - Ac[2];
+    // do sensor fusion /////////////////////////////////////////////////////////////////////////////////////////////////////////
+    roll_fusion(Acce_scale[0],Acce_scale[1],Acce_scale[2],Gyro_scale[2],Gyro_scale[0],5);// alpha = 3 represents the best behavior for robot bike
+    droll_angle = lpf(Gyro_scale[0], droll_angle_old, 62.8);  // 62.8 = pi * 20
+    droll_angle_old = droll_angle;
+    droll_angle = droll_angle + 0.5f/180.0f*pi;
+    // bias cancelation
+//    roll_angle = roll_angle - 2.2f/180.0f*pi;
+    // roll angle of turning command ////////////////////////////////////////////////////////////////////////////////////////////
+    roll_ref = lpf(roll_cmd, roll_old, 3.1415f);
+    roll_old = roll_ref;
+    // steering angle of turning command ////////////////////////////////////////////////////////////////////////////////////////
+    /*
+                                                        -L*g
+                                        steer_ss = ---------------  * roll_ss
+                                                     V*V*cos(lammda)
+    */
+    steer_ref = -L*g/velocity/velocity/cos(lammda)*roll_ref;
+    // controller ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    if(start_control == 0) {
+        deg_s = (180.0f + steer_out)*4096.0f/360.0f;
+    }
+    if(start_control == 1) {
+        
+        // lpf for velocity, cut-off freq. = 3Hz = 18.85 (rad/s)
+        velocity = lpf(velocity, velocity_old, 18.85);
+        velocity_old = velocity;
+        
+        controller(velocity);
+        steer_angle(u, velocity);
+
+        //Steer output
+        deg_s = (180.0f - steering_angle)*4096.0f/360.0f;
+        // count to turn stright
+        if(count2straight < CNT2STR)  count2straight++;
+        if(count2straight == CNT2STR) roll_cmd = 2.0f/180.0f*pi;
+    }
+    // send data by UART //////////////////////////////////////////////////////////////////////////////////////////////////////////
+    uart_send();
+}// timer1_interrupt
+
+// timer1_interrupt *********************************************************************************************************************************************
+void timer2_interrupt(void)
+{
+ dynamixelClass.servo(Steering_SERVO_ID,deg_s,1000);
+}// timer2_interrupt
+
+// read_sensor **************************************************************************************************************************************************
+void read_sensor(void)
+{
+    // sensor raw data
+    Gyro_axis_data[0] = sensor.readRawGyroX();
+    Gyro_axis_data[1] = sensor.readRawGyroY();
+    Gyro_axis_data[2] = sensor.readRawGyroZ();
+
+    Acce_axis_data[0] = sensor.readRawAccelX();
+    Acce_axis_data[1] = sensor.readRawAccelY();
+    Acce_axis_data[2] = sensor.readRawAccelZ();
+}// read_sensor
+
+// uart_send ****************************************************************************************************************************************************
+void uart_send(void)
+{
+    // uart send data
+    display[0] = roll_angle/pi*180.0f*100.0f;
+    display[1] = droll_angle/pi*180.0f*100.0f;
+    display[2] = speed*100;
+    display[3] = steering_angle*100;
+    display[4] = u_1*100;
+    display[5] = u_2*100;
+
+    separate();
+
+    int j = 0;
+    int k = 1;
+    while(k)    {
+        if(BT_Data.writeable()) {
+            BT_Data.putc(num[i]);
+            i++;
+            j++;
+        }
+        if(j>1) {// send 2 bytes
+            k = 0;
+            j = 0;
+        }
+    }
+    if(i>13) i = 0;
+}// uart_send
+
+//separate  ****************************************************************************************************************************************************
+void separate(void)
+{
+    num[2] = display[0] >> 8;           // HSB(8bit)資料存入陣列
+    num[3] = display[0] & 0x00ff;       // LSB(8bit)資料存入陣列
+    num[4] = display[1] >> 8;
+    num[5] = display[1] & 0x00ff;
+    num[6] = display[2] >> 8;
+    num[7] = display[2] & 0x00ff;
+    num[8] = display[3] >> 8;
+    num[9] = display[3] & 0x00ff;
+    num[10] = display[4] >> 8;
+    num[11] = display[4] & 0x00ff;
+    num[12] = display[5] >> 8;
+    num[13] = display[5] & 0x00ff;
+}//separate
\ No newline at end of file
diff -r 000000000000 -r bf9bf4b7625f mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jun 08 14:11:49 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/e7ca05fa8600
\ No newline at end of file