/*
 *
 * $Id: ax12.cpp 1 2008-07-30 09:24:06Z mschneider $
 * $URL: svn://localhost/Bioloid/trunk/ax12.cpp $
 
 * \version   $Rev: 1 $
 * \author    Markus Schneider
 * \date      17.07.08
 *
 */

#include "ax12.h"

// Constuctor, needs ID of Dynamixel actuators and a serial connection
AX12::AX12(const byte id, Connection *conn)
  : Dynamixel(id,conn)
{
}

// ID. The unique ID number assigned to each Dynamixel actuators for identifying them. 
// Different IDs are required for each Dynamixel actuators that are on the same network.
int AX12::setID(const byte id){
  int status = writeByte(AX12_ID, id);
  if( status == NO_ERROR ){
    this->id = id;
  }
  return status;
}

// Model Number.   For AX-12, this value is 0X000C (12) 
int AX12::getModelNumber(word *buff) const {
  return readWord(AX12_MODEL_NUMBER_L, buff);
}

// Firmware Version
int AX12::getVersion(byte *buff) const {
  return readByte(AX12_VERSION, buff);
}

// Baud Rate. Determines the communication speed. The computation is done by the 
// following formula. Speed (BPS) = 2000000 / (Address4 + 1) 
int AX12::getBaudRate(byte *buff) const{
  return readByte(AX12_BAUD_RATE, buff);
}
int AX12::setBaudRate(const byte value) const{
  return writeByte(AX12_BAUD_RATE, value);
}  

// Return Delay Time. The time it takes for the Status Packet to return after the Instruction 
// Packet is sent. The delay time is given by 2uSec * Address5 value.
int AX12::getReturnDelayTime(byte *buff) const{
  return readByte(AX12_RETURN_DELAY_TIME, buff);
}
int AX12::setReturnDelayTime(const byte value) const{
  return writeByte(AX12_RETURN_DELAY_TIME, value);
} 

// Operating Angle Limit. Sets the Dynamixel actuator’s operating angle range. The Goal 
// Position needs to be within the range of: CW Angle Limit <= Goal Position <= CCW 
// Angle Limit. An Angle Limit Error will occur if the Goal Position is set outside this range 
// set by the operating angle limits. 
int AX12::getCWAngleLimit(word *buff) const{
  return readWord(AX12_CW_ANGLE_LIMIT_L, buff);
}
int AX12::getCCWAngleLimmit(word *buff) const{
  return readWord(AX12_CCW_ANGLE_LIMIT_L, buff);
}
int AX12::setCWAngleLimit(const word value) const{
  return writeWord(AX12_CW_ANGLE_LIMIT_L, value);
}
int AX12::setCCWAngleLimmit(const word value) const{
  return writeWord(AX12_CCW_ANGLE_LIMIT_L, value);
}  

// the Highest Limit Temperature. The upper limit of the Dynamixel actuator’s operating 
// temperature. If the internal temperature of the Dynamixel actuator gets higher than this 
// value, the Over Heating Error Bit (Bit 2 of the Status Packet) will return the value 1, and 
// an alarm will be set by Address 17, 18. The values are in Degrees Celsius. 
int AX12::getLimitTemperature(byte *buff) const{
  return readByte(AX12_LIMIT_TEMPERATURE, buff);
}
int AX12::setLimitTemperature(const byte value) const{
  return writeByte(AX12_LIMIT_TEMPERATURE, value);
}

// the Lowest (Highest) Limit Voltage. The upper and lower limits of the Dynamixel 
// actuator’s operating voltage. If the present voltage (Address 42) is out of the specified 
// range, a Voltage Range Error Bit  (Bit 0 of the Status Packet) will return the value 1, 
// and an alarm will be set by Address 17, 18. The values are 10 times the actual voltage 
// value. For example, if the Address 12 value is 80, then the lower voltage limit is set to 8V.     
int AX12::getMinLimitVoltage(byte *buff) const{
  return readByte(AX12_MIN_LIMIT_VOLTAGE, buff);
}
int AX12::getMaxLimitVoltage(byte *buff) const{
  return readByte(AX12_MAX_LIMIT_VOLTAGE, buff);
}
int AX12::setMinLimitVoltage(const byte value) const{
  return writeByte(AX12_MIN_LIMIT_VOLTAGE, value);
}
int AX12::setMaxLimitVoltage(const byte value) const{
  return writeByte(AX12_MAX_LIMIT_VOLTAGE, value);
}  

