Skip to content

Commit

Permalink
Fixed issue w/change in sense of return values from WPILib I2C Write …
Browse files Browse the repository at this point in the history
…and Read methods.
  • Loading branch information
kauailabs committed Mar 3, 2017
1 parent d34b079 commit 73aa4ea
Showing 1 changed file with 10 additions and 10 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -32,12 +32,12 @@ public void enableLogging(boolean enable) {

@Override
public boolean write(byte address, byte value ) {
boolean success;
boolean aborted;
synchronized(this){
success = port.write(address | 0x80, value);
aborted = port.write(address | 0x80, value);
}
if ( !success && trace ) System.out.println("navX-MXP I2C Write Error");
return success;
if ( aborted && trace ) System.out.println("navX-MXP I2C Write Error");
return !aborted;
}

final static int MAX_WPILIB_I2C_READ_BYTES = 127;
Expand All @@ -49,15 +49,15 @@ public boolean read(byte first_address, byte[] buffer) {
while ( len > 0 ) {
int read_len = (len > MAX_WPILIB_I2C_READ_BYTES) ? MAX_WPILIB_I2C_READ_BYTES : len;
byte[] read_buffer = new byte[read_len];
boolean write_ok;
boolean read_ok = false;
boolean write_aborted;
boolean read_aborted = true;
synchronized(this){
write_ok = port.write(first_address + buffer_offset, read_len);
if ( write_ok ) {
read_ok = port.readOnly(read_buffer, read_len);
write_aborted = port.write(first_address + buffer_offset, read_len);
if ( !write_aborted ) {
read_aborted = port.readOnly(read_buffer, read_len);
}
}
if ( write_ok && read_ok ) {
if ( !write_aborted && !read_aborted ) {
System.arraycopy(read_buffer, 0, buffer, buffer_offset, read_len);
buffer_offset += read_len;
len -= read_len;
Expand Down

0 comments on commit 73aa4ea

Please sign in to comment.