int Ax12Class::readPosition(unsigned char ID)
{
TChecksum = (ID + AX_POS_LENGTH + AX_READ_DATA + AX_PRESENT_POSITION_L + AX_BYTE_READ_POS);
while ( TChecksum >= 255){
TChecksum -= 255;
}
Checksum = 255 - TChecksum;
digitalWrite(Direction_Pin,HIGH);
usart_write(AX_START);
usart_write(AX_START);
usart_write(ID);
usart_write(AX_POS_LENGTH);
usart_write(AX_READ_DATA);
usart_write(AX_PRESENT_POSITION_L);
usart_write(AX_BYTE_READ_POS);
usart_write(Checksum);
delayMicroseconds(TX_DELAY_TIME);
digitalWrite(Direction_Pin,LOW); // Set Rx Mode
Position_Long_Byte = 0;
Time_Counter = 0;
while(usart_available() < 7 & Time_Counter < TIME_OUT){
Time_Counter++;
delay(1);
if( usart_peek() != 255 ){
usart_read();
}
}
while (usart_available() > 0){
Incoming_Byte = usart_read();
if ( Incoming_Byte == 255 & usart_peek() == 255 ){
usart_read(); // Start Bytes
usart_read(); // Ax-12 ID
usart_read(); // Length
if( (Error_Byte = usart_read()) != 0 ) // Error
return (Error_Byte*(-1));
Position_Low_Byte = usart_read(); // Position Bytes
Position_High_Byte = usart_read();
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 Ax12Class::readVoltage(unsigned char ID)
{
TChecksum = (ID + AX_VOLT_LENGTH + AX_READ_DATA + AX_PRESENT_VOLTAGE + AX_BYTE_READ);
while ( TChecksum >= 255){
TChecksum -= 255;
}
Checksum = 255 - TChecksum;
digitalWrite(Direction_Pin,HIGH);
usart_write(AX_START);
usart_write(AX_START);
usart_write(ID);
usart_write(AX_VOLT_LENGTH);
usart_write(AX_READ_DATA);
usart_write(AX_PRESENT_VOLTAGE);
usart_write(AX_BYTE_READ);
usart_write(Checksum);
delayMicroseconds(TX_DELAY_TIME);
digitalWrite(Direction_Pin,LOW); // Set Rx Mode
Voltage_Byte = 0;
Time_Counter = 0;
while(usart_available() < 6 & Time_Counter < TIME_OUT){
Time_Counter++;
delay(1);
if( usart_peek() != 255 ){
usart_read();
}
}
while (usart_available() > 0){
Incoming_Byte = usart_read();
if ( Incoming_Byte == 255 & usart_peek() == 255 ){
usart_read(); // Start Bytes
usart_read(); // Ax-12 ID
usart_read(); // Length
if( (Error_Byte = usart_read()) != 0 ) // Error
return (Error_Byte*(-1));
Voltage_Byte = usart_read(); // Voltage
}
}
return (Voltage_Byte); // Returns the read Voltage
}
int Ax12Class::read_error(void)
{
Time_Counter = 0;
while(usart_available() < 5 & Time_Counter < TIME_OUT){ // Wait for Data
Time_Counter++;
delay(1);
if( usart_peek() != 255 ){
usart_read();
}
}
while (usart_available() > 0){
Incoming_Byte = usart_read();
if ( Incoming_Byte == 255 & usart_peek() == 255 ){
usart_read(); // Start Bytes
usart_read(); // Ax-12 ID
usart_read(); // Length
Error_Byte = usart_read(); // Error
return (Error_Byte);
}
}
return (-1); // No Ax Response
}
int HardwareSerial::peek(void)
{
return usart_peek(this->usart_device);
}