// Max Torque. The maximum torque output for the Dynamixel actuator. When this value 
// is set to 0, the Dynamixel actuator enters the Free Run mode. There are two locations 
// where this maximum torque limit is defined; in the EEPROM (Address 0X0E, 0x0F) and 
// in the RAM (Address 0x22, 0x23). When the power is turned on, the maximum torque 
// limit value defined in the EEPROM is copied to the location in the RAM. The torque of 
// the Dynamixel actuator is limited by the values located in the 
// RAM (Address 0x22, 0x23).   
int AX12::getMaxTorque(word *buff) const{
  return readWord(AX12_MAX_TORQUE_L, buff);
}
int AX12::setMaxTorque(const word value) const{
  return writeWord(AX12_MAX_TORQUE_L, value);
}

// Status Return Level. Determines whether the Dynamixel actuator will return a Status 
// Packet after receiving an Instruction Packet. 
// +-----------+-----------------------------------------+
// | Address16 | Returning the Status Packet             |
// |     0     | Do not respond to any instructions      |
// |     1     | Respond only to READ_DATA instructions  |
// |     2     | Respond to all instructions             |
// +-----------+-----------------------------------------+
// In the case of an instruction which uses the Broadcast ID (0XFE) the Status Packet will 
// not be returned regardless of the Address 0x10 value. 
int AX12::getReturnLevel(byte *buff) const{
  return readByte(AX12_RETURN_LEVEL, buff);
}
int AX12::setReturnLevel(const byte value) const{
  return writeByte(AX12_RETURN_LEVEL, value);
}

// Alarm LED. If the corresponding Bit is set to 1, the LED blinks when an Error occurs. 
// +-------+-----------------------------------------------------------------+
// | Bit   | Function                                                        |
// | Bit 7 | 0                                                               |
// | Bit 6 | If set to 1, the LED blinks when an Instruction Error occurs    |
// | Bit 5 | If set to 1, the LED blinks when an Overload Error occurs       |
// | Bit 4 | If set to 1, the LED blinks when a Checksum Error occurs        |
// | Bit 3 | If set to 1, the LED blinks when a Range Error occurs           |
// | Bit 2 | If set to 1, the LED blinks when an Overheating Error occurs    |
// | Bit 1 | If set to 1, the LED blinks when an Angle Limit Error occurs    |
// | Bit 0 | If set to 1, the LED blinks when an Input Voltage Error occurs  |
// +-------+-----------------------------------------------------------------+
// This function operates following the “OR” logical operation of all bits. For example, if the 
// value is set to 0X05, the LED will blink when an Input Voltage Error occurs or when an 
// Overheating Error occurs. Upon returning to a normal condition from an error state, the 
// LED stops blinking after 2 seconds.     
int AX12::getAlarmLed(byte *buff) const{
  return readByte(AX12_ALARM_LED, buff);
}
int AX12::setAlarmLed(const byte value) const{
  return writeByte(AX12_ALARM_LED, value);
}

// Alarm Shutdown. If the corresponding Bit is set to a 1, the Dynamixel actuator’s torque 
// will be turned off when an error occurs. 
// +-------+-----------------------------------------------------------------+
// | Bit   | Function                                                        |
// | Bit 7 | 0                                                               |
// | Bit 6 | If set to 1, torque off when an Instruction Error occurs        |
// | Bit 5 | If set to 1, torque off when an Overload Error occurs           |
// | Bit 4 | If set to 1, torque off when a Checksum Error occurs            |
// | Bit 3 | If set to 1, torque off when a Range Error occurs               |
// | Bit 2 | If set to 1, torque off when an Overheating Error occurs        |
// | Bit 1 | If set to 1, torque off when an Angle Limit Error occurs        |
// | Bit 0 | If set to 1, torque off when an Input Voltage Error occurs      |
// +-------+-----------------------------------------------------------------+
// This function operates following the “OR” logical operation of all bits. However, unlike 
// the Alarm LED, after returning to a normal condition, it maintains the torque off status. 
// To recover, the Torque Enable (Address0X18) needs to be reset to 1.   
int AX12::getAlarmShutdown(byte *buff) const{
  return readByte(AX12_ALARM_SHUTDOWN, buff);
}
int AX12::setAlarmShutdown(const byte value) const{
  return writeByte(AX12_ALARM_SHUTDOWN, value);
}

