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

#include "axs1.h"

// Constuctor, needs ID of Dynamixel actuators and a serial connection
AXS1::AXS1(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 AXS1::setID(const byte id){
  int status = writeByte(AXS1_ID, id);
  if( status == NO_ERROR ){
    this->id = id;
  }
  return status;
}  

// Model Number.   For AX-S1, the value is 0X000D(13).  
int AXS1::getModelNumber(word *buff) const{
  return readWord(AXS1_MODEL_NUMBER_L, buff);
}

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

// Baud Rate. Determines the communication speed. The computation is done by the 
// following formula. Speed (BPS) = 2000000 / (Address4 + 1) 
int AXS1::getBaudRate(byte *buff) const{
  return readByte(AXS1_BAUD_RATE, buff);
}
int AXS1::setBaudRate(const byte value) const{
  return writeByte(AXS1_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 AXS1::getReturnDelayTime(byte *buff) const{
  return readByte(AXS1_RETURN_DELAY_TIME, buff);
}
int AXS1::setReturnDelayTime(const byte value) const{
  return writeByte(AXS1_RETURN_DELAY_TIME, 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 AXS1::getLimitTemperature(byte *buff) const{
  return readByte(AXS1_LIMIT_TEMPERATURE, buff);
}
int AXS1::setLimitTemperature(const byte value) const{
  return writeByte(AXS1_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 AXS1::getMinLimitVoltage(byte *buff) const{
  return readByte(AXS1_MIN_LIMIT_VOLTAGE, buff);
}
int AXS1::getMaxLimitVoltage(byte *buff) const{
  return readByte(AXS1_MAX_LIMIT_VOLTAGE, buff);
}
int AXS1::setMinLimitVoltage(const byte value) const{
  return writeByte(AXS1_MIN_LIMIT_VOLTAGE, value);
}
int AXS1::setMaxLimitVoltage(const byte value) const{
  return writeByte(AXS1_MAX_LIMIT_VOLTAGE, 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 AXS1::getReturnLevel(byte *buff) const{
  return readByte(AXS1_RETURN_LEVEL, buff);
}
int AXS1::setReturnLevel(const byte value) const{
  return writeByte(AXS1_RETURN_LEVEL, value);
}

// Obstacle Detected Compare Value Dynamixel Sensor Module sets the standard value 
// for the object detection that is in the direct line of object sensor parameter. If the infrared 
// sensor value is greater than a standard value, as it indicates an obstacle within the set 
// distance, the bit is set to a value of “1” in corresponding to sensor of IR Obstacle 
// Detected, Address 0x20, and conversely, when the sensor value is lower than a 
// standard value, it is set to “0.” 
int AXS1::getObstacleDetectedCompareValue(byte *buff) const {
  return readByte(AXS1_OBSTCL_DETECT_CMPVALUE, buff);
}
int AXS1::setObstacleDetectedCompareValue(const byte value) const {
  return writeByte(AXS1_OBSTCL_DETECT_CMPVALUE, value);
}


// Light Detected Compare Value Dynamixel Sensor Module sets the standard value for 
// the light detection that is in the direct line of infrared sensor parameter. If the light sensor 
// value is greater than a standard value, as it indicates a light that is brighter than set light 
// parameter, the bit is set to a value of “1” in corresponding to sensor of Light Detected, 
// and conversely, it is set to “0” when it is lower than a standard value.  
// The Light Detected Compare Value is allocated in the ROM (Address 0x15) and RAM 
// (Address 0x35) and when the power switched on, the value of EEPROM is copied to 
// RAM. Subsequent Address 0x18 is in RAM domain. 
int AXS1::getLightDetectedCompareValue(byte *buff) const {
  return readByte(AXS1_LIGHT_DETECT_CMPVALUE, buff);
}
int AXS1::setLightDetectedCompareValue(const byte value) const {
  return writeByte(AXS1_LIGHT_DETECT_CMPVALUE, value);
}

// Infrared Sensor Data (Left/Center/Right) It is the infrared sensor value of the 
// Dynamixel Sensor Module for measuring distance. The infrared sensor of AX-S1 emits 
// high frequency Infrared and the emitted ray bounces off an object or wall to return to the 
// IR sensor. The Infrared receiver of AX-S1 measures amount of infrared returned. High 
// value will be acquired when an object or wall is near the sensor. Measured value ranges 
// from 0~255. Only 255 will be acquired until a certain distance. Due to the innate 
// properties of infrared measurement method, value of reflected Infrared ray amount 
// might differ depending on the color of an object or surface texture. 
int AXS1::getInfraredSensorData(byte *left, byte *center, byte *right) const {
  byte buff[3];
  int status = readByte(AXS1_IR_SENSOR_LEFT, buff, 3);
  *left   = buff[0];
  *center = buff[1];
  *right  = buff[2];
  return status;
}

// Luminosity (Left/Center/Right) It is the light sensor value of the Dynamixel Sensor 
// Module. The technological concept is similar to the infrared sensor. However, this sensor 
// only measures amount of infrared ray emitted from source of illumination. Therefore, 
// light sensor value can be measured from illuminations, such as incandescent bulb, 
// emitting large amount of infrared. Lighter or candle light can be measured from short 
// distance as well. Measured value ranges from 0~255. 
int AXS1::getLuminositySensorData(byte *left, byte *center, byte *right) const {
  byte buff[3];
  int status = readByte(AXS1_LUMINOSITY_LEFT, buff, 3);
  *left   = buff[0];
  *center = buff[1];
  *right  = buff[2];
  return status;
}

// Obstacle Detection Flag When the value of infrared distance sensor becomes larger 
// than the Obstacle Detected Compare Value, the AX-S1 recognizes existence of an 
// object and sets object detection bit to 1. Refer to the below table for bit representation of 
// each sensor.
// +-------+--------------------------------------------------------------+
// | Bit   | Representation                                               |
// | Bit 2 | An object is detected on the Right Sensor /Light Detected    |
// | Bit 1 | An object is detected on the Center Sensor /Light Detected   |
// | Bit 0 | An object is detected on the Left Sensor /Light Detected     |
// +-------+--------------------------------------------------------------+
int AXS1::getObstacleDetectionFlag(byte *buff) const {
  return readByte(AXS1_OBSTCL_DETECT_FLAG, buff);
}

int AXS1::getObstacleDetected(bool *left, bool *center, bool *right) const {
  byte flag;
  int status = getObstacleDetectionFlag(&flag);
  *left   = ((0x1 & flag) != 0);
  *center = ((0x2 & flag) != 0);
  *right  = ((0x4 & flag) != 0);
  return status;
}

// Luminosity Detection Flag When the value of light sensor becomes larger than the 
// light detected compare value, the AX-S1 recognizes existence of source of illumination 
// and sets luminosity detection flag bit to 1. Bit representation of each sensor is the same 
// with bit representation of object detection flag setting. (Refer to Address 0x20) 
int AXS1::getLuminosityDetectionFlag(byte *buff) const {
  return readByte(AXS1_LUMINOSITY_DETECT_FLAG, buff);
}
int AXS1::getLuminosityDetected(bool *left, bool *center, bool *right) const {
  byte flag;
  int status = getLuminosityDetectionFlag(&flag);
  *left   = ((0x1 & flag) != 0);
  *center = ((0x2 & flag) != 0);
  *right  = ((0x4 & flag) != 0);
  return status;
}

// Sound Data It represents intensity of sound waves detected through the microphone of 
// AX-S1. The magnitude of sound wave fluctuates. 
// Value measured during noiseless state is around 127~128 (0x7F~0x80) and value 
// ranging from 0 to 255 (0xFF) will be measured for noisy state. Sound wave will be 
// measured at the frequency of 3800 input per second. 
int AXS1::getSoundData(byte *buff) const {
  return readByte(AXS1_SOUND_DATA, buff);
}
int AXS1::setSoundData(const byte value) const {
  return writeByte(AXS1_SOUND_DATA, value);
}

// Sound Data Max Hold AX-S1 has put aside a value for loudest sound. That is, when the 
// present sound data exceeds the Sound Data Max Hold value, the present sound data will 
// be copied as the Sound Data Max Hold. 
// Therefore, sound data less than 128 will be ignored and loudest sound intensity will be 
// updated.    
int AXS1::getSoundDataMaxHold(byte *buff) const {
  return readByte(AXS1_SOUND_DATA_MAX_HOLD, buff);
}
int AXS1::setSoundDataMaxHold(const byte value) const {
  return writeByte(AXS1_SOUND_DATA_MAX_HOLD, value);
}

// Sound Detected Count AX-S1 has a counter that counts occurrence of loud sound 
// exceeding standard level. As an example, number of handclap can be counted by using 
// this. Howe single handclap to be recognized as multiple claps. 800msec after the last count, the 
// value of sound detection frequency counter will be saved. 
// Timeline of sound detection frequency will be counted interna 
// sound detection frequency will be saved after 800msec. After saving, the sound 
// detection frequency value will reset to 0.     
int AXS1::getSoundDetectedCount(byte *buff) const {
  return readByte(AXS1_SOUND_DETECT_COUNT, buff);
}
int AXS1::setSoundDetectedCount(const byte value) const {
  return writeByte(AXS1_SOUND_DETECT_COUNT, value);
}

// Sound Detected Time 
// Anytime Sensor Module AX-S1 counts of sound detection, it 
// Detected Time is internally counted (counts 0~65535 repeatedly) and anytime 
// saves the time of sound occurrences. This function exists to detect the direction of 
// sound, and thus, it needs at least two AX-S1s; and by using speed of sound (around 
// 343m/sec in 20℃) it uses the time differences of sound arrival in microphone of two AX-S1s.  
// Sound Detected Count is increased, it saves the counted value. Therefore, by placing 
// the two AX-S1s in appropriate distance, and by simultaneously using broadcasting 
// command and initializing to 0 value, the time differenced sound occurs corresponding to 
// sound direction.  
int AXS1::getSoundDetectedTime(word *buff) const {
  return readWord(AXS1_SOUND_DETECT_TIME_L, buff);
}
int AXS1::setSoundDetectedTime(word buff) const {
  return writeWord(AXS1_SOUND_DETECT_TIME_L, buff);
}

// Buzzer Index All AX-S1 has built-in buzzer and thus, can playback the simple notes. 
// Buzzer can play up to 52 notes and as it has whole and semitone in each octave, it can 
// playback various melody sounds. The buzzer index value is assigned as follows.  
// +--------+--------+--------+--------+--------+--------+--------+--------+
// | Buzzer | Melody | Buzzer | Melody | Buzzer | Melody | Buzzer | Melody |
// | index  | notes  | index  | notes  | index  | notes  | index  | notes  | 
// +--------+--------+--------+--------+--------+--------+--------+--------+
// |   0    |   la   |   13   |   la#  |   26   |   si   |   39   |   do   |
// |   1    |   la#  |   14   |   si   |   27   |   do   |   40   |   do#  | 
// |   2    |   si   |   15   |   do   |   28   |   do#  |   41   |   re   | 
// |   3    |   do   |   16   |   do#  |   29   |   re   |   42   |   re#  | 
// |   4    |   do#  |   17   |   re   |   30   |   re#  |   43   |   mi   | 
// |   5    |   re   |   18   |   re#  |   31   |   mi   |   44   |   fa   | 
// |   6    |   re#  |   19   |   mi   |   32   |   fa   |   45   |   fa#  | 
// |   7    |   mi   |   20   |   fa   |   33   |   fa#  |   46   |   sol  | 
// |   8    |   fa   |   21   |   fa#  |   34   |   sol  |   47   |   ol#  | 
// |   9    |   fa#  |   22   |   sol  |   35   |   ol#  |   48   |   la   | 
// |  10    |   sol  |   23   |   sol# |   36   |   la   |   49   |   la#  |
// |  11    |   sol# |   24   |   la   |   37   |   la#  |   50   |   si   |
// |  12    |   la   |   25   |   la#  |   38   |   si   |   51   |   do   |
// +--------+--------+--------+--------+--------+--------+--------+--------+
int AXS1::getBuzzerIndex(byte *buff) const {
  return readByte(AXS1_BUZZER_INDEX, buff);
}
int AXS1::setBuzzerIndex(const byte value) const {
  return writeByte(AXS1_BUZZER_INDEX, value);
}

// Buzzer Time AX-S has a capability that controls the time intervall of buzzer sound. 
// Controllable within 0.1 second unit, the minimum length of time is 0.3 second and the 
// maximum length of time is 5.0 seconds. That is, if user inputs the value of 0~3, the 
// buzzer goes off in 0.3 second, whereas, if the input value is 50 or above, it goes off in 5 
// second. When the buzzer sound completes, the value automatically initializes back to 0. 
// There are two special features of AX-S1 buzzer time. 
// First is the function that sets the buzzer to sound cons 
// on buzzer time and input the melody note number on buzzer index, the buzzer sounds 
// the note constantly. To stop the buzzer, input 0 on buzzer time. 
// The second function plays back the special notes. If user inputs valu 
// time and value between 0~26 on buzzer index, 27 various melodies is replayed 
// corresponding to each number. When the melody playback is finished, the value 
// automatically initializes back to 0.    
int AXS1::getBuzzerTime(byte *buff) const {
  return readByte(AXS1_BUZZER_TIME, buff);
}
int AXS1::setBuzzerTime(const byte value) const {
  return writeByte(AXS1_BUZZER_TIME, value);
}

// 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 AXS1::getPresentVoltage(byte *buff) const {
  return readByte(AXS1_PRESENT_VOLTAGE, buff);
}

// Present Temperature. The internal temperature of the Dynamixel actuator in Degrees 
// Celsius. 
int AXS1::getPresentTemperature(byte *buff) const {
  return readByte(AXS1_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 AXS1::getRegisteredInstruction(byte *buff) const {
  return readByte(AXS1_REGISTERED_INSTRUCTION, buff);
}

int AXS1::setRegisteredInstruction(const byte value) const {
  return writeByte(AXS1_REGISTERED_INSTRUCTION, value);
}

// IR Remocon Arrived AX-S1 Sensor Module has infrared sensor module built-in in 
// the infrared emitter is built into left/center/right, it can transmit 
// I  Arrived value changes center and thus, it allows infrared remocon communication 
// between AX-S1’s. 2 byte transmission is possible.  
// Be cautious, however, as infrared remocon in all directions, but, as infrared remocon sensor 
// is built in only in center, its remocon data transmission is limited to certain angle. 
// When Infrared remocon data is received by sensor, IR Remocon 
// to 2, signaling 2 byte transmission. If you read IR Remocon RX data, the IR Remocon 
// and automatically initializes back to 0. 
int AXS1::getIRRemoconArrived(byte *buff) const {
  return readByte(AXS1_IR_REMOTE_ARRIVED, 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 AXS1::getLock(byte *buff) const{
  return readByte(AXS1_LOCK, buff);
}
int AXS1::setLock(const byte value) const{
  return writeByte(AXS1_LOCK, value);
}

// IR Remocon RX Data. Address where data from infrared remocon sensor is saved. It 
// reads the value and the IR Remocon Arrived value automatically initializes back to 0. 
int AXS1::getIRRemoconRXData(word *buff) const {
  return readWord(AXS1_IR_REMOCON_RX_DATA_0, buff);
}

// IR Remocon TX Data. Address where remocon data that will be transmitted via infrared 
// emitter is written to. Upon writing of 2 byte value, remocon data is immediately 
// transmitted.  
int AXS1::getIRRemoconTXData(word *buff) const {
  return readWord(AXS1_IR_REMOCON_TX_DATA_0, buff);
}
int AXS1::setIRRemoconTXData(word buff) const {
  return writeWord(AXS1_IR_REMOCON_TX_DATA_0, buff);
}

// Control Table RAM Range where obstacle detected 
// compare value of Address 0x14 is saved.  
// The IR sensors of AX-S1 emit powerful infrared rays to detect an object at a long
// distance. It is impossible to detect an object in a short distance around 5cm since 
// it always has maximum value in short distance 
// To prevent this, AX-S1 support low sensitive mod 
// distance. If the Obstacle Detected Compare Value is 0, it converts to low sensitive mode. 
// The low sensitive mode has very weak long-distance sensing capability but it is possible 
// to detect precise and sensitive short-distance detection not to saturate maximum value.  
int AXS1::getObstacleDetectedCompare(byte *buff) const {
  return readByte(AXS1_OBSTCL_DETECT_CMP, buff);
}
int AXS1::setObstacleDetectedCompare(const byte value) const {
  return writeByte(AXS1_OBSTCL_DETECT_CMP, value);
}

// Control Table RAM Range where light detected 
// compare value of Address 0x15 is saved 
// Each data has set value where their valid range is defined. Outside of this range, their 
// write command will return Error. Below table indicates the length for writing and its range. 
// 16 bit data is indicated (L) and (H) and as 2 byte. This 2 byte must be written as one in 
// instruction packet.    
int AXS1::getLightDetectedCompare(byte *buff) const {
  return readByte(AXS1_LIGHT_DETECT_CMP, buff);
}
int AXS1::setLightDetectedCompare(const byte value) const {
  return writeByte(AXS1_LIGHT_DETECT_CMP, value);
}
