From 73aa4ea8401094d3be22276a53059420d16f059d Mon Sep 17 00:00:00 2001 From: kauailabs Date: Fri, 3 Mar 2017 09:38:51 -1000 Subject: [PATCH] Fixed issue w/change in sense of return values from WPILib I2C Write and Read methods. --- .../kauailabs/navx/frc/RegisterIO_I2C.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_I2C.java b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_I2C.java index c4ec69fd..d48a0094 100644 --- a/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_I2C.java +++ b/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_I2C.java @@ -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; @@ -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;