// Calibration. Data used for compensating for the differences between the 
// potentiometers used in the Dynamixel units. The user cannot change this data. 
// The following (from Address 0x18) is in the RAM area. 
int AX12::getDownCalibration(word *buff) const{
  return readWord(AX12_DOWN_CALIBRATION_L, buff);
}  
int AX12::getUpCalibration(word *buff) const{
  return readWord(AX12_UP_CALIBRATION_L, buff);
}

// Torque Enable. When the power is first turned on, the Dynamixel actuator enters the 
// Torque Free Run condition (zero torque). Setting the value in Address 0x18 to 1 enables 
// the torque. 
int AX12::getTorqueEnable(byte *buff) const{
  return readByte(AX12_TORQUE_ENABLE, buff);
}
int AX12::setTorqueEnable(const byte value) const{
  return writeByte(AX12_TORQUE_ENABLE, value);
}

// LED.  The LED turns on when set to 1 and turns off if set to 0. 
int AX12::getLed(byte *buff) const{
  return readByte(AX12_LED, buff);
}
int AX12::setLed(const byte value) const{
  return writeByte(AX12_LED, value);
}

// Compliance Margin and Slope. The compliance of the Dynamixel actuator is defined 
// by setting the compliance Margin and Slope. This feature can be utilized for absorbing 
// shocks at the output shaft.
int AX12::getCWComplianceMargin(byte *buff) const{
  return readByte(AX12_CW_COMPLIANCE_MARGIN, buff);
}
int AX12::getCCWComplicanceMargin(byte *buff) const{
  return readByte(AX12_CCW_COMPLIANCE_MARGIN, buff);
}
int AX12::getCWComplianceSlope(byte *buff) const{
  return readByte(AX12_CCW_COMPLIANCE_SLOPE, buff);
}
int AX12::getCCWComplicanceSlope(byte *buff) const{
  return readByte(AX12_CCW_COMPLIANCE_SLOPE, buff);
} 
int AX12::setCWComplianceMargin(const byte value) const{
  return writeByte(AX12_CW_COMPLIANCE_MARGIN, value);
}
int AX12::setCCWComplicanceMargin(const byte value) const{
  return writeByte(AX12_CCW_COMPLIANCE_MARGIN, value);
}
int AX12::setCWComplianceSlope(const byte value) const{
  return writeByte(AX12_CW_COMPLIANCE_SLOPE, value);
}
int AX12::setCCWComplicanceSlope(const byte value) const{
  return writeByte(AX12_CCW_COMPLIANCE_SLOPE, value);
}   

// Goal Position Requested angular position for the Dynamixel actuator output to move to. 
// Setting this value to 0x3ff moves the output shaft to the position at 300°. 
int AX12::getGoalPosition(word *buff) const{
  return readWord(AX12_GOAL_POSITION_L, buff);
}
int AX12::setGoalPosition(const word value) const{
  return writeWord(AX12_GOAL_POSITION_L, value);
}

// Moving Speed. Sets the angular velocity of the output moving to the Goal Position. 
// Setting this value to its maximum value of 0x3ff moves the output with an angular 
// velocity of 114 RPM, provided that there is enough power supplied (The lowest velocity 
// is when this value is set to 1. When set to 0, the velocity is the largest possible for the 
// supplied voltage, e.g. no velocity control is applied.) 
int AX12::getMovingSpeed(word *buff) const{
  return readWord(AX12_MOVING_SPEED_L, buff);
}
int AX12::setMovingSpeed(const word value) const{
  return writeWord(AX12_MOVING_SPEED_L, value);
}

// Gets both Position and Speed with 1 Packet
int AX12::getGoalPositionSpeed(word *position, word *speed) const {
  word buffer[2];
  int status;
  status = readWord(AX12_GOAL_POSITION_L, buffer, 2);
  *position = buffer[0];
  *speed    = buffer[1];
  return status;
}

