text: /*
   Dynamixel.cpp - Ax-12+ Half Duplex USART Comunication
   Copyright (c) 2011 Savage Electronics.
   Created by Savage on 27/01/11.
   
   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,  
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
   
   *****************************************************************************
   Modifications:
   
   25/07/2011 - Eliminado la modificacion serial para ser modificada dentro del mismo Hardware Serial.
   25/07/2011 - Modificado la funcion setBD() para aceptar todas la velocidades sin PDF.
   25/07/2011 - Agregada la funcion de Rotacion Continua.
   26/07/2011 - Agregada la funcion begin sin seteo de Direction_Pin.
   25/07/2011 - Agregada la funcion Reset.
   26/07/2011 - Agregada la funcion Reg_Write en move y moveSpeed.
   26/07/2011 - Agregada la funcion Action.
   13/12/2011 - Arreglado el manejo y envio de variables.
   22/12/2011 - Compatible con la actualizacion Arduino 1.0.
   10/01/2012 - Utilizacion de Macros y eliminacion codigo no necesario.
   11/01/2012 - Agregadas las funciones:
                int setTempLimit(unsigned char ID, unsigned char Temperature);
                int setAngleLimit(unsigned char ID, int CWLimit, int CCWLimit);
                int setVoltageLimit(unsigned char ID, unsigned char DVoltage, unsigned char UVoltage);
  			  int setMaxTorque(unsigned char ID, int MaxTorque);
                int setSRL(unsigned char ID, unsigned char SRL);
                int setRDT(unsigned char ID, unsigned char RDT);
                int setLEDAlarm(unsigned char ID, unsigned char LEDAlarm);
                int setShutdownAlarm(unsigned char ID, unsigned char SALARM);
                int setCMargin(unsigned char ID, unsigned char CWCMargin, unsigned char CCWCMargin);
  			  int setCSlope(unsigned char ID, unsigned char CWCSlope, unsigned char CCWCSlope);
   15/01/2012 - Agregadas las funciones:             
                int setPunch(unsigned char ID, int Punch);
                int moving(unsigned char ID);
                int lockRegister(unsigned char ID);
  			  int RWStatus(unsigned char ID);
                int readSpeed(unsigned char ID);
                int readLoad(unsigned char ID);
   
   Bug Reports:
   
   13/06/2013 - CSlope Function "Bad command Error", by filip aka.
   
   
   TODO:
   
   FUNCION SYNCWRITE.
   
   *****************************************************************************
   
   Contact: savageelectronics@gmail.com 
   Web:     http://savageelectrtonics.blogspot.com/
   Autor:   Josue Alejandro Savage
   
   */
  
  #if defined(ARDUINO) && ARDUINO >= 100  // Arduino IDE Version
  #include "Arduino.h"
  #else
  #include "WProgram.h"
  #endif
  
  #include "DynamixelSerial.h"
  
  // Macro for the selection of the Serial Port
  
  #define sendData(args)  (Serial.write(args))    // Write Over Serial
  #define availableData() (Serial.available())    // Check Serial Data Available
  #define readData()      (Serial.read())         // Read Serial Data
  #define peekData()      (Serial.peek())         // Peek Serial Data
  #define beginCom(args)  (Serial.begin(args))    // Begin Serial Comunication
  #define endCom()        (Serial.end())          // End Serial Comunication
  
  // Macro for Timing
  
  #define delayus(args) (delayMicroseconds(args))  // Delay Microseconds
  
  // Macro for Comunication Flow Control
  
  #define setDPin(DirPin,Mode)   (pinMode(DirPin,Mode))       // Select the Switch to TX/RX Mode Pin
  #define switchCom(DirPin,Mode) (digitalWrite(DirPin,Mode))  // Switch to TX/RX Mode
  
  
  // Private Methods //////////////////////////////////////////////////////////////
  
  int DynamixelClass::read_error(void)
  {
  	Time_Counter = 0;
  	while((availableData() < 5) & (Time_Counter < TIME_OUT)){  // Wait for Data
  		Time_Counter++;
  		delayus(1000);
  	}
  	
  	while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                                    // Start Bytes
  			readData();                                    // Ax-12 ID
  			readData();                                    // Length
  			Error_Byte = readData();                       // Error
  				return (Error_Byte);
  		}
  	}
  	return (-1);											 // No Ax Response
  }
  
  // Public Methods //////////////////////////////////////////////////////////////
  
  void DynamixelClass::begin(long baud, unsigned char directionPin)
  {	
  	Direction_Pin = directionPin;
  	setDPin(Direction_Pin,OUTPUT);
  	beginCom(baud);
  }	
  
  void DynamixelClass::begin(long baud)
  {	
  	beginCom(baud);
  }	
  
  void DynamixelClass::end()
  {
  	endCom();
  }
  
  int DynamixelClass::reset(unsigned char ID)
  {
  	Checksum = (~(ID + AX_RESET_LENGTH + AX_RESET))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
  	sendData(AX_START);                     
  	sendData(AX_START);
  	sendData(ID);
  	sendData(AX_RESET_LENGTH);
  	sendData(AX_RESET);    
  	sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  
      return (read_error());  
  }
  
  int DynamixelClass::ping(unsigned char ID)
  {
  	Checksum = (~(ID + AX_READ_DATA + AX_PING))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
  	sendData(AX_START);                     
  	sendData(AX_START);
  	sendData(ID);
  	sendData(AX_READ_DATA);
  	sendData(AX_PING);    
  	sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());              
  }
  
  int DynamixelClass::setID(unsigned char ID, unsigned char newID)
  {    
  	Checksum = (~(ID + AX_ID_LENGTH + AX_WRITE_DATA + AX_ID + newID))&0xFF;
  
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
  	sendData(AX_ID_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_ID);
      sendData(newID);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());                // Return the read error
  }
  
  int DynamixelClass::setBD(unsigned char ID, long baud)
  {    
  	unsigned char Baud_Rate = (2000000/baud) - 1;
      Checksum = (~(ID + AX_BD_LENGTH + AX_WRITE_DATA + AX_BAUD_RATE + Baud_Rate))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                 // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
  	sendData(AX_BD_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_BAUD_RATE);
      sendData(Baud_Rate);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());                // Return the read error
  }
  
  int DynamixelClass::move(unsigned char ID, int Position)
  {
      char Position_H,Position_L;
      Position_H = Position >> 8;           // 16 bits - 2 x 8 bits variables
      Position_L = Position;
  	Checksum = (~(ID + AX_GOAL_LENGTH + AX_WRITE_DATA + AX_GOAL_POSITION_L + Position_L + Position_H))&0xFF;
      
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                 // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_GOAL_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_GOAL_POSITION_L);
      sendData(Position_L);
      sendData(Position_H);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  
      return (read_error());                 // Return the read error
  }
  
  int DynamixelClass::moveSpeed(unsigned char ID, int Position, int Speed)
  {
      char Position_H,Position_L,Speed_H,Speed_L;
      Position_H = Position >> 8;    
      Position_L = Position;                // 16 bits - 2 x 8 bits variables
      Speed_H = Speed >> 8;
      Speed_L = Speed;                      // 16 bits - 2 x 8 bits variables
  	Checksum = (~(ID + AX_GOAL_SP_LENGTH + AX_WRITE_DATA + AX_GOAL_POSITION_L + Position_L + Position_H + Speed_L + Speed_H))&0xFF;
   
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_GOAL_SP_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_GOAL_POSITION_L);
      sendData(Position_L);
      sendData(Position_H);
      sendData(Speed_L);
      sendData(Speed_H);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());               // Return the read error
  }
  
  int DynamixelClass::setEndless(unsigned char ID, bool Status)
  {
   if ( Status ) {	
  	  char AX_CCW_AL_LT = 0;     // Changing the CCW Angle Limits for Full Rotation.
  	  Checksum = (~(ID + AX_GOAL_LENGTH + AX_WRITE_DATA + AX_CCW_ANGLE_LIMIT_L))&0xFF;
  	
  	  switchCom(Direction_Pin,Tx_MODE);
        sendData(AX_START);                // Send Instructions over Serial
        sendData(AX_START);
        sendData(ID);
        sendData(AX_GOAL_LENGTH);
        sendData(AX_WRITE_DATA);
        sendData(AX_CCW_ANGLE_LIMIT_L );
        sendData(AX_CCW_AL_LT);
        sendData(AX_CCW_AL_LT);
        sendData(Checksum);
        delayus(TX_DELAY_TIME);
  	  switchCom(Direction_Pin,Rx_MODE);
  
  	  return(read_error());
   }
   else
   {
  	 turn(ID,0,0);
  	 Checksum = (~(ID + AX_GOAL_LENGTH + AX_WRITE_DATA + AX_CCW_ANGLE_LIMIT_L + AX_CCW_AL_L + AX_CCW_AL_H))&0xFF;
  	
  	 switchCom(Direction_Pin,Tx_MODE);
  	 sendData(AX_START);                 // Send Instructions over Serial
  	 sendData(AX_START);
  	 sendData(ID);
  	 sendData(AX_GOAL_LENGTH);
  	 sendData(AX_WRITE_DATA);
  	 sendData(AX_CCW_ANGLE_LIMIT_L);
  	 sendData(AX_CCW_AL_L);
  	 sendData(AX_CCW_AL_H);
  	 sendData(Checksum);
  	 delayus(TX_DELAY_TIME);
  	 switchCom(Direction_Pin,Rx_MODE);
  	 
  	 return (read_error());                 // Return the read error
    }
   } 
  
  int DynamixelClass::turn(unsigned char ID, bool SIDE, int Speed)
  {		
  		if (SIDE == 0){                          // Move Left///////////////////////////
  			
  			char Speed_H,Speed_L;
  			Speed_H = Speed >> 8;
  			Speed_L = Speed;                     // 16 bits - 2 x 8 bits variables
  			Checksum = (~(ID + AX_SPEED_LENGTH + AX_WRITE_DATA + AX_GOAL_SPEED_L + Speed_L + Speed_H))&0xFF;
  			
  			switchCom(Direction_Pin,Tx_MODE);
  			sendData(AX_START);                // Send Instructions over Serial
  			sendData(AX_START);
  			sendData(ID);
  			sendData(AX_SPEED_LENGTH);
  			sendData(AX_WRITE_DATA);
  			sendData(AX_GOAL_SPEED_L);
  			sendData(Speed_L);
  			sendData(Speed_H);
  			sendData(Checksum);
  			delayus(TX_DELAY_TIME);
  			switchCom(Direction_Pin,Rx_MODE);
  			
  			return(read_error());               // Return the read error		
  		}
  		else
  		{                                            // Move Rigth////////////////////
  			char Speed_H,Speed_L;
  			Speed_H = (Speed >> 8) + 4;
  			Speed_L = Speed;                     // 16 bits - 2 x 8 bits variables
  			Checksum = (~(ID + AX_SPEED_LENGTH + AX_WRITE_DATA + AX_GOAL_SPEED_L + Speed_L + Speed_H))&0xFF;
  			
  			switchCom(Direction_Pin,Tx_MODE);
  			sendData(AX_START);                // Send Instructions over Serial
  			sendData(AX_START);
  			sendData(ID);
  			sendData(AX_SPEED_LENGTH);
  			sendData(AX_WRITE_DATA);
  			sendData(AX_GOAL_SPEED_L);
  			sendData(Speed_L);
  			sendData(Speed_H);
  			sendData(Checksum);
  			delayus(TX_DELAY_TIME);
  			switchCom(Direction_Pin,Rx_MODE);
  			
  			return(read_error());                // Return the read error	
  		}
  }
  
  int DynamixelClass::moveRW(unsigned char ID, int Position)
  {
      char Position_H,Position_L;
      Position_H = Position >> 8;           // 16 bits - 2 x 8 bits variables
      Position_L = Position;
      Checksum = (~(ID + AX_GOAL_LENGTH + AX_REG_WRITE + AX_GOAL_POSITION_L + Position_L + Position_H))&0xFF;
  
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                 // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_GOAL_LENGTH);
      sendData(AX_REG_WRITE);
      sendData(AX_GOAL_POSITION_L);
      sendData(Position_L);
      sendData(Position_H);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error());                 // Return the read error
  }
  
  int DynamixelClass::moveSpeedRW(unsigned char ID, int Position, int Speed)
  {
      char Position_H,Position_L,Speed_H,Speed_L;
      Position_H = Position >> 8;    
      Position_L = Position;                // 16 bits - 2 x 8 bits variables
      Speed_H = Speed >> 8;
      Speed_L = Speed;                      // 16 bits - 2 x 8 bits variables
      Checksum = (~(ID + AX_GOAL_SP_LENGTH + AX_REG_WRITE + AX_GOAL_POSITION_L + Position_L + Position_H + Speed_L + Speed_H))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_GOAL_SP_LENGTH);
      sendData(AX_REG_WRITE);
      sendData(AX_GOAL_POSITION_L);
      sendData(Position_L);
      sendData(Position_H);
      sendData(Speed_L);
      sendData(Speed_H);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());               // Return the read error
  }
  
  void DynamixelClass::action()
  {	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(BROADCAST_ID);
      sendData(AX_ACTION_LENGTH);
      sendData(AX_ACTION);
      sendData(AX_ACTION_CHECKSUM);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  }
  
  int DynamixelClass::torqueStatus( unsigned char ID, bool Status)
  {
      Checksum = (~(ID + AX_TORQUE_LENGTH + AX_WRITE_DATA + AX_TORQUE_ENABLE + Status))&0xFF;
  
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);              // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_TORQUE_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_TORQUE_ENABLE);
      sendData(Status);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());              // Return the read error
  }
  
  int DynamixelClass::ledStatus(unsigned char ID, bool Status)
  {    
      Checksum = (~(ID + AX_LED_LENGTH + AX_WRITE_DATA + AX_LED + Status))&0xFF;
  
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);              // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_LED_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_LED);
      sendData(Status);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());              // Return the read error
  }
  
  int DynamixelClass::readTemperature(unsigned char ID)
  {	
      Checksum = (~(ID + AX_TEM_LENGTH  + AX_READ_DATA + AX_PRESENT_TEMPERATURE + AX_BYTE_READ))&0xFF;
      
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);
      sendData(AX_START);
      sendData(ID);
      sendData(AX_TEM_LENGTH);
      sendData(AX_READ_DATA);
      sendData(AX_PRESENT_TEMPERATURE);
      sendData(AX_BYTE_READ);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      Temperature_Byte = -1;
      Time_Counter = 0;
      while((availableData() < 6) & (Time_Counter < TIME_OUT)){
  		Time_Counter++;
  		delayus(1000);
      }
  	
      while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                            // Start Bytes
  			readData();                            // Ax-12 ID
  			readData();                            // Length
  			if( (Error_Byte = readData()) != 0 )   // Error
  				return (Error_Byte*(-1));
  			Temperature_Byte = readData();         // Temperature
  		}
      }
  	return (Temperature_Byte);               // Returns the read temperature
  }
  
  int DynamixelClass::readPosition(unsigned char ID)
  {	
      Checksum = (~(ID + AX_POS_LENGTH  + AX_READ_DATA + AX_PRESENT_POSITION_L + AX_BYTE_READ_POS))&0xFF;
    
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);
      sendData(AX_START);
      sendData(ID);
      sendData(AX_POS_LENGTH);
      sendData(AX_READ_DATA);
      sendData(AX_PRESENT_POSITION_L);
      sendData(AX_BYTE_READ_POS);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      Position_Long_Byte = -1;
  	Time_Counter = 0;
      while((availableData() < 7) & (Time_Counter < TIME_OUT)){
  		Time_Counter++;
  		delayus(1000);
      }
  	
      while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                            // Start Bytes
  			readData();                            // Ax-12 ID
  			readData();                            // Length
  			if( (Error_Byte = readData()) != 0 )   // Error
  				return (Error_Byte*(-1));
      
  			Position_Low_Byte = readData();            // Position Bytes
  			Position_High_Byte = readData();
  			Position_Long_Byte = Position_High_Byte << 8; 
  			Position_Long_Byte = Position_Long_Byte + Position_Low_Byte;
  		}
      }
  	return (Position_Long_Byte);     // Returns the read position
  }
  
  int DynamixelClass::readVoltage(unsigned char ID)
  {    
      Checksum = (~(ID + AX_VOLT_LENGTH  + AX_READ_DATA + AX_PRESENT_VOLTAGE + AX_BYTE_READ))&0xFF;
      
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);
      sendData(AX_START);
      sendData(ID);
      sendData(AX_VOLT_LENGTH);
      sendData(AX_READ_DATA);
      sendData(AX_PRESENT_VOLTAGE);
      sendData(AX_BYTE_READ);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      Voltage_Byte = -1;
  	Time_Counter = 0;
      while((availableData() < 6) & (Time_Counter < TIME_OUT)){
  		Time_Counter++;
  		delayus(1000);
      }
  	
      while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                            // Start Bytes
  			readData();                            // Ax-12 ID
  			readData();                            // Length
  			if( (Error_Byte = readData()) != 0 )   // Error
  				return (Error_Byte*(-1));
  			Voltage_Byte = readData();             // Voltage
  		}
      }
  	return (Voltage_Byte);               // Returns the read Voltage
  }
  
  int DynamixelClass::setTempLimit(unsigned char ID, unsigned char Temperature)
  {
  	Checksum = (~(ID + AX_TL_LENGTH +AX_WRITE_DATA+ AX_LIMIT_TEMPERATURE + Temperature))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
  	sendData(AX_START);                     
  	sendData(AX_START);
  	sendData(ID);
  	sendData(AX_TL_LENGTH);
  	sendData(AX_WRITE_DATA);
  	sendData(AX_LIMIT_TEMPERATURE);
      sendData(Temperature);
  	sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error()); 
  }
  
  int DynamixelClass::setVoltageLimit(unsigned char ID, unsigned char DVoltage, unsigned char UVoltage)
  {
  	Checksum = (~(ID + AX_VL_LENGTH +AX_WRITE_DATA+ AX_DOWN_LIMIT_VOLTAGE + DVoltage + UVoltage))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
  	sendData(AX_START);                     
  	sendData(AX_START);
  	sendData(ID);
  	sendData(AX_VL_LENGTH);
  	sendData(AX_WRITE_DATA);
  	sendData(AX_DOWN_LIMIT_VOLTAGE);
      sendData(DVoltage);
      sendData(UVoltage);
  	sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error()); 
  }
  
  int DynamixelClass::setAngleLimit(unsigned char ID, int CWLimit, int CCWLimit)
  {
  	char CW_H,CW_L,CCW_H,CCW_L;
      CW_H = CWLimit >> 8;    
      CW_L = CWLimit;                // 16 bits - 2 x 8 bits variables
      CCW_H = CCWLimit >> 8;
      CCW_L = CCWLimit;  
  	Checksum = (~(ID + AX_VL_LENGTH +AX_WRITE_DATA+ AX_CW_ANGLE_LIMIT_L + CW_H + CW_L + AX_CCW_ANGLE_LIMIT_L + CCW_H + CCW_L))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
  	sendData(AX_START);                     
  	sendData(AX_START);
  	sendData(ID);
  	sendData(AX_CCW_CW_LENGTH);
  	sendData(AX_WRITE_DATA);
  	sendData(AX_CW_ANGLE_LIMIT_L);
      sendData(CW_L);
  	sendData(CW_H);
  	sendData(AX_CCW_ANGLE_LIMIT_L);
      sendData(CCW_L);
  	sendData(CCW_H);
  	sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error()); 
  }
  
  int DynamixelClass::setMaxTorque(unsigned char ID, int MaxTorque)
  {
      char MaxTorque_H,MaxTorque_L;
      MaxTorque_H = MaxTorque >> 8;           // 16 bits - 2 x 8 bits variables
      MaxTorque_L = MaxTorque;
  	Checksum = (~(ID + AX_MT_LENGTH + AX_WRITE_DATA + AX_MAX_TORQUE_L + MaxTorque_L + MaxTorque_H))&0xFF;
      
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                 // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_MT_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_MAX_TORQUE_L);
      sendData(MaxTorque_L);
      sendData(MaxTorque_H);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error());                 // Return the read error
  }
  
  int DynamixelClass::setSRL(unsigned char ID, unsigned char SRL)
  {    
  	Checksum = (~(ID + AX_SRL_LENGTH + AX_WRITE_DATA + AX_RETURN_LEVEL + SRL))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
  	sendData(AX_SRL_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_RETURN_LEVEL);
      sendData(SRL);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());                // Return the read error
  }
  
  int DynamixelClass::setRDT(unsigned char ID, unsigned char RDT)
  {    
  	Checksum = (~(ID + AX_RDT_LENGTH + AX_WRITE_DATA + AX_RETURN_DELAY_TIME + (RDT/2)))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
  	sendData(AX_RDT_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_RETURN_DELAY_TIME);
      sendData((RDT/2));
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());                // Return the read error
  }
  
  int DynamixelClass::setLEDAlarm(unsigned char ID, unsigned char LEDAlarm)
  {    
  	Checksum = (~(ID + AX_LEDALARM_LENGTH + AX_WRITE_DATA + AX_ALARM_LED + LEDAlarm))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
  	sendData(AX_LEDALARM_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_ALARM_LED);
      sendData(LEDAlarm);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());                // Return the read error
  }
  
  int DynamixelClass::setShutdownAlarm(unsigned char ID, unsigned char SALARM)
  {    
  	Checksum = (~(ID + AX_SALARM_LENGTH + AX_ALARM_SHUTDOWN + AX_ALARM_LED + SALARM))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
  	sendData(AX_SALARM_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_ALARM_SHUTDOWN);
      sendData(SALARM);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());                // Return the read error
  }
  
  int DynamixelClass::setCMargin(unsigned char ID, unsigned char CWCMargin, unsigned char CCWCMargin)
  {
  	Checksum = (~(ID + AX_CM_LENGTH +AX_WRITE_DATA+ AX_CW_COMPLIANCE_MARGIN + CWCMargin + AX_CCW_COMPLIANCE_MARGIN + CCWCMargin))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
  	sendData(AX_START);                     
  	sendData(AX_START);
  	sendData(ID);
  	sendData(AX_CM_LENGTH);
  	sendData(AX_WRITE_DATA);
  	sendData(AX_CW_COMPLIANCE_MARGIN);
      sendData(CWCMargin);
  	sendData(AX_CCW_COMPLIANCE_MARGIN);
      sendData(CCWCMargin);
  	sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error()); 
  }
  
  int DynamixelClass::setCSlope(unsigned char ID, unsigned char CWCSlope, unsigned char CCWCSlope)
  {
  	Checksum = (~(ID + AX_CS_LENGTH +AX_WRITE_DATA+ AX_CW_COMPLIANCE_SLOPE + CWCSlope + CCWCSlope))&0xFF;  // AX_CCW_COMPLIANCE_SLOPE +
  	
  	switchCom(Direction_Pin,Tx_MODE);
  	sendData(AX_START);                     
  	sendData(AX_START);
  	sendData(ID);
  	sendData(AX_CS_LENGTH);
  	sendData(AX_WRITE_DATA);
  	sendData(AX_CW_COMPLIANCE_SLOPE);
      sendData(CWCSlope);
  	// sendData(AX_CCW_COMPLIANCE_SLOPE);
      sendData(CCWCSlope);
  	sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error()); 
  }
  
  int DynamixelClass::setPunch(unsigned char ID, int Punch)
  {
      char Punch_H,Punch_L;
      Punch_H = Punch >> 8;           // 16 bits - 2 x 8 bits variables
      Punch_L = Punch;
  	Checksum = (~(ID + AX_PUNCH_LENGTH + AX_WRITE_DATA + AX_PUNCH_L + Punch_L + Punch_H))&0xFF;
      
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                 // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
      sendData(AX_PUNCH_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_PUNCH_L);
      sendData(Punch_L);
      sendData(Punch_H);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      return (read_error());                 // Return the read error
  }
  
  int DynamixelClass::moving(unsigned char ID)
  {	
      Checksum = (~(ID + AX_MOVING_LENGTH  + AX_READ_DATA + AX_MOVING + AX_BYTE_READ))&0xFF;
      
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);
      sendData(AX_START);
      sendData(ID);
      sendData(AX_MOVING_LENGTH);
      sendData(AX_READ_DATA);
      sendData(AX_MOVING);
      sendData(AX_BYTE_READ);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      Moving_Byte = -1;
      Time_Counter = 0;
      while((availableData() < 6) & (Time_Counter < TIME_OUT)){
  		Time_Counter++;
  		delayus(1000);
      }
  	
      while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                            // Start Bytes
  			readData();                            // Ax-12 ID
  			readData();                            // Length
  			if( (Error_Byte = readData()) != 0 )   // Error
  				return (Error_Byte*(-1));
  			Moving_Byte = readData();         // Temperature
  		}
      }
  	return (Moving_Byte);               // Returns the read temperature
  }
  
  int DynamixelClass::lockRegister(unsigned char ID)
  {    
  	Checksum = (~(ID + AX_LR_LENGTH + AX_WRITE_DATA + AX_LOCK + LOCK))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);                // Send Instructions over Serial
      sendData(AX_START);
      sendData(ID);
  	sendData(AX_LR_LENGTH);
      sendData(AX_WRITE_DATA);
      sendData(AX_LOCK);
      sendData(LOCK);
      sendData(Checksum);
  	delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
      
      return (read_error());                // Return the read error
  }
  
  int DynamixelClass::RWStatus(unsigned char ID)
  {	
      Checksum = (~(ID + AX_RWS_LENGTH  + AX_READ_DATA + AX_REGISTERED_INSTRUCTION + AX_BYTE_READ))&0xFF;
      
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);
      sendData(AX_START);
      sendData(ID);
      sendData(AX_RWS_LENGTH);
      sendData(AX_READ_DATA);
      sendData(AX_REGISTERED_INSTRUCTION);
      sendData(AX_BYTE_READ);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      RWS_Byte = -1;
      Time_Counter = 0;
      while((availableData() < 6) & (Time_Counter < TIME_OUT)){
  		Time_Counter++;
  		delayus(1000);
      }
  	
      while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                            // Start Bytes
  			readData();                            // Ax-12 ID
  			readData();                            // Length
  			if( (Error_Byte = readData()) != 0 )   // Error
  				return (Error_Byte*(-1));
  			RWS_Byte = readData();         // Temperature
  		}
      }
  	return (RWS_Byte);               // Returns the read temperature
  }
  
  int DynamixelClass::readSpeed(unsigned char ID)
  {	
      Checksum = (~(ID + AX_POS_LENGTH  + AX_READ_DATA + AX_PRESENT_SPEED_L + AX_BYTE_READ_POS))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);
      sendData(AX_START);
      sendData(ID);
      sendData(AX_POS_LENGTH);
      sendData(AX_READ_DATA);
      sendData(AX_PRESENT_SPEED_L);
      sendData(AX_BYTE_READ_POS);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      Speed_Long_Byte = -1;
  	Time_Counter = 0;
      while((availableData() < 7) & (Time_Counter < TIME_OUT)){
  		Time_Counter++;
  		delayus(1000);
      }
  	
      while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                            // Start Bytes
  			readData();                            // Ax-12 ID
  			readData();                            // Length
  			if( (Error_Byte = readData()) != 0 )   // Error
  				return (Error_Byte*(-1));
  			
  			Speed_Low_Byte = readData();            // Position Bytes
  			Speed_High_Byte = readData();
  			Speed_Long_Byte = Speed_High_Byte << 8; 
  			Speed_Long_Byte = Speed_Long_Byte + Speed_Low_Byte;
  		}
      }
  	return (Speed_Long_Byte);     // Returns the read position
  }
  
  int DynamixelClass::readLoad(unsigned char ID)
  {	
      Checksum = (~(ID + AX_POS_LENGTH  + AX_READ_DATA + AX_PRESENT_LOAD_L + AX_BYTE_READ_POS))&0xFF;
  	
  	switchCom(Direction_Pin,Tx_MODE);
      sendData(AX_START);
      sendData(AX_START);
      sendData(ID);
      sendData(AX_POS_LENGTH);
      sendData(AX_READ_DATA);
      sendData(AX_PRESENT_LOAD_L);
      sendData(AX_BYTE_READ_POS);
      sendData(Checksum);
      delayus(TX_DELAY_TIME);
  	switchCom(Direction_Pin,Rx_MODE);
  	
      Load_Long_Byte = -1;
  	Time_Counter = 0;
      while((availableData() < 7) & (Time_Counter < TIME_OUT)){
  		Time_Counter++;
  		delayus(1000);
      }
  	
      while (availableData() > 0){
  		Incoming_Byte = readData();
  		if ( (Incoming_Byte == 255) & (peekData() == 255) ){
  			readData();                            // Start Bytes
  			readData();                            // Ax-12 ID
  			readData();                            // Length
  			if( (Error_Byte = readData()) != 0 )   // Error
  				return (Error_Byte*(-1));
  			
  			Load_Low_Byte = readData();            // Position Bytes
  			Load_High_Byte = readData();
  			Load_Long_Byte = Load_High_Byte << 8; 
  			Load_Long_Byte = Load_Long_Byte + Load_Low_Byte;
  		}
      }
  	return (Load_Long_Byte);     // Returns the read position
  }
  
  DynamixelClass Dynamixel;