// Sets both Position and Speed with 1 Packet
int AX12::setGoalPositionSpeed(const word position, const word speed) const {
  word buffer[2];
  buffer[0] = position;
  buffer[1] = speed;
  return writeWord(AX12_GOAL_POSITION_L, buffer, 2);
}


int AX12::getTorqueLimit(word *buff) const{
  return readWord(AX12_TORQUE_LIMIT_L, buff);
}
int AX12::setTorqueLimit(const word value) const{
  return writeWord(AX12_TORQUE_LIMIT_L, value);
}

// Present Position. Current angular position of the Dynamixel actuator output.
int AX12::getPresentPosition(word *buff) const{
  return readWord(AX12_PRESENT_POSITION_L, buff);
}

// Present Speed. Current angular velocity of the Dynamixel actuator output. 
int AX12::getPresentSpeed(word *buff) const{
  return readWord(AX12_PRESENT_SPEED_L, buff);
}

// Gets present position and speed with 1 packet
int AX12::getPresentPositionSpeed(word *position, word *speed) const {
  word buffer[2];
  int status;
  status = readWord(AX12_PRESENT_POSITION_L, buffer, 2);
  *position = buffer[0];
  *speed    = buffer[1];
  return status;
}  

// Gets present position, load and speed with 1 packet
int AX12::getPresentPositionSpeedLoad(word *position, word *speed, word *load) const {
  word buffer[3];
  int status;
  status = readWord(AX12_PRESENT_POSITION_L, buffer, 3);
  *position = buffer[0];
  *speed    = buffer[1];
  *load     = buffer[2];
  return status;
}    

// Present Load. The magnitude of the load on the operating Dynamixel actuator. Bit 10 is 
// the direction of the load. 
// +-------+-------+----------------+---+---+---+---+---+---+---+---+---+---+
// |  BIT  | 15~11 |       10       | 9 | 8 | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |
// | Value |   0   | Load Direction |               Load Value              |
// +-------+-------+----------------+---+---+---+---+---+---+---+---+---+---+ 
// Load Direction = 0 : CCW Load,   Load Direction = 1: CW Load 
int AX12::getPresentLoad(word *buff) const{
  return readWord(AX12_PRESENT_LOAD_L, buff);
}

// Present Voltage. The voltage currently applied to the Dynamixel actuator. The value is 
// 10 times the actual voltage. For example, 10V is represented as 100 (0x64). 
int AX12::getPresentVoltage(byte *buff) const{
  return readByte(AX12_PRESENT_VOLTAGE, buff);
}

// Present Temperature. The internal temperature of the Dynamixel actuator in Degrees 
// Celsius. 
int AX12::getPresentTemperature(byte *buff) const{
  return readByte(AX12_PRESENT_TEMPERATURE, buff);
}

// Registered Instruction. Set to 1 when an instruction is assigned by the REG_WRITE 
// command. Set to 0 after it completes the assigned instruction by the Action command.  
int AX12::getRegisteredInstruction(byte *buff) const{
  return readByte(AX12_REGISTERED_INSTRUCTION, buff);
}
int AX12::setRegisteredInstruction(const byte value) const{
  return writeByte(AX12_REGISTERED_INSTRUCTION, value);
}

// Moving. Set to 1 when the Dynamixel actuator is moving by its own power. 
int AX12::getMoving(byte *buff) const{
  return readByte(AX12_MOVING, buff);
}

// Lock. If set to 1, only Address 0x18 to 0x23 can be written to and other areas cannot. 
// Once locked, it can only be unlocked by turning the power off. 
int AX12::getLock(byte *buff) const{
  return readByte(AX12_LOCK, buff);
}
int AX12::setLock(const byte value) const{
  return writeByte(AX12_LOCK, value);
}

// Punch. The minimum current supplied to the motor during operation. The initial value is 
// set to 0x20 and its maximum value is 0x3ff.   
int AX12::getPunch(word *buff) const{
  return readWord(AX12_PUNCH_L, buff);
}
int AX12::setPunch(const word value) const{
  return writeWord(AX12_PUNCH_L, value);
} 
