();
+ for (Button button : values()) {
+ if (button.isButtonPressed(mask)) {
+ buttons.add(button);
+ }
+ }
+ return buttons;
+ }
}
\ No newline at end of file
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java
index 79902ac2..95998427 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/Message.java
@@ -36,11 +36,11 @@ public static void main(String[] params) throws IOException, InitializeException
}
Color color = Color.GREEN;
String message = "";
- for (int i = 0; i < params.length; i++) {
- if (params[i].startsWith(COLOR_PREFIX)) {
- color = Color.getByName(params[i].substring(COLOR_PREFIX.length()));
+ for (String param : params) {
+ if (param.startsWith(COLOR_PREFIX)) {
+ color = Color.getByName(param.substring(COLOR_PREFIX.length()));
} else {
- message = params[i];
+ message = param;
}
}
AdafruitLcd lcd = LcdFactory.createLCD();
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/impl/AdafruitLcdImpl.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/impl/AdafruitLcdImpl.java
index 6f78592e..a294abdd 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/impl/AdafruitLcdImpl.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/impl/AdafruitLcdImpl.java
@@ -16,592 +16,592 @@
*/
package com.robo4j.hw.rpi.i2c.adafruitlcd.impl;
-import java.io.IOException;
-
import com.robo4j.hw.rpi.i2c.AbstractI2CDevice;
import com.robo4j.hw.rpi.i2c.adafruitlcd.AdafruitLcd;
import com.robo4j.hw.rpi.i2c.adafruitlcd.Button;
import com.robo4j.hw.rpi.i2c.adafruitlcd.Color;
import com.robo4j.hw.rpi.utils.I2cBus;
+import java.io.IOException;
+
+import static com.robo4j.hw.rpi.lcd.StringUtils.STRING_SPACE;
+
/**
* Javaification of the python script example for the Adafruit LCD shield. I
* have deliberately kept this close to the original python script, including
* most comments, even though it leads to less than beautiful code.
- *
+ *
* Here is an example on how to use this class:
* LCD lcd = new LCD();
* lcd.setText("Hello World!\n2nd Hello World!");
*
- *
+ *
* Here is an example with buttons:
* final LCD lcd = new LCD();
* lcd.setText("LCD Test!\nPress up/down...");
* ButtonPressedObserver observer = new ButtonPressedObserver(lcd);
* observer.addButtonListener(new ButtonListener() {
- * @Override
- * public void onButtonPressed(Button button) {
- * lcd.clear();
- * lcd.setText(button.toString());
- * }
+ * @Override
+ * public void onButtonPressed(Button button) {
+ * lcd.clear();
+ * lcd.setText(button.toString());
+ * }
* });
*
- *
+ *
* For more examples, check out the se.hirt.adafruitlcd.test package.
- *
+ *
* @author Marcus Hirt (@hirt)
* @author Miro Wengner (@miragemiko)
*/
public class AdafruitLcdImpl extends AbstractI2CDevice implements AdafruitLcd {
- public enum Direction {
- LEFT, RIGHT;
- }
-
- // LCD Commands
- private static final int LCD_CLEARDISPLAY = 0x01;
- private static final int LCD_RETURNHOME = 0x02;
- private static final int LCD_ENTRYMODESET = 0x04;
- private static final int LCD_DISPLAYCONTROL = 0x08;
- private static final int LCD_CURSORSHIFT = 0x10;
- // private static final int LCD_FUNCTIONSET = 0x20;
- private static final int LCD_SETCGRAMADDR = 0x40;
- private static final int LCD_SETDDRAMADDR = 0x80;
-
- // Flags for display on/off control
- private static final int LCD_DISPLAYON = 0x04;
- // private static final int LCD_DISPLAYOFF = 0x00;
- private static final int LCD_CURSORON = 0x02;
- private static final int LCD_CURSOROFF = 0x00;
- private static final int LCD_BLINKON = 0x01;
- private static final int LCD_BLINKOFF = 0x00;
-
- // Flags for display entry mode
- // private static final int LCD_ENTRYRIGHT = 0x00;
- private static final int LCD_ENTRYLEFT = 0x02;
- private static final int LCD_ENTRYSHIFTINCREMENT = 0x01;
- private static final int LCD_ENTRYSHIFTDECREMENT = 0x00;
-
- // Flags for display/cursor shift
- private static final int LCD_DISPLAYMOVE = 0x08;
- private static final int LCD_CURSORMOVE = 0x00;
- private static final int LCD_MOVERIGHT = 0x04;
- private static final int LCD_MOVELEFT = 0x00;
-
- // Port expander registers
- // IOCON when Bank 0 active
- private static final int MCP23017_IOCON_BANK0 = 0x0A;
- // IOCON when Bank 1 active
- private static final int MCP23017_IOCON_BANK1 = 0x15;
-
- // These are register addresses when in Bank 1 only:
- private static final int MCP23017_GPIOA = 0x09;
- private static final int MCP23017_IODIRB = 0x10;
- private static final int MCP23017_GPIOB = 0x19;
-
- // The LCD data pins (D4-D7) connect to MCP pins 12-9 (PORTB4-1), in
- // that order. Because this sequence is 'reversed,' a direct shift
- // won't work. This table remaps 4-bit data values to MCP PORTB
- // outputs, incorporating both the reverse and shift.
- private static final int[] SHIFT_REVERSE = { 0x00, 0x10, 0x08, 0x18, 0x04, 0x14, 0x0C, 0x1C, 0x02, 0x12, 0x0A, 0x1A,
- 0x06, 0x16, 0x0E, 0x1E };
-
- private static final int[] ROW_OFFSETS = new int[] { 0x00, 0x40, 0x14, 0x54 };
-
- private int portA = 0x00;
- private int portB = 0x00;
- private int ddrB = 0x10;
- private int displayShift = LCD_CURSORMOVE | LCD_MOVERIGHT;
- private int displayMode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT;
- private int displayControl = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF;
- private Color color = Color.WHITE;
-
- public AdafruitLcdImpl() throws IOException {
- // This seems to be the default for AdaFruit 1115.
- this(AdafruitLcd.DEFAULT_BUS, AdafruitLcd.DEFAULT_ADDRESS);
- }
-
- public AdafruitLcdImpl(I2cBus bus, int address) throws IOException {
- super(bus, address);
- initialize();
- }
-
- private synchronized void initialize() throws IOException {
- // Set MCP23017 IOCON register to Bank 0 with sequential operation.
- // If chip is already set for Bank 0, this will just write to OLATB,
- // which won't seriously bother anything on the plate right now
- // (blue backlight LED will come on, but that's done in the next
- // step anyway).
- write(MCP23017_IOCON_BANK1, (byte) 0);
-
- // Brute force reload ALL registers to known state. This also
- // sets up all the input pins, pull-ups, etc. for the Pi Plate.
- // NOTE(marcus/9 dec 2013): 0x3F assumes that GPA5 is input too -
- // it is however not connected.
- byte[] registers = { 0x3F, // IODIRA R+G LEDs=outputs, buttons=inputs
- (byte) ddrB, // IODIRB LCD D7=input, Blue LED=output
- 0x3F, // IPOLA Invert polarity on button inputs
- 0x00, // IPOLB
- 0x00, // GPINTENA Disable interrupt-on-change
- 0x00, // GPINTENB
- 0x00, // DEFVALA
- 0x00, // DEFVALB
- 0x00, // INTCONA
- 0x00, // INTCONB
- 0x00, // IOCON
- 0x00, // IOCON
- 0x3F, // GPPUA Enable pull-ups on buttons
- 0x00, // GPPUB
- 0x00, // INTFA
- 0x00, // INTFB
- 0x00, // INTCAPA
- 0x00, // INTCAPB
- (byte) portA, // GPIOA
- (byte) portB, // GPIOB
- (byte) portA, // OLATA 0 on all outputs; side effect of
- (byte) portB // OLATB turning on R+G+B backlight LEDs.
- };
- write(0, registers, 0, registers.length);
-
- // Switch to Bank 1 and disable sequential operation.
- // From this point forward, the register addresses do NOT match
- // the list immediately above. Instead, use the constants defined
- // at the start of the class. Also, the address register will no
- // longer increment automatically after this -- multi-byte
- // operations must be broken down into single-byte calls.
- write(MCP23017_IOCON_BANK0, (byte) 0xA0);
-
- write(0x33); // Init
- write(0x32); // Init
- write(0x28); // 2 line 5x8 matrix
- write(LCD_CLEARDISPLAY);
- write(LCD_CURSORSHIFT | displayShift);
- write(LCD_ENTRYMODESET | displayMode);
- write(LCD_DISPLAYCONTROL | displayControl);
- write(LCD_RETURNHOME);
- }
-
- private synchronized void write(int i, byte[] registers, int j, int length) throws IOException {
- // i2CConfig.write(i, registers, j, length);
- writeByteBufferByAddress(i, registers, j, length);
- }
-
- private synchronized void write(int bank, byte b) throws IOException {
- // i2CConfig.write(bank, b);
- writeByte(bank, b);
- }
-
- private synchronized void write(int value) throws IOException {
- waitOnLCDBusyFlag();
- int bitmask = portB & 0x01; // Mask out PORTB LCD control bits
-
- byte[] data = out4(bitmask, value);
- // i2CConfig.write(MCP23017_GPIOB, data, 0, 4);
- // TODO review
- writeByteBufferByAddress(MCP23017_GPIOB, data, 0, 4);
- portB = data[3];
-
- // If a poll-worthy instruction was issued, reconfigure D7
- // pin as input to indicate need for polling on next call.
- if (value == LCD_CLEARDISPLAY || value == LCD_RETURNHOME) {
- ddrB |= 0x10;
- // i2CConfig.write(MCP23017_IODIRB, (byte) ddrB);
- writeByte(MCP23017_IODIRB, (byte) ddrB);
- }
- }
-
- private synchronized void waitOnLCDBusyFlag() throws IOException {
- // The speed of LCD accesses is inherently limited by I2C through the
- // port expander. A 'well behaved program' is expected to poll the
- // LCD to know that a prior instruction completed. But the timing of
- // most instructions is a known uniform 37 ms. The enable strobe
- // can't even be twiddled that fast through I2C, so it's a safe bet
- // with these instructions to not waste time polling (which requires
- // several I2C transfers for reconfiguring the port direction).
- // The D7 pin is set as input when a potentially time-consuming
- // instruction has been issued (e.g. screen clear), as well as on
- // startup, and polling will then occur before more commands or data
- // are issued.
-
- // If pin D7 is in input state, poll LCD busy flag until clear.
- if ((ddrB & 0x10) != 0) {
- int lo = (portB & 0x01) | 0x40;
- int hi = lo | 0x20; // E=1 (strobe)
- // i2CConfig.write(MCP23017_GPIOB, (byte) lo);
- writeByte(MCP23017_GPIOB, (byte) lo);
- while (true) {
- // i2CConfig.write((byte) hi); // Strobe high (enable)
- writeByte((byte) hi); // Strobe high (enable)
- // int bits = i2CConfig.read(); // First nybble contains busy state
- // TODO: review
- int bits = i2C.read(); // First nybble contains busy state
- // i2CConfig.write(MCP23017_GPIOB, new byte[] { (byte) lo, (byte) hi, (byte) lo
- // }, 0, 3); // Strobe
- writeByteBufferByAddress(MCP23017_GPIOB, new byte[] { (byte) lo, (byte) hi, (byte) lo }, 0, 3); // Strobe
- // low,
- // high,
- // low.
- // Second
- // nybble
- // (A3)
- // is
- // ignored.
- if ((bits & 0x02) == 0)
- break; // D7=0, not busy
- }
- portB = lo;
- ddrB &= 0xEF; // Polling complete, change D7 pin to output
- // i2CConfig.write(MCP23017_IODIRB, (byte) ddrB);
- writeByte(MCP23017_IODIRB, (byte) ddrB);
- }
- }
-
- private byte[] out4(int bitmask, int value) {
- int hi = bitmask | SHIFT_REVERSE[value >> 4];
- int lo = bitmask | SHIFT_REVERSE[value & 0x0F];
-
- return new byte[] { (byte) (hi | 0x20), (byte) hi, (byte) (lo | 0x20), (byte) lo };
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setText(java.lang.String)
- */
- @Override
- public synchronized void setText(String s) throws IOException {
- String[] rowStrings = s.split("\n");
- for (int i = 0; i < rowStrings.length; i++) {
- setText(i, pad(rowStrings[i]));
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setText(int, java.lang.String)
- */
- @Override
- public synchronized void setText(int row, String string) throws IOException {
- setCursorPosition(row, 0);
- internalWrite(string);
- }
-
- private void internalWrite(String s) throws IOException {
- int sLen = s.length();
- int bytesLen = 4 * sLen;
- if (sLen < 1) {
- return;
- }
-
- waitOnLCDBusyFlag();
- int bitmask = portB & 0x01; // Mask out PORTB LCD control bits
- bitmask |= 0x80; // Set data bit
-
- byte[] bytes = new byte[4 * sLen];
- for (int i = 0; i < sLen; i++) {
- byte[] data = out4(bitmask, s.charAt(i));
- for (int j = 0; j < 4; j++) {
- bytes[(i * 4) + j] = data[j];
- }
- }
- write(MCP23017_GPIOB, bytes, 0, bytesLen);
- portB = bytes[bytesLen - 1];
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setCursorPosition(int, int)
- */
- @Override
- public synchronized void setCursorPosition(int row, int column) throws IOException {
- write(LCD_SETDDRAMADDR | (column + ROW_OFFSETS[row]));
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#stop()
- */
- @Override
- public synchronized void stop() throws IOException {
- portA = 0xC0; // Turn off LEDs on the way out
- portB = 0x01;
- sleep(2);
- write(MCP23017_IOCON_BANK1, (byte) 0);
- byte[] registers = { 0x3F, // IODIRA
- (byte) ddrB, // IODIRB
- 0x0, // IPOLA
- 0x0, // IPOLB
- 0x0, // GPINTENA
- 0x0, // GPINTENB
- 0x0, // DEFVALA
- 0x0, // DEFVALB
- 0x0, // INTCONA
- 0x0, // INTCONB
- 0x0, // IOCON
- 0x0, // IOCON
- 0x3F, // GPPUA
- 0x0, // GPPUB
- 0x0, // INTFA
- 0x0, // INTFB
- 0x0, // INTCAPA
- 0x0, // INTCAPB
- (byte) portA, // GPIOA
- (byte) portB, // GPIOB
- (byte) portA, // OLATA
- (byte) portB // OLATB
- };
- write(0, registers, 0, registers.length);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#clear()
- */
- @Override
- public synchronized void clear() throws IOException {
- write(LCD_CLEARDISPLAY);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#home()
- */
- @Override
- public synchronized void home() throws IOException {
- write(LCD_RETURNHOME);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setCursorEnabled(boolean)
- */
- @Override
- public synchronized void setCursorEnabled(boolean enable) throws IOException {
- if (enable) {
- displayControl |= LCD_CURSORON;
- write(LCD_DISPLAYCONTROL | displayControl);
- } else {
- displayControl &= ~LCD_CURSORON;
- write(LCD_DISPLAYCONTROL | displayControl);
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#isCursorEnabled()
- */
- @Override
- public synchronized boolean isCursorEnabled() {
- return (displayControl & LCD_CURSORON) > 0;
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setDisplayEnabled(boolean)
- */
- @Override
- public synchronized void setDisplayEnabled(boolean enable) throws IOException {
- if (enable) {
- displayControl |= LCD_DISPLAYON;
- write(LCD_DISPLAYCONTROL | displayControl);
- } else {
- displayControl &= ~LCD_DISPLAYON;
- write(LCD_DISPLAYCONTROL | displayControl);
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#isDisplayEnabled()
- */
- @Override
- public synchronized boolean isDisplayEnabled() {
- return (displayControl & LCD_DISPLAYON) > 0;
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setBlinkEnabled(boolean)
- */
- @Override
- public synchronized void setBlinkEnabled(boolean enable) throws IOException {
- if (enable) {
- displayControl |= LCD_BLINKON;
- write(LCD_DISPLAYCONTROL | displayControl);
- } else {
- displayControl &= ~LCD_BLINKON;
- write(LCD_DISPLAYCONTROL | displayControl);
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#isBlinkEnabled()
- */
- @Override
- public synchronized boolean isBlinkEnabled() {
- return (displayControl & LCD_BLINKON) > 0;
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setBacklight(se.hirt.pi.adafruitlcd.LCD.
- * Color)
- */
- @Override
- public synchronized void setBacklight(Color color) throws IOException {
- int c = ~color.getValue();
- portA = (portA & 0x3F) | ((c & 0x03) << 6);
- portB = (portB & 0xFE) | ((c & 0x04) >> 2);
- // Has to be done as two writes because sequential operation is off.
+ public enum Direction {
+ LEFT, RIGHT;
+ }
+
+ // LCD Commands
+ private static final int LCD_DISPLAY_CLEAR = 0x01;
+ private static final int LCD_DISPLAY_CONTROL = 0x08;
+ private static final int LCD_RETURN_HOME = 0x02;
+ private static final int LCD_ENTRY_MODE_SET = 0x04;
+ private static final int LCD_CURSOR_SHIFT = 0x10;
+ // private static final int LCD_FUNCTIONSET = 0x20;
+ private static final int LCD_SET_CGRAMADDR = 0x40;
+ private static final int LCD_SET_DDRAMADDR = 0x80;
+
+ // Flags for display on/off control
+ private static final int LCD_DISPLAY_ON = 0x04;
+ // private static final int LCD_DISPLAY_OFF = 0x00;
+ private static final int LCD_CURSOR_ON = 0x02;
+ private static final int LCD_CURSOR_OFF = 0x00;
+ private static final int LCD_BLINK_ON = 0x01;
+ private static final int LCD_BLINK_OFF = 0x00;
+
+ // Flags for display entry mode
+ // private static final int LCD_ENTRY_RIGHT = 0x00;
+ private static final int LCD_ENTRY_LEFT = 0x02;
+ private static final int LCD_ENTRY_SHIFT_INCREMENT = 0x01;
+ private static final int LCD_ENTRY_SHIFT_DECREMENT = 0x00;
+
+ // Flags for display/cursor shift
+ private static final int LCD_DISPLAY_MOVE = 0x08;
+ private static final int LCD_CURSOR_MOVE = 0x00;
+ private static final int LCD_MOVE_RIGHT = 0x04;
+ private static final int LCD_MOVE_LEFT = 0x00;
+
+ // Port expander registers
+ // IOCON when Bank 0 active
+ private static final int MCP23017_IOCON_BANK0 = 0x0A;
+ // IOCON when Bank 1 active
+ private static final int MCP23017_IOCON_BANK1 = 0x15;
+
+ // These are register addresses when in Bank 1 only:
+ private static final int MCP23017_GPIOA = 0x09;
+ private static final int MCP23017_IODIRB = 0x10;
+ private static final int MCP23017_GPIOB = 0x19;
+
+ // The LCD data pins (D4-D7) connect to MCP pins 12-9 (PORTB4-1), in
+ // that order. Because this sequence is 'reversed,' a direct shift
+ // won't work. This table remaps 4-bit data values to MCP PORTB
+ // outputs, incorporating both the reverse and shift.
+ private static final int[] SHIFT_REVERSE = {0x00, 0x10, 0x08, 0x18, 0x04, 0x14, 0x0C, 0x1C, 0x02, 0x12, 0x0A, 0x1A,
+ 0x06, 0x16, 0x0E, 0x1E};
+
+ private static final int[] ROW_OFFSETS = new int[]{0x00, 0x40, 0x14, 0x54};
+
+ private int portA = 0x00;
+ private int portB = 0x00;
+ private int ddrB = 0x10;
+ private int displayShift = LCD_CURSOR_MOVE | LCD_MOVE_RIGHT;
+ private int displayMode = LCD_ENTRY_LEFT | LCD_ENTRY_SHIFT_DECREMENT;
+ private int displayControl = LCD_DISPLAY_ON | LCD_CURSOR_OFF | LCD_BLINK_OFF;
+ private Color color = Color.WHITE;
+
+ public AdafruitLcdImpl() throws IOException {
+ // This seems to be the default for AdaFruit 1115.
+ this(AdafruitLcd.DEFAULT_BUS, AdafruitLcd.DEFAULT_ADDRESS);
+ }
+
+ public AdafruitLcdImpl(I2cBus bus, int address) throws IOException {
+ super(bus, address);
+ initialize();
+ }
+
+ private synchronized void initialize() throws IOException {
+ // Set MCP23017 IOCON register to Bank 0 with sequential operation.
+ // If chip is already set for Bank 0, this will just write to OLATB,
+ // which won't seriously bother anything on the plate right now
+ // (blue backlight LED will come on, but that's done in the next
+ // step anyway).
+ write(MCP23017_IOCON_BANK1, (byte) 0);
+
+ // Brute force reload ALL registers to known state. This also
+ // sets up all the input pins, pull-ups, etc. for the Pi Plate.
+ // NOTE(marcus/9 dec 2013): 0x3F assumes that GPA5 is input too -
+ // it is however not connected.
+ byte[] registers = {0x3F, // IODIRA R+G LEDs=outputs, buttons=inputs
+ (byte) ddrB, // IODIRB LCD D7=input, Blue LED=output
+ 0x3F, // IPOLA Invert polarity on button inputs
+ 0x00, // IPOLB
+ 0x00, // GPINTENA Disable interrupt-on-change
+ 0x00, // GPINTENB
+ 0x00, // DEFVALA
+ 0x00, // DEFVALB
+ 0x00, // INTCONA
+ 0x00, // INTCONB
+ 0x00, // IOCON
+ 0x00, // IOCON
+ 0x3F, // GPPUA Enable pull-ups on buttons
+ 0x00, // GPPUB
+ 0x00, // INTFA
+ 0x00, // INTFB
+ 0x00, // INTCAPA
+ 0x00, // INTCAPB
+ (byte) portA, // GPIOA
+ (byte) portB, // GPIOB
+ (byte) portA, // OLATA 0 on all outputs; side effect of
+ (byte) portB // OLATB turning on R+G+B backlight LEDs.
+ };
+ write(0, registers, 0, registers.length);
+
+ // Switch to Bank 1 and disable sequential operation.
+ // From this point forward, the register addresses do NOT match
+ // the list immediately above. Instead, use the constants defined
+ // at the start of the class. Also, the address register will no
+ // longer increment automatically after this -- multi-byte
+ // operations must be broken down into single-byte calls.
+ write(MCP23017_IOCON_BANK0, (byte) 0xA0);
+
+ write(0x33); // Init
+ write(0x32); // Init
+ write(0x28); // 2 line 5x8 matrix
+ write(LCD_DISPLAY_CLEAR);
+ write(LCD_CURSOR_SHIFT | displayShift);
+ write(LCD_ENTRY_MODE_SET | displayMode);
+ write(LCD_DISPLAY_CONTROL | displayControl);
+ write(LCD_RETURN_HOME);
+ }
+
+ private synchronized void write(int i, byte[] registers, int j, int length) throws IOException {
+ // i2CConfig.write(i, registers, j, length);
+ writeByteBufferByAddress(i, registers, j, length);
+ }
+
+ private synchronized void write(int bank, byte b) throws IOException {
+ // i2CConfig.write(bank, b);
+ writeByte(bank, b);
+ }
+
+ private synchronized void write(int value) throws IOException {
+ waitOnLCDBusyFlag();
+ int bitmask = portB & 0x01; // Mask out PORTB LCD control bits
+
+ byte[] data = out4(bitmask, value);
+ // i2CConfig.write(MCP23017_GPIOB, data, 0, 4);
+ // TODO review
+ writeByteBufferByAddress(MCP23017_GPIOB, data, 0, 4);
+ portB = data[3];
+
+ // If a poll-worthy instruction was issued, reconfigure D7
+ // pin as input to indicate need for polling on next call.
+ if (value == LCD_DISPLAY_CLEAR || value == LCD_RETURN_HOME) {
+ ddrB |= 0x10;
+ // i2CConfig.write(MCP23017_IODIRB, (byte) ddrB);
+ writeByte(MCP23017_IODIRB, (byte) ddrB);
+ }
+ }
+
+ private synchronized void waitOnLCDBusyFlag() throws IOException {
+ // The speed of LCD accesses is inherently limited by I2C through the
+ // port expander. A 'well behaved program' is expected to poll the
+ // LCD to know that a prior instruction completed. But the timing of
+ // most instructions is a known uniform 37 ms. The enable strobe
+ // can't even be twiddled that fast through I2C, so it's a safe bet
+ // with these instructions to not waste time polling (which requires
+ // several I2C transfers for reconfiguring the port direction).
+ // The D7 pin is set as input when a potentially time-consuming
+ // instruction has been issued (e.g. screen clear), as well as on
+ // startup, and polling will then occur before more commands or data
+ // are issued.
+
+ // If pin D7 is in input state, poll LCD busy flag until clear.
+ if ((ddrB & 0x10) != 0) {
+ int lo = (portB & 0x01) | 0x40;
+ int hi = lo | 0x20; // E=1 (strobe)
+ // i2CConfig.write(MCP23017_GPIOB, (byte) lo);
+ writeByte(MCP23017_GPIOB, (byte) lo);
+ while (true) {
+ // i2CConfig.write((byte) hi); // Strobe high (enable)
+ writeByte((byte) hi); // Strobe high (enable)
+ // int bits = i2CConfig.read(); // First nybble contains busy state
+ // TODO: review
+ int bits = i2C.read(); // First nybble contains busy state
+ // i2CConfig.write(MCP23017_GPIOB, new byte[] { (byte) lo, (byte) hi, (byte) lo
+ // }, 0, 3); // Strobe
+ writeByteBufferByAddress(MCP23017_GPIOB, new byte[]{(byte) lo, (byte) hi, (byte) lo}, 0, 3); // Strobe
+ // low,
+ // high,
+ // low.
+ // Second
+ // nybble
+ // (A3)
+ // is
+ // ignored.
+ if ((bits & 0x02) == 0)
+ break; // D7=0, not busy
+ }
+ portB = lo;
+ ddrB &= 0xEF; // Polling complete, change D7 pin to output
+ // i2CConfig.write(MCP23017_IODIRB, (byte) ddrB);
+ writeByte(MCP23017_IODIRB, (byte) ddrB);
+ }
+ }
+
+ private byte[] out4(int bitmask, int value) {
+ int hi = bitmask | SHIFT_REVERSE[value >> 4];
+ int lo = bitmask | SHIFT_REVERSE[value & 0x0F];
+
+ return new byte[]{(byte) (hi | 0x20), (byte) hi, (byte) (lo | 0x20), (byte) lo};
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setText(java.lang.String)
+ */
+ @Override
+ public synchronized void setText(String s) throws IOException {
+ String[] rowStrings = s.split("\n");
+ for (int i = 0; i < rowStrings.length; i++) {
+ setText(i, pad(rowStrings[i]));
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setText(int, java.lang.String)
+ */
+ @Override
+ public synchronized void setText(int row, String string) throws IOException {
+ setCursorPosition(row, 0);
+ internalWrite(string);
+ }
+
+ private void internalWrite(String s) throws IOException {
+ int sLen = s.length();
+ int bytesLen = 4 * sLen;
+ if (sLen < 1) {
+ return;
+ }
+
+ waitOnLCDBusyFlag();
+ int bitmask = portB & 0x01; // Mask out PORTB LCD control bits
+ bitmask |= 0x80; // Set data bit
+
+ byte[] bytes = new byte[4 * sLen];
+ for (int i = 0; i < sLen; i++) {
+ byte[] data = out4(bitmask, s.charAt(i));
+ System.arraycopy(data, 0, bytes, (i * 4), 4);
+ }
+ write(MCP23017_GPIOB, bytes, 0, bytesLen);
+ portB = bytes[bytesLen - 1];
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setCursorPosition(int, int)
+ */
+ @Override
+ public synchronized void setCursorPosition(int row, int column) throws IOException {
+ write(LCD_SET_DDRAMADDR | (column + ROW_OFFSETS[row]));
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#stop()
+ */
+ @Override
+ public synchronized void stop() throws IOException {
+ portA = 0xC0; // Turn off LEDs on the way out
+ portB = 0x01;
+ sleep(2);
+ write(MCP23017_IOCON_BANK1, (byte) 0);
+ byte[] registers = {0x3F, // IODIRA
+ (byte) ddrB, // IODIRB
+ 0x0, // IPOLA
+ 0x0, // IPOLB
+ 0x0, // GPINTENA
+ 0x0, // GPINTENB
+ 0x0, // DEFVALA
+ 0x0, // DEFVALB
+ 0x0, // INTCONA
+ 0x0, // INTCONB
+ 0x0, // IOCON
+ 0x0, // IOCON
+ 0x3F, // GPPUA
+ 0x0, // GPPUB
+ 0x0, // INTFA
+ 0x0, // INTFB
+ 0x0, // INTCAPA
+ 0x0, // INTCAPB
+ (byte) portA, // GPIOA
+ (byte) portB, // GPIOB
+ (byte) portA, // OLATA
+ (byte) portB // OLATB
+ };
+ write(0, registers, 0, registers.length);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#clear()
+ */
+ @Override
+ public synchronized void clear() throws IOException {
+ write(LCD_DISPLAY_CLEAR);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#home()
+ */
+ @Override
+ public synchronized void home() throws IOException {
+ write(LCD_RETURN_HOME);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setCursorEnabled(boolean)
+ */
+ @Override
+ public synchronized void setCursorEnabled(boolean enable) throws IOException {
+ if (enable) {
+ displayControl |= LCD_CURSOR_ON;
+ write(LCD_DISPLAY_CONTROL | displayControl);
+ } else {
+ displayControl &= ~LCD_CURSOR_ON;
+ write(LCD_DISPLAY_CONTROL | displayControl);
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#isCursorEnabled()
+ */
+ @Override
+ public synchronized boolean isCursorEnabled() {
+ return (displayControl & LCD_CURSOR_ON) > 0;
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setDisplayEnabled(boolean)
+ */
+ @Override
+ public synchronized void setDisplayEnabled(boolean enable) throws IOException {
+ if (enable) {
+ displayControl |= LCD_DISPLAY_ON;
+ write(LCD_DISPLAY_CONTROL | displayControl);
+ } else {
+ displayControl &= ~LCD_DISPLAY_ON;
+ write(LCD_DISPLAY_CONTROL | displayControl);
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#isDisplayEnabled()
+ */
+ @Override
+ public synchronized boolean isDisplayEnabled() {
+ return (displayControl & LCD_DISPLAY_ON) > 0;
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setBlinkEnabled(boolean)
+ */
+ @Override
+ public synchronized void setBlinkEnabled(boolean enable) throws IOException {
+ if (enable) {
+ displayControl |= LCD_BLINK_ON;
+ write(LCD_DISPLAY_CONTROL | displayControl);
+ } else {
+ displayControl &= ~LCD_BLINK_ON;
+ write(LCD_DISPLAY_CONTROL | displayControl);
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#isBlinkEnabled()
+ */
+ @Override
+ public synchronized boolean isBlinkEnabled() {
+ return (displayControl & LCD_BLINK_ON) > 0;
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setBacklight(se.hirt.pi.adafruitlcd.LCD.
+ * Color)
+ */
+ @Override
+ public synchronized void setBacklight(Color color) throws IOException {
+ int c = ~color.getValue();
+ portA = (portA & 0x3F) | ((c & 0x03) << 6);
+ portB = (portB & 0xFE) | ((c & 0x04) >> 2);
+ // Has to be done as two writes because sequential operation is off.
// i2CConfig.write(MCP23017_GPIOA, (byte) portA);
- writeByte(MCP23017_GPIOA, (byte) portA);
+ writeByte(MCP23017_GPIOA, (byte) portA);
// i2CConfig.write(MCP23017_GPIOB, (byte) portB);
- writeByte(MCP23017_GPIOB, (byte) portB);
- this.color = color;
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#scrollDisplay(se.hirt.pi.adafruitlcd.LCD.
- * Direction)
- */
- @Override
- public synchronized void scrollDisplay(Direction direction) throws IOException {
- if (direction == Direction.LEFT) {
- displayShift = LCD_DISPLAYMOVE | LCD_MOVELEFT;
- write(LCD_CURSORSHIFT | displayShift);
- } else {
- displayShift = LCD_DISPLAYMOVE | LCD_MOVERIGHT;
- write(LCD_CURSORSHIFT | displayShift);
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setTextFlowDirection(se.hirt.pi.adafruitlcd.
- * LCD.Direction)
- */
- @Override
- public synchronized void setTextFlowDirection(Direction direction) throws IOException {
- if (direction == Direction.LEFT) {
- // This is for text that flows right to left
- displayMode &= ~LCD_ENTRYLEFT;
- write(LCD_ENTRYMODESET | displayMode);
- } else {
- // This is for text that flows left to right
- displayMode |= LCD_ENTRYLEFT;
- write(LCD_ENTRYMODESET | displayMode);
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#setAutoScrollEnabled(boolean)
- */
- @Override
- public synchronized void setAutoScrollEnabled(boolean enable) throws IOException {
- if (enable) {
- // This will 'right justify' text from the cursor
- displayMode |= LCD_ENTRYSHIFTINCREMENT;
- write(LCD_ENTRYMODESET | displayMode);
- } else {
- // This will 'left justify' text from the cursor
- displayMode &= ~LCD_ENTRYSHIFTINCREMENT;
- write(LCD_ENTRYMODESET | displayMode);
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#isAutoScrollEnabled()
- */
- @Override
- public synchronized boolean isAutoScrollEnabled() {
- return (displayControl & LCD_ENTRYSHIFTINCREMENT) > 0;
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#isButtonPressed(se.hirt.pi.adafruitlcd.
- * Button)
- */
- @Override
- public synchronized boolean isButtonPressed(Button button) throws IOException {
- return ((read(MCP23017_GPIOA) >> button.getPin()) & 1) > 0;
- }
-
- private synchronized int read(int bank) throws IOException {
+ writeByte(MCP23017_GPIOB, (byte) portB);
+ this.color = color;
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#scrollDisplay(se.hirt.pi.adafruitlcd.LCD.
+ * Direction)
+ */
+ @Override
+ public synchronized void scrollDisplay(Direction direction) throws IOException {
+ if (direction == Direction.LEFT) {
+ displayShift = LCD_DISPLAY_MOVE | LCD_MOVE_LEFT;
+ write(LCD_CURSOR_SHIFT | displayShift);
+ } else {
+ displayShift = LCD_DISPLAY_MOVE | LCD_MOVE_RIGHT;
+ write(LCD_CURSOR_SHIFT | displayShift);
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setTextFlowDirection(se.hirt.pi.adafruitlcd.
+ * LCD.Direction)
+ */
+ @Override
+ public synchronized void setTextFlowDirection(Direction direction) throws IOException {
+ if (direction == Direction.LEFT) {
+ // This is for text that flows right to left
+ displayMode &= ~LCD_ENTRY_LEFT;
+ write(LCD_ENTRY_MODE_SET | displayMode);
+ } else {
+ // This is for text that flows left to right
+ displayMode |= LCD_ENTRY_LEFT;
+ write(LCD_ENTRY_MODE_SET | displayMode);
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#setAutoScrollEnabled(boolean)
+ */
+ @Override
+ public synchronized void setAutoScrollEnabled(boolean enable) throws IOException {
+ if (enable) {
+ // This will 'right justify' text from the cursor
+ displayMode |= LCD_ENTRY_SHIFT_INCREMENT;
+ write(LCD_ENTRY_MODE_SET | displayMode);
+ } else {
+ // This will 'left justify' text from the cursor
+ displayMode &= ~LCD_ENTRY_SHIFT_INCREMENT;
+ write(LCD_ENTRY_MODE_SET | displayMode);
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#isAutoScrollEnabled()
+ */
+ @Override
+ public synchronized boolean isAutoScrollEnabled() {
+ return (displayControl & LCD_ENTRY_SHIFT_INCREMENT) > 0;
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#isButtonPressed(se.hirt.pi.adafruitlcd.
+ * Button)
+ */
+ @Override
+ public synchronized boolean isButtonPressed(Button button) throws IOException {
+ return ((read(MCP23017_GPIOA) >> button.getPin()) & 1) > 0;
+ }
+
+ private synchronized int read(int bank) throws IOException {
// return i2CConfig.read(bank);
- return i2C.readRegister(bank);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see se.hirt.pi.adafruitlcd.ILCD#buttonsPressedBitmask()
- */
- @Override
- public synchronized int buttonsPressedBitmask() throws IOException {
- return read(MCP23017_GPIOA) & 0x1F;
- }
-
- @Override
- public synchronized Color getBacklight() throws IOException {
- // Should probably read the registers instead of caching...
- return color;
- }
-
- @Override
- public synchronized void reset() throws IOException {
- initialize();
- }
-
- @Override
- public void createChar(int location, byte[] pattern) throws IOException {
- if (location < 0 || location > 7) {
- throw new IllegalArgumentException(
- "Location should be between 0 and 7, value supplied is invalid: " + location);
- }
- if (pattern.length != 8) {
- throw new IllegalArgumentException(
- "Pattern length should be 8, array supplied has invalid length: " + pattern.length);
- }
-
- // Send ccgram update command
- location &= 0x7; // Only position 0..7 are allowed
- int command = LCD_SETCGRAMADDR | (location << 3);
- write(command);
-
- // Send custom character definition
- internalWrite(new String(pattern));
- }
-
- private final static String pad(String inputString) {
- // NOTE(Marcus/Aug 30, 2017): The VRAM IIRC is 40, but I'm
- // just assuming that people will not use this with scroll (need to
- // write less). Change if this makes someone unhappy. We could also just
- // clear before writing, but then we get flicker if we, for example,
- // want to write the same characters on a certain line.
- for (int i = inputString.length(); i < 16; i++) {
- inputString += " ";
- }
- return inputString;
- }
+ return i2C.readRegister(bank);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see se.hirt.pi.adafruitlcd.ILCD#buttonsPressedBitmask()
+ */
+ @Override
+ public synchronized int buttonsPressedBitmask() throws IOException {
+ return read(MCP23017_GPIOA) & 0x1F;
+ }
+
+ @Override
+ public synchronized Color getBacklight() throws IOException {
+ // Should probably read the registers instead of caching...
+ return color;
+ }
+
+ @Override
+ public synchronized void reset() throws IOException {
+ initialize();
+ }
+
+ @Override
+ public void createChar(int location, byte[] pattern) throws IOException {
+ if (location < 0 || location > 7) {
+ throw new IllegalArgumentException(
+ "Location should be between 0 and 7, value supplied is invalid: " + location);
+ }
+ if (pattern.length != 8) {
+ throw new IllegalArgumentException(
+ "Pattern length should be 8, array supplied has invalid length: " + pattern.length);
+ }
+
+ // Send ccgram update command
+ location &= 0x7; // Only position 0..7 are allowed
+ int command = LCD_SET_CGRAMADDR | (location << 3);
+ write(command);
+
+ // Send custom character definition
+ internalWrite(new String(pattern));
+ }
+
+ private static String pad(String inputString) {
+ // NOTE(Marcus/Aug 30, 2017): The VRAM IIRC is 40, but I'm
+ // just assuming that people will not use this with scroll (need to
+ // write less). Change if this makes someone unhappy. We could also just
+ // clear before writing, but then we get flicker if we, for example,
+ // want to write the same characters on a certain line.
+ StringBuilder inputStringBuilder = new StringBuilder(inputString);
+ inputStringBuilder.append(STRING_SPACE.repeat(Math.max(0, 16 - inputStringBuilder.length())));
+ inputString = inputStringBuilder.toString();
+ return inputString;
+ }
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/mockup/AdafruitLcdMockup.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/mockup/AdafruitLcdMockup.java
index 56ab88ce..879a5c47 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/mockup/AdafruitLcdMockup.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitlcd/mockup/AdafruitLcdMockup.java
@@ -16,20 +16,6 @@
*/
package com.robo4j.hw.rpi.i2c.adafruitlcd.mockup;
-import java.awt.BorderLayout;
-import java.awt.Component;
-import java.awt.event.MouseAdapter;
-import java.awt.event.MouseEvent;
-import java.io.IOException;
-import java.util.Arrays;
-
-import javax.swing.JButton;
-import javax.swing.JFrame;
-import javax.swing.JPanel;
-import javax.swing.JTextArea;
-import javax.swing.SwingUtilities;
-import javax.swing.WindowConstants;
-
import com.robo4j.hw.rpi.i2c.adafruitlcd.AdafruitLcd;
import com.robo4j.hw.rpi.i2c.adafruitlcd.Button;
import com.robo4j.hw.rpi.i2c.adafruitlcd.ButtonListener;
@@ -38,6 +24,15 @@
import com.robo4j.hw.rpi.i2c.adafruitlcd.impl.AdafruitLcdImpl.Direction;
import com.robo4j.hw.rpi.utils.I2cBus;
+import javax.swing.*;
+import java.awt.*;
+import java.awt.event.MouseAdapter;
+import java.awt.event.MouseEvent;
+import java.io.IOException;
+import java.util.Arrays;
+
+import static com.robo4j.hw.rpi.lcd.StringUtils.STRING_SPACE;
+
// TODO maybe move to examples
/**
* Swing mockup for the LCD.
@@ -190,12 +185,12 @@ private static String goLeft(String data) {
}
private static String goRight(String data) {
- data = " " + data;
+ data = STRING_SPACE + data;
int nlIndex = data.indexOf("\n");
if (nlIndex == -1) {
return data;
}
- return data.substring(0, nlIndex) + " " + data.substring(nlIndex + 1);
+ return data.substring(0, nlIndex) + STRING_SPACE + data.substring(nlIndex + 1);
}
public void setTextFlowDirection(Direction direction) {
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java
index f169c84c..2f1a4c58 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/adafruitoled/SSD1306Device.java
@@ -124,7 +124,7 @@ public byte getCommandValue() {
private enum MemoryModes {
HORIZONTAL((byte) 0), VERTICAL((byte) 1), PAGE((byte) 2);
- private byte value;
+ private final byte value;
MemoryModes(byte value) {
this.value = value;
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java
index 552dfda5..0a8657e0 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/TitanX1GPS.java
@@ -34,6 +34,8 @@
*/
public class TitanX1GPS implements GPS {
private static final Logger LOGGER = LoggerFactory.getLogger(TitanX1GPS.class);
+ // TODO : consider configurable
+ private static final int TIMEOUT_MILLS = 10;
private static final long READ_INTERVAL = 1000;
private final XA1110Device device;
@@ -146,8 +148,13 @@ private void notifyVelocity(VelocityEvent event) {
private void awaitTermination() {
try {
- internalExecutor.awaitTermination(10, TimeUnit.MILLISECONDS);
- internalExecutor.shutdown();
+ var terminated = internalExecutor.awaitTermination(TIMEOUT_MILLS, TimeUnit.MILLISECONDS);
+ if(terminated){
+ internalExecutor.shutdown();
+ } else {
+ LOGGER.warn("unit not terminated properly");
+ internalExecutor.shutdownNow();
+ }
} catch (InterruptedException e) {
LOGGER.error("awaitTermination", e);
// Don't care if we were interrupted.
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110PositionEvent.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110PositionEvent.java
index a6954361..6da76d06 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110PositionEvent.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110PositionEvent.java
@@ -137,11 +137,11 @@ public static boolean isAcceptedLine(String nmeaMessage) {
if (nmeaMessage == null) {
return false;
}
- for (int i = 0; i < ACCEPTED_MESSAGES.length; i++) {
- if (nmeaMessage.startsWith(ACCEPTED_MESSAGES[i])) {
- return true;
- }
- }
+ for (String acceptedMessage : ACCEPTED_MESSAGES) {
+ if (nmeaMessage.startsWith(acceptedMessage)) {
+ return true;
+ }
+ }
return false;
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110VelocityEvent.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110VelocityEvent.java
index 5da4cb32..20a7a1ce 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110VelocityEvent.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gps/XA1110VelocityEvent.java
@@ -61,11 +61,11 @@ public static boolean isAcceptedLine(String nmeaMessage) {
if (nmeaMessage == null) {
return false;
}
- for (int i = 0; i < ACCEPTED_MESSAGES.length; i++) {
- if (nmeaMessage.startsWith(ACCEPTED_MESSAGES[i])) {
- return true;
- }
- }
+ for (String acceptedMessage : ACCEPTED_MESSAGES) {
+ if (nmeaMessage.startsWith(acceptedMessage)) {
+ return true;
+ }
+ }
return false;
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java
index 20440cc8..c19c90e2 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/gyro/CalibratedGyro.java
@@ -46,21 +46,21 @@ public CalibratedGyro(ReadableDevice device, Tuple3f offsets, Tuple3f m
*/
public void calibrate() throws IOException {
setCalibration(new Tuple3f(), Tuple3f.createIdentity());
- float[] xvalues = new float[NUMBER_OF_CALIBRATION_READINGS];
- float[] yvalues = new float[NUMBER_OF_CALIBRATION_READINGS];
- float[] zvalues = new float[NUMBER_OF_CALIBRATION_READINGS];
+ float[] xValues = new float[NUMBER_OF_CALIBRATION_READINGS];
+ float[] yValues = new float[NUMBER_OF_CALIBRATION_READINGS];
+ float[] zValues = new float[NUMBER_OF_CALIBRATION_READINGS];
for (int i = 0; i < NUMBER_OF_CALIBRATION_READINGS; i++) {
Tuple3f tmp = read();
- xvalues[i] = tmp.x;
- yvalues[i] = tmp.y;
- zvalues[i] = tmp.z;
+ xValues[i] = tmp.x;
+ yValues[i] = tmp.y;
+ zValues[i] = tmp.z;
sleep(20);
}
Tuple3f calibration = new Tuple3f();
- calibration.x = calibrate(xvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
- calibration.y = calibrate(yvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
- calibration.z = calibrate(zvalues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
+ calibration.x = calibrate(xValues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
+ calibration.y = calibrate(yValues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
+ calibration.z = calibrate(zValues, NUMBER_OF_CALIBRATION_READINGS_TO_DROP);
LOGGER.info("calibrate:{}", RANGE_MULTIPLIERS);
setCalibration(calibration, RANGE_MULTIPLIERS);
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/HBridgeMC33926Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/HBridgeMC33926Device.java
index 0e168d59..4c1e101f 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/HBridgeMC33926Device.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/HBridgeMC33926Device.java
@@ -36,8 +36,6 @@ public class HBridgeMC33926Device implements Motor {
private final String name;
private final PWMChannel channel;
private final boolean invert;
- // private final GpioPinDigitalOutput in1;
-// private final GpioPinDigitalOutput in2;
private final DigitalOutput gpioOut1;
private final DigitalOutput gpioOut2;
@@ -58,9 +56,6 @@ public HBridgeMC33926Device(String name, PWMChannel channel, GpioPin pin1, GpioP
var gpioConfig1 = digitalOutputBuilder.address(pin1.address()).onState(DigitalState.LOW).build();
var gpioConfig2 = digitalOutputBuilder.address(pin2.address()).onState(DigitalState.HIGH).build();
-// GpioController gpio = GpioFactory.getInstance();
-// this.in1 = gpio.provisionDigitalOutputPin(in1, "IN1", PinState.LOW);
-// this.in2 = gpio.provisionDigitalOutputPin(in2, "IN2", PinState.HIGH);
gpioOut1 = pi4jRpiContext.dout().create(gpioConfig1);
gpioOut2 = pi4jRpiContext.dout().create(gpioConfig2);
setDirection(Direction.FORWARD);
@@ -110,13 +105,9 @@ private void setDirection(Direction direction) {
}
if (forward) {
-// in1.setState(PinState.HIGH);
-// in2.setState(PinState.LOW);
gpioOut1.setState(digitalStateToByte(DigitalState.HIGH));
gpioOut2.setState(digitalStateToByte(DigitalState.LOW));
} else {
-// in1.setState(PinState.LOW);
-// in2.setState(PinState.HIGH);
gpioOut1.setState(digitalStateToByte(DigitalState.LOW));
gpioOut2.setState(digitalStateToByte(DigitalState.HIGH));
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Servo.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Servo.java
index 685fbbc0..5907360b 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Servo.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PCA9685Servo.java
@@ -154,7 +154,7 @@ public boolean isInverted() {
@Override
public String toString() {
- return String.format("Servo on ch %d [min:%d, max:%d, invert:%s, trim:%f, dualrate:%f, expo:%f]",
+ return String.format("Servo on ch %s [min:%d, max:%d, invert:%s, trim:%f, dualrate:%f, expo:%f]",
channel.getChannelID(), min, max, invert, trim, dualRate, expo);
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PWMPCA9685Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PWMPCA9685Device.java
index 58019b52..21a74776 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PWMPCA9685Device.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/i2c/pwm/PWMPCA9685Device.java
@@ -16,238 +16,222 @@
*/
package com.robo4j.hw.rpi.i2c.pwm;
-import java.io.IOException;
-
import com.robo4j.hw.rpi.i2c.AbstractI2CDevice;
import com.robo4j.hw.rpi.utils.I2cBus;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+import java.io.IOException;
/**
* Abstraction for talking to a PCA9685 PWM/Servo driver. For example an
* Adafruit 16 channel I2C PWM driver breakout board.
- *
+ *
* @author Marcus Hirt (@hirt)
* @author Miro Wengner (@miragemiko)
*/
// Not using all commands - yet.
@SuppressWarnings("unused")
public class PWMPCA9685Device extends AbstractI2CDevice {
- private static final int DEFAULT_I2C_ADDRESS = 0x40;
- private static final int DEFAULT_FREQUENCY = 50;
-
- private static final double PRESCALE_FACTOR = 25000000.0 / 4096.0;
-
- private static final int MODE1 = 0x00;
- private static final int MODE2 = 0x01;
- private static final int SUBADR1 = 0x02;
- private static final int SUBADR2 = 0x03;
- private static final int SUBADR13 = 0x04;
- private static final int PRESCALE = 0xFE;
- private static final int LED0_ON_L = 0x06;
- private static final int LED0_ON_H = 0x07;
- private static final int LED0_OFF_L = 0x08;
- private static final int LED0_OFF_H = 0x09;
- private static final int ALL_LED_ON_L = 0xFA;
- private static final int ALL_LED_ON_H = 0xFB;
- private static final int ALL_LED_OFF_L = 0xFC;
- private static final int ALL_LED_OFF_H = 0xFD;
-
- private static final int RESTART = 0x80;
- private static final int SLEEP = 0x10;
- private static final int ALLCALL = 0x01;
- private static final int INVRT = 0x10;
- private static final int OUTDRV = 0x04;
-
- private double frequency = Double.NaN;
-
- /**
- * Constructs a PWM device using the default settings. (I2CBUS.BUS_1, 0x40)
- *
- * @throws IOException
- * if there was communication problem
- */
- public PWMPCA9685Device() throws IOException {
- // 0x40 is the default address used by the AdaFruit PWM board.
- //this(I2CBus.BUS_1, DEFAULT_I2C_ADDRESS);
- this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS);
- }
-
- /**
- * Creates a software interface to an Adafruit 16 channel I2C PWM driver
- * board (PCA9685).
- *
- * @param bus
- * the I2C bus to use.
- * @param address
- * the address to use.
- *
- * @see I2cBus
- *
- * @throws IOException
- * if there was communication problem
- */
- public PWMPCA9685Device(I2cBus bus, int address) throws IOException {
- super(bus, address);
- initialize();
- }
-
- /**
- * Sets all PWM channels to the provided settings.
- *
- * @param on
- * when to turn on the signal [0, 4095]
- * @param off
- * when to turn off the signal [0, 4095]
- *
- * @throws IOException
- * if there was a problem communicating with the device.
- */
- public void setAllPWM(int on, int off) throws IOException {
- writeByte(ALL_LED_ON_L, (byte) (on & 0xFF));
- writeByte(ALL_LED_ON_H, (byte) (on >> 8));
- writeByte(ALL_LED_OFF_L, (byte) (off & 0xFF));
- writeByte(ALL_LED_OFF_H, (byte) (off >> 8));
- }
-
- /**
- * Sets the PWM frequency to use. This is common across all channels. For
- * controlling RC servos, 50Hz is a good starting point.
- *
- * @param frequency
- * the PWM frequency to use, in Hz.
- * @throws IOException
- * if a problem occurred accessing the device.
- */
- public void setPWMFrequency(double frequency) throws IOException {
- double prescaleval = PRESCALE_FACTOR / frequency;
- prescaleval -= 1.0;
- double prescale = Math.floor(prescaleval + 0.5);
- //int oldmode = i2CConfig.read(MODE1);
- int oldmode = readByte(MODE1);
- int newmode = (oldmode & 0x7F) | 0x10;
- writeByte(MODE1, (byte) newmode);
- writeByte(PRESCALE, (byte) (Math.floor(prescale)));
- writeByte(MODE1, (byte) oldmode);
- sleep(50);
- writeByte(MODE1, (byte) (oldmode | 0x80));
- this.frequency = frequency;
- }
-
- /**
- * @return the PWM frequency set, or Double.NaN if no frequency have been
- * explicitly set.
- */
- public double getPWMFrequency() {
- return frequency;
- }
-
- /**
- * Returns one of the PWM channels on the device. Allowed range is [0, 15].
- *
- * @param channel
- * the channel to retrieve.
- *
- * @return the specified PWM channel.
- */
- public PWMChannel getChannel(int channel) {
- return new PWMChannel(channel);
- }
-
- /**
- * Use to control a PWM channel on the PWM device.
- *
- * @see PWMPCA9685Device#getChannel(int)
- */
- public class PWMChannel {
- private final int channel;
-
- private PWMChannel(int channel) {
- if (channel < 0 || channel > 15) {
- throw new IllegalArgumentException("There is no channel " + channel + " on the board.");
- }
- this.channel = channel;
- }
-
- /**
- * Configures the PWM pulse for the PWMChannel.
- *
- * @param on
- * when to go from low to high [0, 4095]. 0 means at the very
- * start of the pulse, 4095 at the very end.
- * @param off
- * when to go from high to low [0, 4095]. 0 means at the very
- * start of the pulse, 4095 at the very end.
- *
- * @throws IOException
- * exception
- */
- public void setPWM(int on, int off) throws IOException {
+ private static final Logger LOGGER = LoggerFactory.getLogger(PWMPCA9685Device.class);
+ private static final int DEFAULT_I2C_ADDRESS = 0x40;
+ private static final int DEFAULT_FREQUENCY = 50;
+
+ private static final double PRESCALE_FACTOR = 25000000.0 / 4096.0;
+
+ private static final int MODE1 = 0x00;
+ private static final int MODE2 = 0x01;
+ private static final int SUBADR1 = 0x02;
+ private static final int SUBADR2 = 0x03;
+ private static final int SUBADR13 = 0x04;
+ private static final int PRESCALE = 0xFE;
+ private static final int LED0_ON_L = 0x06;
+ private static final int LED0_ON_H = 0x07;
+ private static final int LED0_OFF_L = 0x08;
+ private static final int LED0_OFF_H = 0x09;
+ private static final int ALL_LED_ON_L = 0xFA;
+ private static final int ALL_LED_ON_H = 0xFB;
+ private static final int ALL_LED_OFF_L = 0xFC;
+ private static final int ALL_LED_OFF_H = 0xFD;
+
+ private static final int RESTART = 0x80;
+ private static final int SLEEP = 0x10;
+ private static final int ALLCALL = 0x01;
+ private static final int INVRT = 0x10;
+ private static final int OUTDRV = 0x04;
+
+ private double frequency = Double.NaN;
+
+ /**
+ * Constructs a PWM device using the default settings. (I2CBUS.BUS_1, 0x40)
+ *
+ * @throws IOException if there was communication problem
+ */
+ public PWMPCA9685Device() throws IOException {
+ // 0x40 is the default address used by the AdaFruit PWM board.
+ //this(I2CBus.BUS_1, DEFAULT_I2C_ADDRESS);
+ this(I2cBus.BUS_1, DEFAULT_I2C_ADDRESS);
+ }
+
+ /**
+ * Creates a software interface to an Adafruit 16 channel I2C PWM driver
+ * board (PCA9685).
+ *
+ * @param bus the I2C bus to use.
+ * @param address the address to use.
+ * @throws IOException if there was communication problem
+ * @see I2cBus
+ */
+ public PWMPCA9685Device(I2cBus bus, int address) throws IOException {
+ super(bus, address);
+ initialize();
+ }
+
+ /**
+ * Sets all PWM channels to the provided settings.
+ *
+ * @param on when to turn on the signal [0, 4095]
+ * @param off when to turn off the signal [0, 4095]
+ * @throws IOException if there was a problem communicating with the device.
+ */
+ public void setAllPWM(int on, int off) throws IOException {
+ writeByte(ALL_LED_ON_L, (byte) (on & 0xFF));
+ writeByte(ALL_LED_ON_H, (byte) (on >> 8));
+ writeByte(ALL_LED_OFF_L, (byte) (off & 0xFF));
+ writeByte(ALL_LED_OFF_H, (byte) (off >> 8));
+ }
+
+ /**
+ * Sets the PWM frequency to use. This is common across all channels. For
+ * controlling RC servos, 50Hz is a good starting point.
+ *
+ * @param frequency the PWM frequency to use, in Hz.
+ * @throws IOException if a problem occurred accessing the device.
+ */
+ public void setPWMFrequency(double frequency) throws IOException {
+ double prescaleval = PRESCALE_FACTOR / frequency;
+ prescaleval -= 1.0;
+ double prescale = Math.floor(prescaleval + 0.5);
+ //int oldmode = i2CConfig.read(MODE1);
+ int oldmode = readByte(MODE1);
+ int newmode = (oldmode & 0x7F) | 0x10;
+ writeByte(MODE1, (byte) newmode);
+ writeByte(PRESCALE, (byte) (Math.floor(prescale)));
+ writeByte(MODE1, (byte) oldmode);
+ sleep(50);
+ writeByte(MODE1, (byte) (oldmode | 0x80));
+ this.frequency = frequency;
+ }
+
+ /**
+ * @return the PWM frequency set, or Double.NaN if no frequency have been
+ * explicitly set.
+ */
+ public double getPWMFrequency() {
+ return frequency;
+ }
+
+ /**
+ * Returns one of the PWM channels on the device. Allowed range is [0, 15].
+ *
+ * @param channel the channel to retrieve.
+ * @return the specified PWM channel.
+ */
+ public PWMChannel getChannel(int channel) {
+ return new PWMChannel(channel);
+ }
+
+ /**
+ * Use to control a PWM channel on the PWM device.
+ *
+ * @see PWMPCA9685Device#getChannel(int)
+ */
+ public class PWMChannel {
+ private final int channel;
+
+ private PWMChannel(int channel) {
+ if (channel < 0 || channel > 15) {
+ throw new IllegalArgumentException("There is no channel " + channel + " on the board.");
+ }
+ this.channel = channel;
+ }
+
+ /**
+ * Configures the PWM pulse for the PWMChannel.
+ *
+ * @param on when to go from low to high [0, 4095]. 0 means at the very
+ * start of the pulse, 4095 at the very end.
+ * @param off when to go from high to low [0, 4095]. 0 means at the very
+ * start of the pulse, 4095 at the very end.
+ * @throws IOException exception
+ */
+ public void setPWM(int on, int off) throws IOException {
// i2CConfig.write(LED0_ON_L + 4 * channel, (byte) (on & 0xFF));
// i2CConfig.write(LED0_ON_H + 4 * channel, (byte) (on >> 8));
// i2CConfig.write(LED0_OFF_L + 4 * channel, (byte) (off & 0xFF));
// i2CConfig.write(LED0_OFF_H + 4 * channel, (byte) (off >> 8));
- writeByte(LED0_ON_L + 4 * channel, (byte) (on & 0xFF));
- writeByte(LED0_ON_H + 4 * channel, (byte) (on >> 8));
- writeByte(LED0_OFF_L + 4 * channel, (byte) (off & 0xFF));
- writeByte(LED0_OFF_H + 4 * channel, (byte) (off >> 8));
- }
-
- /**
- * @return the PWM device that this channel is associated with.
- */
- public PWMPCA9685Device getPWMDevice() {
- return PWMPCA9685Device.this;
- }
-
- /**
- * @return the channel id used by this channel.
- */
- public Object getChannelID() {
- return channel;
- }
- }
-
- private void sleep(int millis) {
- try {
- Thread.sleep(millis);
- } catch (InterruptedException e) {
- // Don't care
- }
- }
-
- private void initialize() throws IOException {
- setAllPWM(0, 0);
- writeByte(MODE2, (byte) OUTDRV);
- writeByte(MODE1, (byte) ALLCALL);
- sleep(50);
- int mode1 = readByte(MODE1);
- mode1 = mode1 & ~SLEEP;
- writeByte(MODE1, (byte) mode1);
- sleep(50);
- }
-
- /**
- * Creates a {@link PWMPCA9685Device}, or returns null if unsuccessful.
- * Meant to be used in lambdas.
- *
- * @param bus
- * the bus
- * @param address
- * the address
- * @return the device if it all worked out, null if it failed.
- */
- public static PWMPCA9685Device createDevice(I2cBus bus, int address) {
- return createDevice(bus, address, DEFAULT_FREQUENCY);
- }
-
- public static PWMPCA9685Device createDevice(I2cBus bus, int address, int frequency) {
- try {
- PWMPCA9685Device result = new PWMPCA9685Device(bus, address);
- result.setPWMFrequency(frequency);
- return result;
- } catch (IOException e) {
- e.printStackTrace();
- return null;
- }
- }
+ writeByte(LED0_ON_L + 4 * channel, (byte) (on & 0xFF));
+ writeByte(LED0_ON_H + 4 * channel, (byte) (on >> 8));
+ writeByte(LED0_OFF_L + 4 * channel, (byte) (off & 0xFF));
+ writeByte(LED0_OFF_H + 4 * channel, (byte) (off >> 8));
+ }
+
+ /**
+ * @return the PWM device that this channel is associated with.
+ */
+ public PWMPCA9685Device getPWMDevice() {
+ return PWMPCA9685Device.this;
+ }
+
+ /**
+ * @return the channel id used by this channel.
+ */
+ public Object getChannelID() {
+ return channel;
+ }
+ }
+
+ private void sleep(int millis) {
+ try {
+ Thread.sleep(millis);
+ } catch (InterruptedException e) {
+ // Don't care
+ }
+ }
+
+ private void initialize() throws IOException {
+ setAllPWM(0, 0);
+ writeByte(MODE2, (byte) OUTDRV);
+ writeByte(MODE1, (byte) ALLCALL);
+ sleep(50);
+ int mode1 = readByte(MODE1);
+ mode1 = mode1 & ~SLEEP;
+ writeByte(MODE1, (byte) mode1);
+ sleep(50);
+ }
+
+ /**
+ * Creates a {@link PWMPCA9685Device}, or returns null if unsuccessful.
+ * Meant to be used in lambdas.
+ *
+ * @param bus the bus
+ * @param address the address
+ * @return the device if it all worked out, null if it failed.
+ */
+ public static PWMPCA9685Device createDevice(I2cBus bus, int address) {
+ return createDevice(bus, address, DEFAULT_FREQUENCY);
+ }
+
+ public static PWMPCA9685Device createDevice(I2cBus bus, int address, int frequency) {
+ try {
+ PWMPCA9685Device result = new PWMPCA9685Device(bus, address);
+ result.setPWMFrequency(frequency);
+ return result;
+ } catch (IOException e) {
+ LOGGER.error("createDevice:{}", e.getMessage(), e);
+ // TODO : create empty object design and remove null
+ return null;
+ }
+ }
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055CalibrationStatus.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055CalibrationStatus.java
index c21a482f..93a3c670 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055CalibrationStatus.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055CalibrationStatus.java
@@ -30,7 +30,7 @@ public enum CalibrationStatus {
private final int statusValue;
- private CalibrationStatus(int statusValue) {
+ CalibrationStatus(int statusValue) {
this.statusValue = statusValue;
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055Device.java
index 1761f4a7..29ba77cc 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055Device.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno055Device.java
@@ -233,7 +233,7 @@ public enum Unit {
* Temperature unit (Fahrenheit)
*/
FAHRENHEIT(2f);
- private float factor;
+ private final float factor;
Unit(float factor) {
this.factor = factor;
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Device.java
index a8cfd4f1..6b7221ea 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Device.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Device.java
@@ -50,7 +50,7 @@ public interface Bno080Device {
* the kind of data to start listening for.
* @param reportPeriod
* report period in ms.
- * @return
+ * @return boolean status
*/
boolean start(SensorReportId report, int reportPeriod);
@@ -68,7 +68,7 @@ public interface Bno080Device {
/**
* Do calibration cycle. Will block until calibration results are good
- * enough, or the time out is reached.
+ * enough, or the timeout is reached. unit milliseconds
*/
void calibrate(long timeout);
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Factory.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Factory.java
index 4f3e37b5..3d34bc58 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Factory.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/Bno080Factory.java
@@ -16,14 +16,14 @@
*/
package com.robo4j.hw.rpi.imu.bno;
-import java.io.IOException;
-
import com.pi4j.io.spi.SpiChipSelect;
import com.pi4j.io.spi.SpiMode;
import com.robo4j.hw.rpi.imu.bno.Bno055Device.OperatingMode;
import com.robo4j.hw.rpi.imu.bno.impl.Bno080SPIDevice;
import com.robo4j.hw.rpi.utils.GpioPin;
+import java.io.IOException;
+
/**
* Factory for creating BNO080 devices
*
@@ -101,7 +101,7 @@ public static Bno080Device createDevice(String serialPort, OperatingMode operati
* @return an SPI connected BNO80 device.
* @throws IOException
* exception
- * @throws InterruptedException
+ * @throws InterruptedException exception
*/
public static Bno080Device createDefaultSPIDevice() throws IOException, InterruptedException {
return new Bno080SPIDevice();
@@ -114,7 +114,7 @@ public static Bno080Device createDefaultSPIDevice() throws IOException, Interrup
* @return an SPI connected BNO80 device.
* @throws IOException
* exception
- * @throws InterruptedException
+ * @throws InterruptedException exception
*/
public static Bno080Device createDefaultSPIDevice(SpiChipSelect channel, SpiMode mode, int speed, GpioPin wake, GpioPin cs, GpioPin reset, GpioPin interrupt) throws IOException, InterruptedException {
return new Bno080SPIDevice(channel, mode, speed, wake, cs, reset, interrupt);
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/DataEvent3f.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/DataEvent3f.java
index 75eabe0d..d48e6224 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/DataEvent3f.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/DataEvent3f.java
@@ -17,6 +17,7 @@
package com.robo4j.hw.rpi.imu.bno;
+import java.io.Serial;
import java.io.Serializable;
import java.util.Objects;
@@ -29,6 +30,7 @@
* @author Miroslav Wengner (@miragemiko)
*/
public class DataEvent3f implements Serializable {
+ @Serial
private static final long serialVersionUID = 1L;
private final DataEventType type;
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/VectorEvent.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/VectorEvent.java
index 3e0cfb15..e0c3245a 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/VectorEvent.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/VectorEvent.java
@@ -19,6 +19,7 @@
import com.robo4j.math.geometry.Tuple3f;
+import java.io.Serial;
import java.util.EnumSet;
import java.util.Objects;
@@ -29,6 +30,7 @@
* @author Miroslav Wengner (@miragemiko)
*/
public class VectorEvent extends DataEvent3f {
+ @Serial
private static final long serialVersionUID = 1L;
private static final EnumSet ALLOWED = EnumSet.of(DataEventType.VECTOR_ROTATION, DataEventType.VECTOR_GAME);
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno055Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno055Device.java
index 269832a2..f0ac5256 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno055Device.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno055Device.java
@@ -22,457 +22,444 @@
import com.robo4j.hw.rpi.imu.bno.Bno055SystemStatus;
import com.robo4j.math.geometry.Tuple3f;
import com.robo4j.math.geometry.Tuple4d;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
import java.io.IOException;
/**
* Abstraction for a BN0055 absolute orientation device.
- *
+ *
* NOTE(Marcus/Jul 27, 2017): Note that the Raspberry Pi does not properly
* support clock stretching yet (Raspberry Pi 3), so this particular class does
* not work until that has been fixed in the i2c implementation on the Raspberry
* Pi. Until then, wire the BNO055 to use serial tty communication as described
* in that data sheet, and use the {@link Bno055SerialDevice} instead.
- *
+ *
* @author Marcus Hirt (@hirt)
* @author Miro Wengner (@miragemiko)
*/
public abstract class AbstractBno055Device implements Bno055Device {
- private static final int WAIT_STEP_MILLIS = 5;
- // Spec 3.6.5.5
- private static final float QUAT_SCALE = (float) (1.0 / (1 << 14));
- // Registers
- private static final int REGISTER_CHIP_ID = 0x00;
- private static final int REGISTER_PAGE_ID = 0x07;
- private static final int REGISTER_SYS_TRIGGER = 0x3F;
- private static final int REGISTER_PWR_MODE = 0x3E;
- private static final int REGISTER_OPR_MODE = 0x3D;
- private static final int REGISTER_UNIT_SELECT = 0x3B;
- private static final int REGISTER_ST_RESULT = 0x36;
- private static final int REGISTER_CALIB_STAT = 0x35;
- private static final int REGISTER_TEMP = 0x34;
-
- // Gravity vector
- private static final int REGISTER_GRV_DATA_X = 0x2E;
- // Linear acceleration
- private static final int REGISTER_LIA_DATA_X = 0x28;
- // Quaternion w, x, y, z
- private static final int REGISTER_QUA_DATA_W = 0x20;
- private static final int REGISTER_EUL_DATA_X = 0x1A;
- private static final int REGISTER_GYR_DATA_X = 0x14;
- private static final int REGISTER_MAG_DATA_X = 0x0E;
- private static final int REGISTER_ACC_DATA_X = 0x08;
-
- private static final int REGISTER_SYS_ERR = 0x3A;
- private static final int REGISTER_SYS_STATUS = 0x39;
-
- // The constant value of the chip id register for the BNO055
- private static final byte CHIP_ID_VALUE = (byte) 0xA0;
-
- // Caching these to minimize the I2C traffic. Assumes that noone else is
- // tinkering with the hardware.
- private Unit currentAccelerationUnit = Unit.M_PER_S_SQUARED;
- private Unit currentAngularRateUnit = Unit.DEGREES_PER_SECOND;
- private Unit currentEuelerAngleUnit = Unit.DEGREES;
- private Unit currentTemperatureUnit = Unit.CELCIUS;
- private OrientationMode currentOrientation = OrientationMode.Windows;
-
- /**
- * Creates a BNO055Device with the default settings.
- *
- * @throws IOException
- * exception
- *
- * @see PowerMode
- * @see OperatingMode
- */
- public AbstractBno055Device() throws IOException {
-
- }
-
- /*
- * (non-Javadoc)
- *
- * @see
- * com.robo4j.hw.rpi.i2c.imu.BNO055Device#setOperatingMode(com.robo4j.hw.rpi
- * .i2c.imu.BNO055I2CDevice.OperatingMode)
- */
- @Override
- public void setOperatingMode(OperatingMode operatingMode) throws IOException {
- write(REGISTER_OPR_MODE, operatingMode.getCtrlCode());
- waitForOk(20);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getOperatingMode()
- */
- @Override
- public OperatingMode getOperatingMode() throws IOException {
- return OperatingMode.fromCtrlCode(read(REGISTER_OPR_MODE) & 0x0F);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see
- * com.robo4j.hw.rpi.i2c.imu.BNO055Device#setPowerMode(com.robo4j.hw.rpi.i2c
- * .imu.BNO055I2CDevice.PowerMode)
- */
- @Override
- public void setPowerMode(PowerMode powerMode) throws IOException {
- write(REGISTER_PWR_MODE, powerMode.getCtrlCode());
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getPowerMode()
- */
- @Override
- public PowerMode getPowerMode() throws IOException {
- return PowerMode.fromCtrlCode(read(REGISTER_PWR_MODE));
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getCalibrationStatus()
- */
- @Override
- public Bno055CalibrationStatus getCalibrationStatus() throws IOException {
- return new Bno055CalibrationStatus(read(REGISTER_CALIB_STAT));
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getSystemStatus()
- */
- @Override
- public Bno055SystemStatus getSystemStatus() throws IOException {
- return new Bno055SystemStatus(read(REGISTER_SYS_STATUS));
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getTemperature()
- */
- @Override
- public float getTemperature() throws IOException {
- return read(REGISTER_TEMP) / currentTemperatureUnit.getFactor();
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#performSelfTest()
- */
- @Override
- public Bno055SelfTestResult performSelfTest() throws IOException {
- OperatingMode previousOperatingMode = getOperatingMode();
- if (previousOperatingMode != OperatingMode.CONFIG) {
- setOperatingMode(OperatingMode.CONFIG);
- sleep(20);
- }
- write(REGISTER_SYS_TRIGGER, (byte) 0x8);
- sleep(20);
- int resultCode = read(REGISTER_ST_RESULT) & 0x0F;
- int errorCode = 0;
- if (resultCode != 0x0F) {
- errorCode = read(REGISTER_SYS_ERR);
- }
- Bno055SelfTestResult result = new Bno055SelfTestResult(resultCode, errorCode);
- if (previousOperatingMode != OperatingMode.CONFIG) {
- setOperatingMode(previousOperatingMode);
- sleep(20);
- }
- return result;
- }
-
- /**
- * Reads a byte (signed or unsigned) from the provided register.
- *
- * @param register
- * the address of the register to read from.
- * @return the value read.
- * @throws IOException
- * exception
- */
- protected abstract int read(int register) throws IOException;
-
- /**
- * Writes the provided byte into the register address.
- *
- * @param register
- * the register to write to.
- * @param b
- * the byte to write.
- * @throws IOException
- * exception
- */
- protected abstract void write(int register, byte b) throws IOException;
-
- protected void sleep(long millis) {
- try {
- Thread.sleep(millis);
- } catch (InterruptedException e) {
- // TODO Auto-generated catch block
- e.printStackTrace();
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see
- * com.robo4j.hw.rpi.i2c.imu.BNO055Device#setUnits(com.robo4j.hw.rpi.i2c.imu
- * .BNO055I2CDevice.Unit, com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Unit,
- * com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Unit,
- * com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Unit,
- * com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Orientation)
- */
- @Override
- public void setUnits(Unit accelerationUnit, Unit angularRateUnit, Unit angleUnit, Unit temperatureUnit, OrientationMode orientationMode)
- throws IOException {
- int val = 0;
- if (accelerationUnit == Unit.MILI_G) {
- val |= 0x01;
- } else if (accelerationUnit != Unit.MICROTESLA) {
- throw new IllegalArgumentException(accelerationUnit + " is not an acceleration unit!");
- }
- if (angularRateUnit == Unit.RADIANS_PER_SECOND) {
- val |= 0x02;
- } else if (angularRateUnit != Unit.DEGREES_PER_SECOND) {
- throw new IllegalArgumentException(angularRateUnit + " is not an angular rate unit!");
- }
- if (angleUnit == Unit.RADIANS) {
- val |= 0x04;
- } else if (angleUnit != Unit.DEGREES) {
- throw new IllegalArgumentException(angleUnit + " is not an angle unit!");
- }
- if (temperatureUnit == Unit.FAHRENHEIT) {
- val |= 0x10;
- } else if (temperatureUnit != Unit.CELCIUS) {
- throw new IllegalArgumentException(temperatureUnit + " is not a temperature unit!");
- }
- if (orientationMode == OrientationMode.Android) {
- val |= 0x80;
- }
- write(REGISTER_UNIT_SELECT, (byte) val);
- currentAccelerationUnit = accelerationUnit;
- currentAngularRateUnit = angularRateUnit;
- currentEuelerAngleUnit = angleUnit;
- currentTemperatureUnit = temperatureUnit;
- currentOrientation = orientationMode;
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#read()
- */
- @Override
- public Tuple3f read() throws IOException {
- return readVector(REGISTER_EUL_DATA_X, currentEuelerAngleUnit);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readEulerAngles()
- */
- @Override
- public Tuple3f readEulerAngles() throws IOException {
- return read();
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readMagnetometer()
- */
- @Override
- public Tuple3f readMagnetometer() throws IOException {
- return readVector(REGISTER_MAG_DATA_X, Unit.MICROTESLA);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readAccelerometer()
- */
- @Override
- public Tuple3f readAccelerometer() throws IOException {
- return readVector(REGISTER_ACC_DATA_X, currentAccelerationUnit);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readGyro()
- */
- @Override
- public Tuple3f readGyro() throws IOException {
- return readVector(REGISTER_GYR_DATA_X, currentAngularRateUnit);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readQuaternion()
- */
- @Override
- public Tuple4d readQuaternion() throws IOException {
- return readQuaternion(REGISTER_QUA_DATA_W);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readLinearAcceleration()
- */
- @Override
- public Tuple3f readLinearAcceleration() throws IOException {
- return readVector(REGISTER_LIA_DATA_X, currentAccelerationUnit);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readGravityVector()
- */
- @Override
- public Tuple3f readGravityVector() throws IOException {
- return readVector(REGISTER_GRV_DATA_X, currentAccelerationUnit);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#reset()
- */
- @Override
- public void reset() throws IOException {
- write(REGISTER_SYS_TRIGGER, (byte) 0x20);
- sleep(650);
- }
-
- /*
- * (non-Javadoc)
- *
- * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getCurrentOrientation()
- */
- @Override
- public OrientationMode getCurrentOrientation() {
- return currentOrientation;
- }
-
- /**
- * Will wait until the system is ready, an error occurs or the max wait was
- * exceeded.
- *
- * @param maxWaitTimeMillis
- * max time to wait for the system to be ready.
- * @return returns true if the system is ready, false if the system is
- * reporting an error or if the timeout was reached.
- * @throws IOException
- */
- private boolean waitForOk(int maxWaitTimeMillis) throws IOException {
- int waitTime = 0;
- while (true) {
- Bno055SystemStatus systemStatus = getSystemStatus();
- if (systemStatus.isReady() || systemStatus.hasError() || waitTime >= maxWaitTimeMillis) {
- if (systemStatus.isReady()) {
- return true;
- } else {
- return false;
- }
- }
- sleep(WAIT_STEP_MILLIS);
- waitTime += WAIT_STEP_MILLIS;
- }
- }
-
- protected void initialize(OperatingMode operatingMode) throws IOException {
- // First check that we are really communicating with the BNO.
- if (read(REGISTER_CHIP_ID) != CHIP_ID_VALUE) {
- throw new IOException("Not a BNO connected to the defined endpoint!");
- }
-
- try {
- write(REGISTER_PAGE_ID, (byte) 0);
- } catch (IOException ioe) {
- // Seems sometimes the first one fails, so just ignore.
- }
- write(REGISTER_PAGE_ID, (byte) 0);
- // This may be a bit unnecessary, but let's make sure we are in config
- // first.
- OperatingMode currentOperatingMode = getOperatingMode();
- if (currentOperatingMode != OperatingMode.CONFIG) {
- setOperatingMode(OperatingMode.CONFIG);
- waitForOk(20);
- }
- setUseExternalChrystal(true);
- if (currentOperatingMode != operatingMode) {
- setOperatingMode(operatingMode);
- }
- }
-
- @Override
- public void setUseExternalChrystal(boolean useExternalChrystal) throws IOException {
- write(REGISTER_SYS_TRIGGER, useExternalChrystal ? (byte) 0x80 : 0x00);
- }
-
- @Override
- public boolean isUseExternalChrystal() throws IOException {
- return (read(REGISTER_SYS_TRIGGER) & 0x80) > 0;
- }
-
- private Tuple3f readVector(int register, Unit unit) throws IOException {
- byte[] data = read(register, 6);
- Tuple3f tuple = new Tuple3f();
- tuple.x = read16bitSigned(data, 0);
- tuple.y = read16bitSigned(data, 2);
- tuple.z = read16bitSigned(data, 4);
- if (unit.getFactor() != 1f) {
- tuple.x /= unit.getFactor();
- tuple.y /= unit.getFactor();
- tuple.z /= unit.getFactor();
- }
- return tuple;
- }
-
- /**
- * Reads n values from the provided register.
- *
- * @param register
- * the register to read
- * @param length
- * the total length of values to read.
- * @return bytes
- * @throws IOException
- * exception
- */
- protected abstract byte[] read(int register, int length) throws IOException;
-
- protected Tuple4d readQuaternion(int register) throws IOException {
- byte[] data = read(register, 8);
- Tuple4d tuple = new Tuple4d();
- tuple.t = read16bitSigned(data, 0);
- tuple.x = read16bitSigned(data, 2);
- tuple.y = read16bitSigned(data, 4);
- tuple.z = read16bitSigned(data, 6);
- tuple.t *= QUAT_SCALE;
- tuple.x *= QUAT_SCALE;
- tuple.y *= QUAT_SCALE;
- tuple.z *= QUAT_SCALE;
- return tuple;
- }
-
- private short read16bitSigned(byte[] data, int offset) {
- return (short) (data[offset + 1] << 8 | (data[offset] & 0xFF));
- }
+ private static final Logger LOGGER = LoggerFactory.getLogger(AbstractBno055Device.class);
+ private static final int WAIT_STEP_MILLIS = 5;
+ // Spec 3.6.5.5
+ private static final float QUAT_SCALE = (float) (1.0 / (1 << 14));
+ // Registers
+ private static final int REGISTER_CHIP_ID = 0x00;
+ private static final int REGISTER_PAGE_ID = 0x07;
+ private static final int REGISTER_SYS_TRIGGER = 0x3F;
+ private static final int REGISTER_PWR_MODE = 0x3E;
+ private static final int REGISTER_OPR_MODE = 0x3D;
+ private static final int REGISTER_UNIT_SELECT = 0x3B;
+ private static final int REGISTER_ST_RESULT = 0x36;
+ private static final int REGISTER_CALIB_STAT = 0x35;
+ private static final int REGISTER_TEMP = 0x34;
+
+ // Gravity vector
+ private static final int REGISTER_GRV_DATA_X = 0x2E;
+ // Linear acceleration
+ private static final int REGISTER_LIA_DATA_X = 0x28;
+ // Quaternion w, x, y, z
+ private static final int REGISTER_QUA_DATA_W = 0x20;
+ private static final int REGISTER_EUL_DATA_X = 0x1A;
+ private static final int REGISTER_GYR_DATA_X = 0x14;
+ private static final int REGISTER_MAG_DATA_X = 0x0E;
+ private static final int REGISTER_ACC_DATA_X = 0x08;
+
+ private static final int REGISTER_SYS_ERR = 0x3A;
+ private static final int REGISTER_SYS_STATUS = 0x39;
+
+ // The constant value of the chip id register for the BNO055
+ private static final byte CHIP_ID_VALUE = (byte) 0xA0;
+
+ // Caching these to minimize the I2C traffic. Assumes that noone else is
+ // tinkering with the hardware.
+ private Unit currentAccelerationUnit = Unit.M_PER_S_SQUARED;
+ private Unit currentAngularRateUnit = Unit.DEGREES_PER_SECOND;
+ private Unit currentEuelerAngleUnit = Unit.DEGREES;
+ private Unit currentTemperatureUnit = Unit.CELCIUS;
+ private OrientationMode currentOrientation = OrientationMode.Windows;
+
+ /**
+ * Creates a BNO055Device with the default settings.
+ *
+ * @throws IOException exception
+ * @see PowerMode
+ * @see OperatingMode
+ */
+ public AbstractBno055Device() throws IOException {
+
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see
+ * com.robo4j.hw.rpi.i2c.imu.BNO055Device#setOperatingMode(com.robo4j.hw.rpi
+ * .i2c.imu.BNO055I2CDevice.OperatingMode)
+ */
+ @Override
+ public void setOperatingMode(OperatingMode operatingMode) throws IOException {
+ write(REGISTER_OPR_MODE, operatingMode.getCtrlCode());
+ waitForOk(20);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getOperatingMode()
+ */
+ @Override
+ public OperatingMode getOperatingMode() throws IOException {
+ return OperatingMode.fromCtrlCode(read(REGISTER_OPR_MODE) & 0x0F);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see
+ * com.robo4j.hw.rpi.i2c.imu.BNO055Device#setPowerMode(com.robo4j.hw.rpi.i2c
+ * .imu.BNO055I2CDevice.PowerMode)
+ */
+ @Override
+ public void setPowerMode(PowerMode powerMode) throws IOException {
+ write(REGISTER_PWR_MODE, powerMode.getCtrlCode());
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getPowerMode()
+ */
+ @Override
+ public PowerMode getPowerMode() throws IOException {
+ return PowerMode.fromCtrlCode(read(REGISTER_PWR_MODE));
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getCalibrationStatus()
+ */
+ @Override
+ public Bno055CalibrationStatus getCalibrationStatus() throws IOException {
+ return new Bno055CalibrationStatus(read(REGISTER_CALIB_STAT));
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getSystemStatus()
+ */
+ @Override
+ public Bno055SystemStatus getSystemStatus() throws IOException {
+ return new Bno055SystemStatus(read(REGISTER_SYS_STATUS));
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getTemperature()
+ */
+ @Override
+ public float getTemperature() throws IOException {
+ return read(REGISTER_TEMP) / currentTemperatureUnit.getFactor();
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#performSelfTest()
+ */
+ @Override
+ public Bno055SelfTestResult performSelfTest() throws IOException {
+ OperatingMode previousOperatingMode = getOperatingMode();
+ if (previousOperatingMode != OperatingMode.CONFIG) {
+ setOperatingMode(OperatingMode.CONFIG);
+ sleep(20);
+ }
+ write(REGISTER_SYS_TRIGGER, (byte) 0x8);
+ sleep(20);
+ int resultCode = read(REGISTER_ST_RESULT) & 0x0F;
+ int errorCode = 0;
+ if (resultCode != 0x0F) {
+ errorCode = read(REGISTER_SYS_ERR);
+ }
+ Bno055SelfTestResult result = new Bno055SelfTestResult(resultCode, errorCode);
+ if (previousOperatingMode != OperatingMode.CONFIG) {
+ setOperatingMode(previousOperatingMode);
+ sleep(20);
+ }
+ return result;
+ }
+
+ /**
+ * Reads a byte (signed or unsigned) from the provided register.
+ *
+ * @param register the address of the register to read from.
+ * @return the value read.
+ * @throws IOException exception
+ */
+ protected abstract int read(int register) throws IOException;
+
+ /**
+ * Writes the provided byte into the register address.
+ *
+ * @param register the register to write to.
+ * @param b the byte to write.
+ * @throws IOException exception
+ */
+ protected abstract void write(int register, byte b) throws IOException;
+
+ protected void sleep(long millis) {
+ try {
+ Thread.sleep(millis);
+ } catch (InterruptedException e) {
+ LOGGER.error(e.getMessage(), e);
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see
+ * com.robo4j.hw.rpi.i2c.imu.BNO055Device#setUnits(com.robo4j.hw.rpi.i2c.imu
+ * .BNO055I2CDevice.Unit, com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Unit,
+ * com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Unit,
+ * com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Unit,
+ * com.robo4j.hw.rpi.i2c.imu.BNO055I2CDevice.Orientation)
+ */
+ @Override
+ public void setUnits(Unit accelerationUnit, Unit angularRateUnit, Unit angleUnit, Unit temperatureUnit, OrientationMode orientationMode)
+ throws IOException {
+ int val = 0;
+ if (accelerationUnit == Unit.MILI_G) {
+ val |= 0x01;
+ } else if (accelerationUnit != Unit.MICROTESLA) {
+ throw new IllegalArgumentException(accelerationUnit + " is not an acceleration unit!");
+ }
+ if (angularRateUnit == Unit.RADIANS_PER_SECOND) {
+ val |= 0x02;
+ } else if (angularRateUnit != Unit.DEGREES_PER_SECOND) {
+ throw new IllegalArgumentException(angularRateUnit + " is not an angular rate unit!");
+ }
+ if (angleUnit == Unit.RADIANS) {
+ val |= 0x04;
+ } else if (angleUnit != Unit.DEGREES) {
+ throw new IllegalArgumentException(angleUnit + " is not an angle unit!");
+ }
+ if (temperatureUnit == Unit.FAHRENHEIT) {
+ val |= 0x10;
+ } else if (temperatureUnit != Unit.CELCIUS) {
+ throw new IllegalArgumentException(temperatureUnit + " is not a temperature unit!");
+ }
+ if (orientationMode == OrientationMode.Android) {
+ val |= 0x80;
+ }
+ write(REGISTER_UNIT_SELECT, (byte) val);
+ currentAccelerationUnit = accelerationUnit;
+ currentAngularRateUnit = angularRateUnit;
+ currentEuelerAngleUnit = angleUnit;
+ currentTemperatureUnit = temperatureUnit;
+ currentOrientation = orientationMode;
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#read()
+ */
+ @Override
+ public Tuple3f read() throws IOException {
+ return readVector(REGISTER_EUL_DATA_X, currentEuelerAngleUnit);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readEulerAngles()
+ */
+ @Override
+ public Tuple3f readEulerAngles() throws IOException {
+ return read();
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readMagnetometer()
+ */
+ @Override
+ public Tuple3f readMagnetometer() throws IOException {
+ return readVector(REGISTER_MAG_DATA_X, Unit.MICROTESLA);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readAccelerometer()
+ */
+ @Override
+ public Tuple3f readAccelerometer() throws IOException {
+ return readVector(REGISTER_ACC_DATA_X, currentAccelerationUnit);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readGyro()
+ */
+ @Override
+ public Tuple3f readGyro() throws IOException {
+ return readVector(REGISTER_GYR_DATA_X, currentAngularRateUnit);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readQuaternion()
+ */
+ @Override
+ public Tuple4d readQuaternion() throws IOException {
+ return readQuaternion(REGISTER_QUA_DATA_W);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readLinearAcceleration()
+ */
+ @Override
+ public Tuple3f readLinearAcceleration() throws IOException {
+ return readVector(REGISTER_LIA_DATA_X, currentAccelerationUnit);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#readGravityVector()
+ */
+ @Override
+ public Tuple3f readGravityVector() throws IOException {
+ return readVector(REGISTER_GRV_DATA_X, currentAccelerationUnit);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#reset()
+ */
+ @Override
+ public void reset() throws IOException {
+ write(REGISTER_SYS_TRIGGER, (byte) 0x20);
+ sleep(650);
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see com.robo4j.hw.rpi.i2c.imu.BNO055Device#getCurrentOrientation()
+ */
+ @Override
+ public OrientationMode getCurrentOrientation() {
+ return currentOrientation;
+ }
+
+ /**
+ * Will wait until the system is ready, an error occurs or the max wait was
+ * exceeded.
+ *
+ * @param maxWaitTimeMillis max time to wait for the system to be ready.
+ * @return returns true if the system is ready, false if the system is
+ * reporting an error or if the timeout was reached.
+ * @throws IOException exception
+ */
+ private boolean waitForOk(int maxWaitTimeMillis) throws IOException {
+ int waitTime = 0;
+ while (true) {
+ Bno055SystemStatus systemStatus = getSystemStatus();
+ if (systemStatus.isReady() || systemStatus.hasError() || waitTime >= maxWaitTimeMillis) {
+ return systemStatus.isReady();
+ }
+ sleep(WAIT_STEP_MILLIS);
+ waitTime += WAIT_STEP_MILLIS;
+ }
+ }
+
+ protected void initialize(OperatingMode operatingMode) throws IOException {
+ // First check that we are really communicating with the BNO.
+ if (read(REGISTER_CHIP_ID) != CHIP_ID_VALUE) {
+ throw new IOException("Not a BNO connected to the defined endpoint!");
+ }
+
+ try {
+ write(REGISTER_PAGE_ID, (byte) 0);
+ } catch (IOException ioe) {
+ // Seems sometimes the first one fails, so just ignore.
+ }
+ write(REGISTER_PAGE_ID, (byte) 0);
+ // This may be a bit unnecessary, but let's make sure we are in config
+ // first.
+ OperatingMode currentOperatingMode = getOperatingMode();
+ if (currentOperatingMode != OperatingMode.CONFIG) {
+ setOperatingMode(OperatingMode.CONFIG);
+ waitForOk(20);
+ }
+ setUseExternalChrystal(true);
+ if (currentOperatingMode != operatingMode) {
+ setOperatingMode(operatingMode);
+ }
+ }
+
+ @Override
+ public void setUseExternalChrystal(boolean useExternalChrystal) throws IOException {
+ write(REGISTER_SYS_TRIGGER, useExternalChrystal ? (byte) 0x80 : 0x00);
+ }
+
+ @Override
+ public boolean isUseExternalChrystal() throws IOException {
+ return (read(REGISTER_SYS_TRIGGER) & 0x80) > 0;
+ }
+
+ private Tuple3f readVector(int register, Unit unit) throws IOException {
+ byte[] data = read(register, 6);
+ Tuple3f tuple = new Tuple3f();
+ tuple.x = read16bitSigned(data, 0);
+ tuple.y = read16bitSigned(data, 2);
+ tuple.z = read16bitSigned(data, 4);
+ if (unit.getFactor() != 1f) {
+ tuple.x /= unit.getFactor();
+ tuple.y /= unit.getFactor();
+ tuple.z /= unit.getFactor();
+ }
+ return tuple;
+ }
+
+ /**
+ * Reads n values from the provided register.
+ *
+ * @param register the register to read
+ * @param length the total length of values to read.
+ * @return bytes
+ * @throws IOException exception
+ */
+ protected abstract byte[] read(int register, int length) throws IOException;
+
+ protected Tuple4d readQuaternion(int register) throws IOException {
+ byte[] data = read(register, 8);
+ Tuple4d tuple = new Tuple4d();
+ tuple.t = read16bitSigned(data, 0);
+ tuple.x = read16bitSigned(data, 2);
+ tuple.y = read16bitSigned(data, 4);
+ tuple.z = read16bitSigned(data, 6);
+ tuple.t *= QUAT_SCALE;
+ tuple.x *= QUAT_SCALE;
+ tuple.y *= QUAT_SCALE;
+ tuple.z *= QUAT_SCALE;
+ return tuple;
+ }
+
+ private short read16bitSigned(byte[] data, int offset) {
+ return (short) (data[offset + 1] << 8 | (data[offset] & 0xFF));
+ }
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno080Device.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno080Device.java
index 8c7dc079..62346dde 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno080Device.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/AbstractBno080Device.java
@@ -17,6 +17,14 @@
package com.robo4j.hw.rpi.imu.bno.impl;
+import com.robo4j.hw.rpi.imu.bno.Bno080Device;
+import com.robo4j.hw.rpi.imu.bno.DataListener;
+import com.robo4j.hw.rpi.imu.bno.shtp.ControlReportId;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpChannel;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpPacketRequest;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
import java.util.HashMap;
import java.util.List;
import java.util.Map;
@@ -27,12 +35,6 @@
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
-import com.robo4j.hw.rpi.imu.bno.Bno080Device;
-import com.robo4j.hw.rpi.imu.bno.DataListener;
-import com.robo4j.hw.rpi.imu.bno.shtp.ControlReportId;
-import com.robo4j.hw.rpi.imu.bno.shtp.ShtpChannel;
-import com.robo4j.hw.rpi.imu.bno.shtp.ShtpPacketRequest;
-
/**
* AbstractBNO080Device base functionality for BNO080 Devices
*
@@ -40,26 +42,26 @@
* @author Miroslav Wengner (@miragemiko)
*/
public abstract class AbstractBno080Device implements Bno080Device {
- public static final int SHTP_HEADER_SIZE = 4;
-
- static byte RECEIVE_WRITE_BYTE = (byte) 0xFF;
- static byte RECEIVE_WRITE_BYTE_CONTINUAL = (byte) 0;
- final List listeners = new CopyOnWriteArrayList<>();
- final AtomicBoolean active = new AtomicBoolean(false);
- final AtomicBoolean ready = new AtomicBoolean(false);
- final AtomicInteger commandSequenceNumber = new AtomicInteger(0);
-
- private static final short CHANNEL_COUNT = 6; // BNO080 supports 6 channels
- private static final int AWAIT_TERMINATION = 10;
- private final int[] sequenceNumberByChannel = new int[CHANNEL_COUNT];
-
- /**
- * Record IDs (figure 29, page 29 reference manual). These are used to read
- * the metadata for each sensor type.
- */
- enum FrsRecord {
-
- //@formatter:off
+ public static final int SHTP_HEADER_SIZE = 4;
+ static byte RECEIVE_WRITE_BYTE = (byte) 0xFF;
+ static byte RECEIVE_WRITE_BYTE_CONTINUAL = (byte) 0;
+ private static final Logger LOGGER = LoggerFactory.getLogger(AbstractBno080Device.class);
+ final List listeners = new CopyOnWriteArrayList<>();
+ final AtomicBoolean active = new AtomicBoolean(false);
+ final AtomicBoolean ready = new AtomicBoolean(false);
+ final AtomicInteger commandSequenceNumber = new AtomicInteger(0);
+
+ private static final short CHANNEL_COUNT = 6; // BNO080 supports 6 channels
+ private static final int AWAIT_TERMINATION = 10;
+ private final int[] sequenceNumberByChannel = new int[CHANNEL_COUNT];
+
+ /**
+ * Record IDs (figure 29, page 29 reference manual). These are used to read
+ * the metadata for each sensor type.
+ */
+ enum FrsRecord {
+
+ //@formatter:off
NONE (-1),
ACCELEROMETER (0xE302),
GYROSCOPE_CALIBRATED (0xE306),
@@ -67,32 +69,32 @@ enum FrsRecord {
ROTATION_VECTOR (0xE30B);
//@formatter:on
- private final int id;
-
- FrsRecord(int recordId) {
- this.id = recordId;
- }
-
- public int getId() {
- return id;
- }
-
- public static FrsRecord getById(int id) {
- for (FrsRecord r : values()) {
- if (id == r.getId()) {
- return r;
- }
- }
- return NONE;
- }
- }
-
- /**
- * Command IDs (section 6.4, page 42 in the manual). These are used to
- * calibrate, initialize, set orientation, tare etc the sensor.
- */
- public enum CommandId {
- //@formatter:off
+ private final int id;
+
+ FrsRecord(int recordId) {
+ this.id = recordId;
+ }
+
+ public int getId() {
+ return id;
+ }
+
+ public static FrsRecord getById(int id) {
+ for (FrsRecord r : values()) {
+ if (id == r.getId()) {
+ return r;
+ }
+ }
+ return NONE;
+ }
+ }
+
+ /**
+ * Command IDs (section 6.4, page 42 in the manual). These are used to
+ * calibrate, initialize, set orientation, tare etc the sensor.
+ */
+ public enum CommandId {
+ //@formatter:off
NONE (0),
ERRORS (1),
COUNTER (2),
@@ -105,36 +107,36 @@ public enum CommandId {
CLEAR_DCD (11);
//@formatter:on
- private static Map map = getMap();
- private final int id;
-
- CommandId(int id) {
- this.id = id;
- }
-
- public int getId() {
- return id;
- }
-
- public static CommandId getById(int id) {
- CommandId command = map.get(id);
- return command == null ? NONE : command;
- }
-
- private static Map getMap() {
- Map map = new HashMap<>();
- for (CommandId c : values()) {
- map.put(c.id, c);
- }
- return map;
- }
- }
-
- /**
- * Sensor calibration targets.
- */
- enum DeviceCalibrate {
- //@formatter:off
+ private static final Map map = getMap();
+ private final int id;
+
+ CommandId(int id) {
+ this.id = id;
+ }
+
+ public int getId() {
+ return id;
+ }
+
+ public static CommandId getById(int id) {
+ CommandId command = map.get(id);
+ return command == null ? NONE : command;
+ }
+
+ private static Map getMap() {
+ Map map = new HashMap<>();
+ for (CommandId c : values()) {
+ map.put(c.id, c);
+ }
+ return map;
+ }
+ }
+
+ /**
+ * Sensor calibration targets.
+ */
+ enum DeviceCalibrate {
+ //@formatter:off
NONE (-1),
ACCEL (0),
GYRO (1),
@@ -144,109 +146,112 @@ enum DeviceCalibrate {
STOP (5);
//@formatter:on
- private int id;
-
- DeviceCalibrate(int id) {
- this.id = id;
- }
-
- public int getId() {
- return id;
- }
-
- public static DeviceCalibrate getById(int id) {
- for (DeviceCalibrate r : values()) {
- if (id == r.getId()) {
- return r;
- }
- }
- return NONE;
- }
- }
-
- static class ShtpPacketBodyBuilder {
- private int[] body;
- private final AtomicInteger counter = new AtomicInteger();
-
- ShtpPacketBodyBuilder(int size) {
- body = new int[size];
- }
-
- ShtpPacketBodyBuilder addElement(int value) {
- this.body[counter.getAndIncrement()] = value;
- return this;
- }
-
- int[] build() {
- return body;
- }
- }
-
- final ScheduledExecutorService executor = Executors.newScheduledThreadPool(1, (r) -> {
- Thread t = new Thread(r, "Bno080 Internal Executor");
- t.setDaemon(true);
- return t;
- });
-
- @Override
- public void shutdown() {
- synchronized (executor) {
- active.set(false);
- ready.set(false);
- awaitTermination();
- }
- }
-
- @Override
- public void addListener(DataListener listener) {
- listeners.add(listener);
- }
-
- @Override
- public void removeListener(DataListener listener) {
- listeners.remove(listener);
- }
-
- /**
- * SHTP packet contains 1 byte to get Error report. Packet is sent to the
- * COMMAND channel
- *
- * @return error request packet
- */
- public ShtpPacketRequest getErrorRequest() {
- ShtpPacketRequest result = prepareShtpPacketRequest(ShtpChannel.COMMAND, 1);
- result.addBody(0, 0x01 & 0xFF);
- return result;
- }
-
- ShtpPacketRequest prepareShtpPacketRequest(ShtpChannel shtpChannel, int size) {
- ShtpPacketRequest packet = new ShtpPacketRequest(size, sequenceNumberByChannel[shtpChannel.getChannel()]++);
- packet.createHeader(shtpChannel);
- return packet;
- }
-
- ShtpPacketRequest getProductIdRequest() {
- // Check communication with device
- // bytes: Request the product ID and reset info, Reserved
- ShtpPacketRequest result = prepareShtpPacketRequest(ShtpChannel.CONTROL, 2);
- result.addBody(0, ControlReportId.PRODUCT_ID_REQUEST.getId());
- result.addBody(1, 0);
- return result;
- }
-
- ShtpPacketRequest getSoftResetPacket() {
- ShtpChannel shtpChannel = ShtpChannel.EXECUTABLE;
- ShtpPacketRequest packet = prepareShtpPacketRequest(shtpChannel, 1);
- packet.addBody(0, 1);
- return packet;
- }
-
- private void awaitTermination() {
- try {
- executor.awaitTermination(AWAIT_TERMINATION, TimeUnit.MILLISECONDS);
- executor.shutdown();
- } catch (InterruptedException e) {
- System.err.println(String.format("awaitTermination e: %s", e));
- }
- }
+ private int id;
+
+ DeviceCalibrate(int id) {
+ this.id = id;
+ }
+
+ public int getId() {
+ return id;
+ }
+
+ public static DeviceCalibrate getById(int id) {
+ for (DeviceCalibrate r : values()) {
+ if (id == r.getId()) {
+ return r;
+ }
+ }
+ return NONE;
+ }
+ }
+
+ static class ShtpPacketBodyBuilder {
+ private final int[] body;
+ private final AtomicInteger counter = new AtomicInteger();
+
+ ShtpPacketBodyBuilder(int size) {
+ body = new int[size];
+ }
+
+ ShtpPacketBodyBuilder addElement(int value) {
+ this.body[counter.getAndIncrement()] = value;
+ return this;
+ }
+
+ int[] build() {
+ return body;
+ }
+ }
+
+ final ScheduledExecutorService executor = Executors.newScheduledThreadPool(1, (r) -> {
+ Thread t = new Thread(r, "Bno080 Internal Executor");
+ t.setDaemon(true);
+ return t;
+ });
+
+ @Override
+ public void shutdown() {
+ synchronized (executor) {
+ active.set(false);
+ ready.set(false);
+ awaitTermination();
+ }
+ }
+
+ @Override
+ public void addListener(DataListener listener) {
+ listeners.add(listener);
+ }
+
+ @Override
+ public void removeListener(DataListener listener) {
+ listeners.remove(listener);
+ }
+
+ /**
+ * SHTP packet contains 1 byte to get Error report. Packet is sent to the
+ * COMMAND channel
+ *
+ * @return error request packet
+ */
+ public ShtpPacketRequest getErrorRequest() {
+ ShtpPacketRequest result = prepareShtpPacketRequest(ShtpChannel.COMMAND, 1);
+ result.addBody(0, 0x01 & 0xFF);
+ return result;
+ }
+
+ ShtpPacketRequest prepareShtpPacketRequest(ShtpChannel shtpChannel, int size) {
+ ShtpPacketRequest packet = new ShtpPacketRequest(size, sequenceNumberByChannel[shtpChannel.getChannel()]++);
+ packet.createHeader(shtpChannel);
+ return packet;
+ }
+
+ ShtpPacketRequest getProductIdRequest() {
+ // Check communication with device
+ // bytes: Request the product ID and reset info, Reserved
+ ShtpPacketRequest result = prepareShtpPacketRequest(ShtpChannel.CONTROL, 2);
+ result.addBody(0, ControlReportId.PRODUCT_ID_REQUEST.getId());
+ result.addBody(1, 0);
+ return result;
+ }
+
+ ShtpPacketRequest getSoftResetPacket() {
+ ShtpChannel shtpChannel = ShtpChannel.EXECUTABLE;
+ ShtpPacketRequest packet = prepareShtpPacketRequest(shtpChannel, 1);
+ packet.addBody(0, 1);
+ return packet;
+ }
+
+ private void awaitTermination() {
+ try {
+ var terminated = executor.awaitTermination(AWAIT_TERMINATION, TimeUnit.MILLISECONDS);
+ if (!terminated) {
+ LOGGER.warn("reached termination timeout");
+ executor.shutdown();
+ }
+ } catch (InterruptedException e) {
+ LOGGER.error("awaitTermination e: {}", e.getMessage(), e);
+ }
+ }
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno055SerialDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno055SerialDevice.java
index 85e42c23..bb979257 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno055SerialDevice.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno055SerialDevice.java
@@ -165,38 +165,18 @@ private IOException createWrongLengthException(int expectedLength, byte response
}
private IOException createErrorCodeException(byte b) {
- String message;
- switch (b) {
- case 0x02:
- message = "Read fail";
- break;
- case 0x03:
- message = "Write fail";
- break;
- case 0x04:
- message = "Regmap invalid address";
- break;
- case 0x05:
- message = "Regmap write disabled";
- break;
- case 0x06:
- message = "Wrong start byte";
- break;
- case BUFFER_OVERRUN:
- message = "Buffer overrun error";
- break;
- case 0x08:
- message = "Max length error";
- break;
- case 0x09:
- message = "Min length error";
- break;
- case 0x0A:
- message = "Receive character timeout";
- break;
- default:
- message = "Got unknown error code " + b;
- }
+ String message = switch (b) {
+ case 0x02 -> "Read fail";
+ case 0x03 -> "Write fail";
+ case 0x04 -> "Regmap invalid address";
+ case 0x05 -> "Regmap write disabled";
+ case 0x06 -> "Wrong start byte";
+ case BUFFER_OVERRUN -> "Buffer overrun error";
+ case 0x08 -> "Max length error";
+ case 0x09 -> "Min length error";
+ case 0x0A -> "Receive character timeout";
+ default -> "Got unknown error code " + b;
+ };
return new IOException(message);
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java
index 5024161b..bce7d044 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/impl/Bno080SPIDevice.java
@@ -28,7 +28,16 @@
import com.robo4j.hw.rpi.imu.bno.DataEventType;
import com.robo4j.hw.rpi.imu.bno.DataListener;
import com.robo4j.hw.rpi.imu.bno.VectorEvent;
-import com.robo4j.hw.rpi.imu.bno.shtp.*;
+import com.robo4j.hw.rpi.imu.bno.shtp.ControlReportId;
+import com.robo4j.hw.rpi.imu.bno.shtp.SensorReportId;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpChannel;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpOperation;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpOperationBuilder;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpOperationResponse;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpPacketRequest;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpPacketResponse;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpReportIds;
+import com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils;
import com.robo4j.hw.rpi.utils.GpioPin;
import com.robo4j.math.geometry.Tuple3f;
import org.slf4j.Logger;
@@ -39,7 +48,10 @@
import java.util.concurrent.TimeUnit;
import java.util.concurrent.atomic.AtomicInteger;
-import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.*;
+import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.EMPTY_EVENT;
+import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.calculateNumberOfBytesInPacket;
+import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.intToFloat;
+import static com.robo4j.hw.rpi.imu.bno.shtp.ShtpUtils.toInt8U;
/**
* Abstraction for a BNO080 absolute orientation device.
@@ -69,7 +81,7 @@ public class Bno080SPIDevice extends AbstractBno080Device {
public static final SpiChipSelect DEFAULT_SPI_CHANNEL = SpiChipSelect.CS_0;
public static final int MAX_PACKET_SIZE = 32762;
- public static final int DEFAULT_TIMEOUT_MS = 1000;
+ public static final int TIMEOUT_SEC = 1;
public static final int UNIT_TICK_MICRO = 100;
public static final int TIMEBASE_REFER_DELTA = 120;
public static final int MAX_SPI_COUNT = 255;
@@ -78,10 +90,6 @@ public class Bno080SPIDevice extends AbstractBno080Device {
private final AtomicInteger spiWaitCounter = new AtomicInteger();
private final Spi spiDevice;
- // private GpioPinDigitalInput intGpio;
- // private GpioPinDigitalOutput wakeGpio;
- // private GpioPinDigitalOutput rstGpio;
- // private GpioPinDigitalOutput csGpio; // select slave SS = chip select CS
private DigitalOutput intGpio;
private DigitalOutput wakeGpio;
@@ -94,8 +102,8 @@ public class Bno080SPIDevice extends AbstractBno080Device {
/**
* Constructor.
*
- * @throws IOException
- * @throws InterruptedException
+ * @throws IOException exception
+ * @throws InterruptedException exception
*/
public Bno080SPIDevice() throws IOException, InterruptedException {
this(DEFAULT_SPI_CHANNEL, DEFAULT_SPI_MODE, DEFAULT_SPI_SPEED);
@@ -107,8 +115,8 @@ public Bno080SPIDevice() throws IOException, InterruptedException {
* @param channel the {@link SpiChipSelect} to use.
* @param mode the {@link SpiMode} to use.
* @param speed the speed in Hz to use for communication.
- * @throws IOException
- * @throws InterruptedException
+ * @throws IOException exception
+ * @throws InterruptedException exception
*/
public Bno080SPIDevice(SpiChipSelect channel, SpiMode mode, int speed) throws IOException, InterruptedException {
// this(channel, mode, speed, RaspiPin.GPIO_00, RaspiPin.GPIO_25,
@@ -344,22 +352,18 @@ private DataEvent3f processReceivedPacket() {
}
private ShtpReportIds getReportType(ShtpChannel channel, ShtpPacketResponse response) {
- switch (channel) {
- case CONTROL:
- return ControlReportId.getById(response.getBodyFirst());
- case REPORTS:
- return SensorReportId.getById(response.getBodyFirst());
- default:
- return ControlReportId.NONE;
- }
+ return switch (channel) {
+ case CONTROL -> ControlReportId.getById(response.getBodyFirst());
+ case REPORTS -> SensorReportId.getById(response.getBodyFirst());
+ default -> ControlReportId.NONE;
+ };
}
private boolean waitForLatch(CountDownLatch latch) {
try {
- latch.await(DEFAULT_TIMEOUT_MS, TimeUnit.MILLISECONDS);
- return true;
+ return latch.await(TIMEOUT_SEC, TimeUnit.SECONDS);
} catch (InterruptedException e) {
- LOGGER.debug("waitForLatch e: {}", e.getMessage());
+ LOGGER.warn("waitForLatch e: {}", e.getMessage());
return false;
}
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpChannel.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpChannel.java
index f3786321..41889163 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpChannel.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpChannel.java
@@ -35,7 +35,7 @@ public enum ShtpChannel {
//@formatter:on
private static final Map map = getMap();
- private byte channel;
+ private final byte channel;
ShtpChannel(int channel) {
this.channel = (byte) channel;
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketRequest.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketRequest.java
index 7038a5c0..fcde87fe 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketRequest.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpPacketRequest.java
@@ -29,7 +29,7 @@
public final class ShtpPacketRequest {
private final int[] header = new int[SHTP_HEADER_SIZE];
private final int sequenceNumber;
- private int[] body;
+ private final int[] body;
public ShtpPacketRequest(int size, int sequenceNumber) {
this.body = new int[size];
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java
index 4f4f1054..6fd4aa6b 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/imu/bno/shtp/ShtpUtils.java
@@ -48,7 +48,7 @@ public final class ShtpUtils {
*/
public static float intToFloat(int fixedPointValue, int qPoint) {
float qFloat = (short) fixedPointValue;
- qFloat *= Math.pow(2, (qPoint & 0xFF) * -1);
+ qFloat *= (float) Math.pow(2, (qPoint & 0xFF) * -1);
return qFloat;
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java
index 33f77d8f..5ca6bf14 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/lcd/StringUtils.java
@@ -25,7 +25,9 @@
* @author Marcus Hirt (@hirt)
* @author Miroslav Wengner (@miragemiko)
*/
-class StringUtils {
+public class StringUtils {
+ public static final String EMPTY_STRING = "";
+ public static final String STRING_SPACE = " ";
private static final Logger LOGGER = LoggerFactory.getLogger(StringUtils.class);
public static String rightFormat(String string, int length) {
@@ -38,11 +40,7 @@ public static String centerFormat(String string, int length) {
}
private static String getSpaces(int total) {
- String tmp = "";
- for (int i = 0; i < total; i++) {
- tmp += " ";
- }
- return tmp;
+ return STRING_SPACE.repeat(Math.max(0, total));
}
public static void main(String... bla) {
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710ButtonObserver.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710ButtonObserver.java
index 947abaf2..6337cd6e 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710ButtonObserver.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710ButtonObserver.java
@@ -16,6 +16,9 @@
*/
package com.robo4j.hw.rpi.pad;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
import java.io.IOException;
import java.io.InputStream;
import java.util.LinkedList;
@@ -29,120 +32,122 @@
* @author Miro Wengner (@miragemiko)
*/
public class LF710ButtonObserver {
+ private static final Logger LOGGER = LoggerFactory.getLogger(LF710ButtonObserver.class);
+ private static final int SLEEP_MILLIS = 2000;
+ private volatile boolean isRunning = false;
+ private volatile Thread observerThread;
+ private final List buttonListeners = new LinkedList<>();
+ private final RoboControlPad pad;
- private static final int SLEEP_MILLIS = 2000;
- private volatile boolean isRunning = false;
- private volatile Thread observerThread;
- private final List buttonListeners = new LinkedList<>();
- private final RoboControlPad pad;
-
- private class ButtonChecker implements Runnable {
- private static final int ANDING_LEFT = 0x00ff;
- private static final int ANDING_LONG_LEFT = 0x00000000000000ff;
- private static final int BUFFER_SIZE = 8;
- private static final int INDEX_START = 0;
- private static final int INDEX_TIME_1 = 1;
- private static final int INDEX_TIME_2 = 2;
- private static final int INDEX_TIME_3 = 3;
- private static final int INDEX_AMOUNT_4 = 4;
- private static final int INDEX_AMOUNT_5 = 5;
- private static final int INDEX_PART = 6;
- private static final int INDEX_ELEMENT = 7;
- private final byte[] buffer = new byte[BUFFER_SIZE];
- private final InputStream source;
+ private class ButtonChecker implements Runnable {
+ private static final int ANDING_LEFT = 0x00ff;
+ private static final int ANDING_LONG_LEFT = 0x00000000000000ff;
+ private static final int BUFFER_SIZE = 8;
+ private static final int INDEX_START = 0;
+ private static final int INDEX_TIME_1 = 1;
+ private static final int INDEX_TIME_2 = 2;
+ private static final int INDEX_TIME_3 = 3;
+ private static final int INDEX_AMOUNT_4 = 4;
+ private static final int INDEX_AMOUNT_5 = 5;
+ private static final int INDEX_PART = 6;
+ private static final int INDEX_ELEMENT = 7;
+ private final byte[] buffer = new byte[BUFFER_SIZE];
+ private final InputStream source;
- ButtonChecker(InputStream source) {
- this.source = source;
- }
+ ButtonChecker(InputStream source) {
+ this.source = source;
+ }
- @Override
- public void run() {
- while (isRunning) {
- try {
- int bytes = source.read(buffer);
- if (bytes == BUFFER_SIZE) {
- final long time = ((((((buffer[INDEX_TIME_3] & ANDING_LONG_LEFT) << BUFFER_SIZE)
- | (buffer[INDEX_TIME_2] & ANDING_LEFT)) << BUFFER_SIZE)
- | (buffer[INDEX_TIME_1] & ANDING_LEFT)) << BUFFER_SIZE)
- | (buffer[INDEX_START] & ANDING_LEFT);
- final short amount = (short) (((buffer[INDEX_AMOUNT_5] & ANDING_LEFT) << BUFFER_SIZE)
- | (buffer[INDEX_AMOUNT_4] & ANDING_LEFT));
- final short part = buffer[INDEX_PART];
- final short element = buffer[INDEX_ELEMENT];
- if (part > 0) {
- final LF710Part lf710Part = LF710Part.getByMask(part);
- switch (lf710Part) {
- case BUTTON:
- fireNotification(new LF710Message(time, amount, lf710Part,
- LF710Button.getByMask(element), getInputState(amount)));
- break;
- case JOYSTICK:
- fireNotification(new LF710Message(time, amount, lf710Part,
- LF710JoystickButton.getByMask(element), getInputState(amount)));
- break;
- default:
- throw new LF710Exception("uknonw pad part:" + lf710Part);
- }
- }
- }
- } catch (IOException e) {
- throw new LF710Exception("gamepad reading problem", e);
- }
- }
- pad.disconnect();
- }
+ @Override
+ public void run() {
+ while (isRunning) {
+ try {
+ int bytes = source.read(buffer);
+ if (bytes == BUFFER_SIZE) {
+ final long time = ((long) (((((buffer[INDEX_TIME_3] & ANDING_LONG_LEFT) << BUFFER_SIZE)
+ | (buffer[INDEX_TIME_2] & ANDING_LEFT)) << BUFFER_SIZE)
+ | (buffer[INDEX_TIME_1] & ANDING_LEFT)) << BUFFER_SIZE)
+ | (buffer[INDEX_START] & ANDING_LEFT);
+ final short amount = (short) (((buffer[INDEX_AMOUNT_5] & ANDING_LEFT) << BUFFER_SIZE)
+ | (buffer[INDEX_AMOUNT_4] & ANDING_LEFT));
+ final short part = buffer[INDEX_PART];
+ final short element = buffer[INDEX_ELEMENT];
+ if (part > 0) {
+ final LF710Part lf710Part = LF710Part.getByMask(part);
+ switch (lf710Part) {
+ case BUTTON:
+ fireNotification(new LF710Message(time, amount, lf710Part,
+ LF710Button.getByMask(element), getInputState(amount)));
+ break;
+ case JOYSTICK:
+ fireNotification(new LF710Message(time, amount, lf710Part,
+ LF710JoystickButton.getByMask(element), getInputState(amount)));
+ break;
+ default:
+ throw new LF710Exception("uknonw pad part:" + lf710Part);
+ }
+ }
+ }
+ } catch (IOException e) {
+ throw new LF710Exception("gamepad reading problem", e);
+ }
+ }
+ pad.disconnect();
+ }
- // Private Methods
- private void fireNotification(LF710Message response) {
- synchronized (buttonListeners) {
- buttonListeners.forEach(l -> l.onInputPressed(response));
- }
- }
+ // Private Methods
+ private void fireNotification(LF710Message response) {
+ synchronized (buttonListeners) {
+ buttonListeners.forEach(l -> l.onInputPressed(response));
+ }
+ }
- private LF710State getInputState(short amount) {
- return (amount == 0) ? LF710State.RELEASED : LF710State.PRESSED;
- }
- }
+ private LF710State getInputState(short amount) {
+ return (amount == 0) ? LF710State.RELEASED : LF710State.PRESSED;
+ }
+ }
- public LF710ButtonObserver(RoboControlPad pad) {
- this.pad = pad;
- }
+ public LF710ButtonObserver(RoboControlPad pad) {
+ this.pad = pad;
+ }
- public void addButtonListener(PadInputResponseListener listener) {
- synchronized (listener) {
- if (buttonListeners.isEmpty()) {
- observerThread = startMonitor();
- }
- }
- buttonListeners.add(listener);
- }
+ public void addButtonListener(PadInputResponseListener listener) {
+ if (listener != null) {
+ synchronized (listener) {
+ if (buttonListeners.isEmpty()) {
+ observerThread = startMonitor();
+ }
+ }
+ }
+ buttonListeners.add(listener);
+ }
- public void removeButtonListener(PadInputResponseListener listener) {
- synchronized (buttonListeners) {
- buttonListeners.remove(listener);
- if (buttonListeners.isEmpty()) {
- stopButtonMonitor();
- }
- }
- }
+ public void removeButtonListener(PadInputResponseListener listener) {
+ synchronized (buttonListeners) {
+ buttonListeners.remove(listener);
+ if (buttonListeners.isEmpty()) {
+ stopButtonMonitor();
+ }
+ }
+ }
- // Private Methods
- //TODO, FIXME: do not randomly threads use executor also @see ButtonPressedObserver Adafruit
- //FIX : schedule threads
- private Thread startMonitor() {
- isRunning = true;
- Thread result = new Thread(new ButtonChecker(pad.source()), "robo4j-rpi-lf710-observer");
- result.setDaemon(true);
- result.start();
- return result;
- }
+ // Private Methods
+ //TODO, FIXME: do not randomly threads use executor also @see ButtonPressedObserver Adafruit
+ //FIX : schedule threads
+ private Thread startMonitor() {
+ isRunning = true;
+ Thread result = new Thread(new ButtonChecker(pad.source()), "robo4j-rpi-lf710-observer");
+ result.setDaemon(true);
+ result.start();
+ return result;
+ }
- private void stopButtonMonitor() {
- isRunning = false;
- try {
- observerThread.join(SLEEP_MILLIS);
- } catch (InterruptedException e) {
- e.printStackTrace();
- }
- }
+ private void stopButtonMonitor() {
+ isRunning = false;
+ try {
+ observerThread.join(SLEEP_MILLIS);
+ } catch (InterruptedException e) {
+ LOGGER.error(e.getMessage(), e);
+ }
+ }
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Exception.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Exception.java
index 43bb6b46..597a7ce6 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Exception.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Exception.java
@@ -16,6 +16,8 @@
*/
package com.robo4j.hw.rpi.pad;
+import java.io.Serial;
+
/**
* Logitech F710 Gamepad exception
*
@@ -23,7 +25,8 @@
* @author Miro Wengner (@miragemiko)
*/
public class LF710Exception extends RuntimeException {
- private static final long serialVersionUID = 1L;
+ @Serial
+ private static final long serialVersionUID = 1L;
public LF710Exception(String message) {
super(message);
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Part.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Part.java
index cb5e403c..53944b3b 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Part.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710Part.java
@@ -58,7 +58,8 @@ private static Map initMapping() {
public static LF710Part getByMask(Short mask) {
if (internMapByMask == null)
internMapByMask = initMapping();
- return internMapByMask.entrySet().stream().map(Map.Entry::getValue).filter(e -> e.getMask() == mask).findFirst()
+ // TODO: introduce Empty part and remove null
+ return internMapByMask.values().stream().filter(e -> e.getMask() == mask).findFirst()
.orElse(null);
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710State.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710State.java
index b59b7a3c..7d035867 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710State.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/pad/LF710State.java
@@ -59,7 +59,7 @@ private static Map initMapping() {
public static LF710State getByMask(Short mask) {
if (internMapByMask == null)
internMapByMask = initMapping();
- return internMapByMask.entrySet().stream().map(Map.Entry::getValue).filter(e -> e.getMask() == mask).findFirst()
+ return internMapByMask.values().stream().filter(e -> e.getMask() == mask).findFirst()
.orElse(null);
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java
index cccba4a0..09faf876 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/SerialUtil.java
@@ -51,7 +51,6 @@ public static Set getAvailableUSBSerialDevices() throws
* @param serial the (opened) serial port to read from.
* @param timeout the timeout after which to fail.
* @return the byte [] with the result.
- * @throws IOException exception
* @throws IllegalStateException illegal exception
* @throws InterruptedException interrupted exception
*/
@@ -71,7 +70,6 @@ public static byte[] readBytes(Serial serial, int bytes, long timeout)
* @param serial the (opened) serial port to read from.
* @param bytes the number of bytes to read.
* @param timeout the timeout after which to fail.
- * @throws IllegalStateException
* @throws InterruptedException
*/
public static void readBytes(ByteBuffer buffer, Serial serial, int bytes, long timeout)
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java
index 9fdf81bd..0dd1212a 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/gps/MTK3339GPS.java
@@ -29,7 +29,11 @@
import java.nio.charset.StandardCharsets;
import java.util.List;
import java.util.StringTokenizer;
-import java.util.concurrent.*;
+import java.util.concurrent.CopyOnWriteArrayList;
+import java.util.concurrent.Executors;
+import java.util.concurrent.ScheduledExecutorService;
+import java.util.concurrent.ScheduledFuture;
+import java.util.concurrent.TimeUnit;
/**
* Code to talk to the Adafruit "ultimate GPS" over the serial port.
@@ -41,7 +45,6 @@
* @author Miro Wengner (@miragemiko)
*/
public class MTK3339GPS implements GPS {
- private static final Logger LOGGER = LoggerFactory.getLogger(MTK3339GPS.class);
/**
* The position accuracy without any
@@ -61,6 +64,8 @@ public class MTK3339GPS implements GPS {
*/
public static final String DEFAULT_GPS_PORT = "/dev/serial0";
+ private static final Logger LOGGER = LoggerFactory.getLogger(MTK3339GPS.class);
+ private static final int TIMEOUT_SEC = 1;
private static final String POSITION_TAG = "$GPGGA";
private static final String VELOCITY_TAG = "$GPVTG";
@@ -150,10 +155,14 @@ public void shutdown() {
private void awaitTermination() {
try {
- internalExecutor.awaitTermination(10, TimeUnit.MILLISECONDS);
+ var terminated = internalExecutor.awaitTermination(TIMEOUT_SEC, TimeUnit.SECONDS);
+ if (!terminated) {
+ LOGGER.warn("not terminated");
+ }
internalExecutor.shutdown();
} catch (InterruptedException e) {
// Don't care if we were interrupted.
+ LOGGER.error("termination:{}", e.getMessage(), e);
}
}
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/Command.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/Command.java
index b7f38c9f..9d143e3b 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/Command.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/Command.java
@@ -18,12 +18,12 @@
/**
* The commands available.
- *
+ *
* @author Marcus Hirt (@hirt)
* @author Miro Wengner (@miragemiko)
*/
public enum Command {
- //@formatter:off
+ //@formatter:off
FORCE_STOP(0x00),
LOW_POWER_CONSUMPTION(0x01),
LOW_POWER_SHUTDOWN(0x02),
@@ -72,13 +72,13 @@ public enum Command {
GET_RANGING_FREQUENCY(0xD1);
//@formatter:on
- int instructionCode;
+ private final int instructionCode;
- Command(int instructionCode) {
- this.instructionCode = instructionCode;
- }
+ Command(int instructionCode) {
+ this.instructionCode = instructionCode;
+ }
- public int getInstructionCode() {
- return instructionCode;
- }
+ public int getInstructionCode() {
+ return instructionCode;
+ }
}
\ No newline at end of file
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DataHeader.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DataHeader.java
index 273af043..0c31192f 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DataHeader.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DataHeader.java
@@ -38,7 +38,7 @@ public class DataHeader {
public enum PacketType {
POINT_CLOUD(0x0), ZERO(0x1), UNKNOWN(-1);
- private int packetCode;
+ private final int packetCode;
PacketType(int packetCode) {
this.packetCode = packetCode;
@@ -175,11 +175,8 @@ private static boolean isValid(byte[] headerData) {
if (headerData[0] != ANSWER_SYNC_BYTE1) {
return false;
}
- if (headerData[1] != ANSWER_SYNC_BYTE2) {
- return false;
- }
- return true;
- }
+ return headerData[1] == ANSWER_SYNC_BYTE2;
+ }
private static int getByte(byte[] headerData, int index) {
return (headerData[index] & 0xFF);
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DeviceInfo.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DeviceInfo.java
index 66f27556..c7dc885e 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DeviceInfo.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/DeviceInfo.java
@@ -29,7 +29,7 @@ public class DeviceInfo {
public enum Model {
F4(1), T1(2), F2(3), S4(4), G4(5), X4(6), F4Pro(8), G4C(9), UNKNOWN(-1);
- int modelCode;
+ private final int modelCode;
Model(int modelCode) {
this.modelCode = modelCode;
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/HealthInfo.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/HealthInfo.java
index bae7ffd4..d0ad4f9b 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/HealthInfo.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/HealthInfo.java
@@ -34,7 +34,7 @@ public enum HealthStatus {
UNKNOWN((byte) -1);
//@formatter:on
- byte statusCode;
+ private final byte statusCode;
HealthStatus(byte status) {
this.statusCode = status;
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/ResponseHeader.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/ResponseHeader.java
index 8527d807..47d177b1 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/ResponseHeader.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/ResponseHeader.java
@@ -16,6 +16,8 @@
*/
package com.robo4j.hw.rpi.serial.ydlidar;
+import java.io.Serial;
+
/**
* The response header.
*
@@ -33,6 +35,7 @@ public class ResponseHeader {
private final int responseLength;
public static class BadResponseException extends Exception {
+ @Serial
private static final long serialVersionUID = 1L;
public BadResponseException(String message) {
@@ -44,7 +47,7 @@ public BadResponseException(String message) {
public enum ResponseType {
DEVICE_INFO(0x4), DEVICE_HEALTH(0x6), MEASUREMENT(0x81), UNKNOWN(-1);
- private int responseCode;
+ private final int responseCode;
ResponseType(int responseCode) {
this.responseCode = responseCode;
@@ -125,11 +128,8 @@ private static boolean isValid(byte[] headerData) {
if (headerData[0] != ANSWER_SYNC_BYTE1) {
return false;
}
- if (headerData[1] != ANSWER_SYNC_BYTE2) {
- return false;
- }
- return true;
- }
+ return headerData[1] == ANSWER_SYNC_BYTE2;
+ }
private static int getResponseLength(byte[] headerData) {
int responseLength = headerData[5] & 0x3F;
@@ -141,14 +141,11 @@ private static int getResponseLength(byte[] headerData) {
private static ResponseMode getResponseMode(byte[] headerData) {
int mode = (headerData[5] & 0xC0) >> 6;
- switch (mode) {
- case 0:
- return ResponseMode.SINGLE;
- case 1:
- return ResponseMode.CONTINUOUS;
- default:
- return ResponseMode.UNDEFINED;
- }
+ return switch (mode) {
+ case 0 -> ResponseMode.SINGLE;
+ case 1 -> ResponseMode.CONTINUOUS;
+ default -> ResponseMode.UNDEFINED;
+ };
}
/**
diff --git a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java
index 49a19863..fb5599a1 100644
--- a/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java
+++ b/robo4j-hw-rpi/src/main/java/com/robo4j/hw/rpi/serial/ydlidar/YDLidarDevice.java
@@ -28,7 +28,12 @@
import com.robo4j.math.geometry.impl.ScanResultImpl;
import com.robo4j.math.jfr.ScanEvent;
import com.robo4j.math.jfr.ScanId;
-import jdk.jfr.*;
+import jdk.jfr.Category;
+import jdk.jfr.Description;
+import jdk.jfr.Event;
+import jdk.jfr.Label;
+import jdk.jfr.Name;
+import jdk.jfr.StackTrace;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
@@ -102,8 +107,8 @@ public enum IdleMode {
public enum RangingFrequency {
FOUR_KHZ((byte) 0x00, 4), EIGHT_KHZ((byte) 0x01, 8), NINE_KHZ((byte) 0x02, 9);
- byte frequencyCode;
- int frequency;
+ private final byte frequencyCode;
+ private final int frequency;
RangingFrequency(byte frequencyCode, int frequency) {
this.frequencyCode = frequencyCode;
@@ -142,7 +147,7 @@ private enum RetrieverState {
@Label("Yd Debug")
@Description("This is an event to help debug the ydlidar if there is trouble")
@StackTrace(false)
- public class YDLidarDebugEvent extends Event {
+ public static class YDLidarDebugEvent extends Event {
@Label("Scan Id")
@Description("The numerical identifier, uniquely identifying the scan")
@ScanId
@@ -217,7 +222,7 @@ public void run() {
return;
}
}
- if (scanResult.getPoints().size() != 0) {
+ if (!scanResult.getPoints().isEmpty()) {
receiver.onScan(scanResult);
event.commit();
}
@@ -296,8 +301,8 @@ public void stopScanning() {
* scans for 360 degrees from -180 to 180.
*
* @param receiver call back for the receiver of the scans.
- * @throws InterruptedException
- * @throws IOException
+ * @throws InterruptedException exception
+ * @throws IOException exception
*/
public YDLidarDevice(ScanReceiver receiver) throws IOException, InterruptedException {
this(SERIAL_PORT_AUTO, receiver);
@@ -309,8 +314,8 @@ public YDLidarDevice(ScanReceiver receiver) throws IOException, InterruptedExcep
* @param serialPort the serial port to use, or SERIAL_PORT_AUTO if an attempt to
* auto resolve should be made.
* @param receiver call back for the receiver of the scans.
- * @throws InterruptedException
- * @throws IOException
+ * @throws InterruptedException exception
+ * @throws IOException exception
*/
public YDLidarDevice(String serialPort, ScanReceiver receiver) throws IOException, InterruptedException {
this.receiver = receiver;
diff --git a/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java b/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java
index d2c5851a..c88e7ea1 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/features/FeatureExtraction.java
@@ -128,7 +128,7 @@ public static float[] calculateSimpleVectorAngles(List Point2fs) {
float[] alphas = new float[Point2fs.size()];
for (int i = 0; i < Point2fs.size(); i++) {
- Point2f before = i == 0 ? Point2fs.get(0) : Point2fs.get(i - 1);
+ Point2f before = i == 0 ? Point2fs.getFirst() : Point2fs.get(i - 1);
Point2f center = Point2fs.get(i);
Point2f following = i == Point2fs.size() - 1 ? Point2fs.get(i) : Point2fs.get(i + 1);
alphas[i] = calculateVectorAngle(before, center, following);
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/CurvaturePoint2f.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/CurvaturePoint2f.java
index b9de0088..9ce9c955 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/CurvaturePoint2f.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/CurvaturePoint2f.java
@@ -56,10 +56,8 @@ public boolean equals(Object obj) {
if (getClass() != obj.getClass())
return false;
CurvaturePoint2f other = (CurvaturePoint2f) obj;
- if (Float.floatToIntBits(curvature) != Float.floatToIntBits(other.curvature))
- return false;
- return true;
- }
+ return Float.floatToIntBits(curvature) == Float.floatToIntBits(other.curvature);
+ }
/**
* Factory method for creating a point from polar coordinates.
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3d.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3d.java
index 56f8bc7b..78414054 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3d.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3d.java
@@ -122,38 +122,26 @@ public static Matrix3d createIdentity() {
public double getValue(int row, int column) {
switch (row) {
case 0:
- switch (column) {
- case 0:
- return m11;
- case 1:
- return m12;
- case 2:
- return m13;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m11;
+ case 1 -> m12;
+ case 2 -> m13;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 1:
- switch (column) {
- case 0:
- return m21;
- case 1:
- return m22;
- case 2:
- return m23;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m21;
+ case 1 -> m22;
+ case 2 -> m23;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 2:
- switch (column) {
- case 0:
- return m31;
- case 1:
- return m32;
- case 2:
- return m33;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m31;
+ case 1 -> m32;
+ case 2 -> m33;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
default:
throw new IllegalArgumentException("Row does not exist: " + row);
}
@@ -169,25 +157,15 @@ public String toString() {
public int hashCode() {
final int prime = 31;
int result = 1;
- long temp;
- temp = Double.doubleToLongBits(m11);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m12);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m13);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m21);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m22);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m23);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m31);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m32);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m33);
- result = prime * result + (int) (temp ^ (temp >>> 32));
+ result = prime * result + Double.hashCode(m11);
+ result = prime * result + Double.hashCode(m12);
+ result = prime * result + Double.hashCode(m13);
+ result = prime * result + Double.hashCode(m21);
+ result = prime * result + Double.hashCode(m22);
+ result = prime * result + Double.hashCode(m23);
+ result = prime * result + Double.hashCode(m31);
+ result = prime * result + Double.hashCode(m32);
+ result = prime * result + Double.hashCode(m33);
return result;
}
@@ -216,10 +194,8 @@ public boolean equals(Object obj) {
return false;
if (Double.doubleToLongBits(m32) != Double.doubleToLongBits(other.m32))
return false;
- if (Double.doubleToLongBits(m33) != Double.doubleToLongBits(other.m33))
- return false;
- return true;
- }
+ return Double.doubleToLongBits(m33) == Double.doubleToLongBits(other.m33);
+ }
@Override
public int getRows() {
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3f.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3f.java
index 9a563f91..16721a7f 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3f.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3f.java
@@ -121,38 +121,26 @@ public static Matrix3f createIdentity() {
public float getValue(int row, int column) {
switch (row) {
case 0:
- switch (column) {
- case 0:
- return m11;
- case 1:
- return m12;
- case 2:
- return m13;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m11;
+ case 1 -> m12;
+ case 2 -> m13;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 1:
- switch (column) {
- case 0:
- return m21;
- case 1:
- return m22;
- case 2:
- return m23;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m21;
+ case 1 -> m22;
+ case 2 -> m23;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 2:
- switch (column) {
- case 0:
- return m31;
- case 1:
- return m32;
- case 2:
- return m33;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m31;
+ case 1 -> m32;
+ case 2 -> m33;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
default:
throw new IllegalArgumentException("Row does not exist: " + row);
}
@@ -205,10 +193,8 @@ public boolean equals(Object obj) {
return false;
if (Float.floatToIntBits(m32) != Float.floatToIntBits(other.m32))
return false;
- if (Float.floatToIntBits(m33) != Float.floatToIntBits(other.m33))
- return false;
- return true;
- }
+ return Float.floatToIntBits(m33) == Float.floatToIntBits(other.m33);
+ }
@Override
public int getRows() {
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3i.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3i.java
index ed5d20fc..64ca10b7 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3i.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix3i.java
@@ -141,38 +141,26 @@ public Matrix3i multiply(Matrix3i matrix3i) {
public int getValue(int row, int column) {
switch (row) {
case 0:
- switch (column) {
- case 0:
- return m11;
- case 1:
- return m12;
- case 2:
- return m13;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m11;
+ case 1 -> m12;
+ case 2 -> m13;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 1:
- switch (column) {
- case 0:
- return m21;
- case 1:
- return m22;
- case 2:
- return m23;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m21;
+ case 1 -> m22;
+ case 2 -> m23;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 2:
- switch (column) {
- case 0:
- return m31;
- case 1:
- return m32;
- case 2:
- return m33;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m31;
+ case 1 -> m32;
+ case 2 -> m33;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
default:
throw new IllegalArgumentException("Row does not exist: " + row);
}
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4d.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4d.java
index 5983c499..cf929f0b 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4d.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4d.java
@@ -156,57 +156,37 @@ public static Matrix4d createIdentity() {
public double getValue(int row, int column) {
switch (row) {
case 0:
- switch (column) {
- case 0:
- return m11;
- case 1:
- return m12;
- case 2:
- return m13;
- case 3:
- return m14;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m11;
+ case 1 -> m12;
+ case 2 -> m13;
+ case 3 -> m14;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 1:
- switch (column) {
- case 0:
- return m21;
- case 1:
- return m22;
- case 2:
- return m23;
- case 3:
- return m24;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m21;
+ case 1 -> m22;
+ case 2 -> m23;
+ case 3 -> m24;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 2:
- switch (column) {
- case 0:
- return m31;
- case 1:
- return m32;
- case 2:
- return m33;
- case 3:
- return m34;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m31;
+ case 1 -> m32;
+ case 2 -> m33;
+ case 3 -> m34;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 3:
- switch (column) {
- case 0:
- return m41;
- case 1:
- return m42;
- case 2:
- return m43;
- case 3:
- return m44;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m41;
+ case 1 -> m42;
+ case 2 -> m43;
+ case 3 -> m44;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
default:
throw new IllegalArgumentException("Row does not exist: " + row);
}
@@ -223,39 +203,22 @@ public String toString() {
public int hashCode() {
final int prime = 31;
int result = 1;
- long temp;
- temp = Double.doubleToLongBits(m11);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m12);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m13);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m14);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m21);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m22);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m23);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m24);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m31);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m32);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m33);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m34);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m41);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m42);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m43);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(m44);
- result = prime * result + (int) (temp ^ (temp >>> 32));
+ result = prime * result + Double.hashCode(m11);
+ result = prime * result + Double.hashCode(m12);
+ result = prime * result + Double.hashCode(m13);
+ result = prime * result + Double.hashCode(m14);
+ result = prime * result + Double.hashCode(m21);
+ result = prime * result + Double.hashCode(m22);
+ result = prime * result + Double.hashCode(m23);
+ result = prime * result + Double.hashCode(m24);
+ result = prime * result + Double.hashCode(m31);
+ result = prime * result + Double.hashCode(m32);
+ result = prime * result + Double.hashCode(m33);
+ result = prime * result + Double.hashCode(m34);
+ result = prime * result + Double.hashCode(m41);
+ result = prime * result + Double.hashCode(m42);
+ result = prime * result + Double.hashCode(m43);
+ result = prime * result + Double.hashCode(m44);
return result;
}
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4f.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4f.java
index 1e945f86..85a0e77c 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4f.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4f.java
@@ -156,57 +156,37 @@ public static Matrix4f createIdentity() {
public float getValue(int row, int column) {
switch (row) {
case 0:
- switch (column) {
- case 0:
- return m11;
- case 1:
- return m12;
- case 2:
- return m13;
- case 3:
- return m14;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m11;
+ case 1 -> m12;
+ case 2 -> m13;
+ case 3 -> m14;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 1:
- switch (column) {
- case 0:
- return m21;
- case 1:
- return m22;
- case 2:
- return m23;
- case 3:
- return m24;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m21;
+ case 1 -> m22;
+ case 2 -> m23;
+ case 3 -> m24;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 2:
- switch (column) {
- case 0:
- return m31;
- case 1:
- return m32;
- case 2:
- return m33;
- case 3:
- return m34;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m31;
+ case 1 -> m32;
+ case 2 -> m33;
+ case 3 -> m34;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 3:
- switch (column) {
- case 0:
- return m41;
- case 1:
- return m42;
- case 2:
- return m43;
- case 3:
- return m44;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m41;
+ case 1 -> m42;
+ case 2 -> m43;
+ case 3 -> m44;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
default:
throw new IllegalArgumentException("Row does not exist: " + row);
}
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4i.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4i.java
index 71e4913f..f8534082 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4i.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Matrix4i.java
@@ -156,57 +156,37 @@ public static Matrix4i createIdentity() {
public int getValue(int row, int column) {
switch (row) {
case 0:
- switch (column) {
- case 0:
- return m11;
- case 1:
- return m12;
- case 2:
- return m13;
- case 3:
- return m14;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m11;
+ case 1 -> m12;
+ case 2 -> m13;
+ case 3 -> m14;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 1:
- switch (column) {
- case 0:
- return m21;
- case 1:
- return m22;
- case 2:
- return m23;
- case 3:
- return m24;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m21;
+ case 1 -> m22;
+ case 2 -> m23;
+ case 3 -> m24;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 2:
- switch (column) {
- case 0:
- return m31;
- case 1:
- return m32;
- case 2:
- return m33;
- case 3:
- return m34;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m31;
+ case 1 -> m32;
+ case 2 -> m33;
+ case 3 -> m34;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
case 3:
- switch (column) {
- case 0:
- return m41;
- case 1:
- return m42;
- case 2:
- return m43;
- case 3:
- return m44;
- default:
- throw new IllegalArgumentException("Column does not exist: " + column);
- }
+ return switch (column) {
+ case 0 -> m41;
+ case 1 -> m42;
+ case 2 -> m43;
+ case 3 -> m44;
+ default -> throw new IllegalArgumentException("Column does not exist: " + column);
+ };
default:
throw new IllegalArgumentException("Row does not exist: " + row);
}
@@ -281,10 +261,8 @@ public boolean equals(Object obj) {
return false;
if (m43 != other.m43)
return false;
- if (m44 != other.m44)
- return false;
- return true;
- }
+ return m44 == other.m44;
+ }
@Override
public int getRows() {
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Point2f.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Point2f.java
index e0fe6eb8..eaf99d65 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Point2f.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Point2f.java
@@ -112,10 +112,9 @@ public boolean equals(Object obj) {
return true;
if (obj == null)
return false;
- if (!(obj instanceof Point2f))
+ if (!(obj instanceof Point2f other))
return false;
- Point2f other = (Point2f) obj;
- if (Float.floatToIntBits(angle) != Float.floatToIntBits(other.angle))
+ if (Float.floatToIntBits(angle) != Float.floatToIntBits(other.angle))
return false;
return Float.floatToIntBits(range) == Float.floatToIntBits(other.range);
}
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple3d.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple3d.java
index 50910d0d..e6a314b6 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple3d.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple3d.java
@@ -94,13 +94,9 @@ public String toString() {
public int hashCode() {
final int prime = 31;
int result = 1;
- long temp;
- temp = Double.doubleToLongBits(x);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(y);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(z);
- result = prime * result + (int) (temp ^ (temp >>> 32));
+ result = prime * result + Double.hashCode(x);
+ result = prime * result + Double.hashCode(y);
+ result = prime * result + Double.hashCode(z);
return result;
}
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple4d.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple4d.java
index 51820024..ca68184a 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple4d.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/Tuple4d.java
@@ -97,15 +97,10 @@ public String toString() {
public int hashCode() {
final int prime = 31;
int result = 1;
- long temp;
- temp = Double.doubleToLongBits(t);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(x);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(y);
- result = prime * result + (int) (temp ^ (temp >>> 32));
- temp = Double.doubleToLongBits(z);
- result = prime * result + (int) (temp ^ (temp >>> 32));
+ result = prime * result + Double.hashCode(t);
+ result = prime * result + Double.hashCode(x);
+ result = prime * result + Double.hashCode(y);
+ result = prime * result + Double.hashCode(z);
return result;
}
diff --git a/robo4j-math/src/main/java/com/robo4j/math/geometry/impl/ScanResultImpl.java b/robo4j-math/src/main/java/com/robo4j/math/geometry/impl/ScanResultImpl.java
index ae39ed5c..bc4244c5 100644
--- a/robo4j-math/src/main/java/com/robo4j/math/geometry/impl/ScanResultImpl.java
+++ b/robo4j-math/src/main/java/com/robo4j/math/geometry/impl/ScanResultImpl.java
@@ -16,17 +16,16 @@
*/
package com.robo4j.math.geometry.impl;
+import com.robo4j.math.geometry.Point2f;
+import com.robo4j.math.geometry.ScanResult2D;
+import com.robo4j.math.jfr.ScanPoint2DEvent;
+
import java.util.ArrayList;
-import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.function.Predicate;
-import com.robo4j.math.geometry.Point2f;
-import com.robo4j.math.geometry.ScanResult2D;
-import com.robo4j.math.jfr.ScanPoint2DEvent;
-
/**
* The implementation of a scan result. This particular implementation will emit
* JFR events to help with the analysis of the recorded JFR data.
@@ -36,7 +35,7 @@
*/
public class ScanResultImpl implements ScanResult2D {
private static final PointComparator POINT_COMPARATOR = new PointComparator();
- private static final AtomicInteger SCANCOUNTER = new AtomicInteger(0);
+ private static final AtomicInteger SCAN_COUNTER = new AtomicInteger(0);
private static final Predicate PREDICATE_KEEP_ALL = new KeepAllPredicate();
private final List points;
@@ -70,7 +69,7 @@ public ScanResultImpl(float angularResolution, Predicate pointFilter) {
public ScanResultImpl(int size, float angularResolution, Predicate pointFilter) {
this.pointFilter = pointFilter;
- scanId = SCANCOUNTER.incrementAndGet();
+ scanId = SCAN_COUNTER.incrementAndGet();
this.angularResolution = angularResolution;
points = new ArrayList(size);
}
@@ -184,14 +183,14 @@ public float getAngularResolution() {
@Override
public Point2f getLeftmostPoint() {
- return points.get(0);
+ return points.getFirst();
}
@Override
public Point2f getRightmostPoint() {
// NOTE(Marcus/Sep 5, 2017): Should be fine, as the add phase is
// separate from the read phase.
- return points.get(points.size() - 1);
+ return points.getLast();
}
/**
diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java
index 3ed8fc8e..2584cb2e 100644
--- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java
+++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestCallable.java
@@ -81,7 +81,7 @@ public HttpResponseProcess call() throws Exception {
resultBuilder.setResult(factory.processGet(context));
} else {
- resultBuilder.setTarget(pathConfig.getRoboUnit().getId());
+ resultBuilder.setTarget(pathConfig.getRoboUnit().id());
final Object unitDescription;
// the system needs to have one more worker thread to evaluate Future get
final HttpRequestDenominator denominator = (HttpRequestDenominator) decoratedRequest
@@ -91,26 +91,26 @@ public HttpResponseProcess call() throws Exception {
if (requestAttributes == null) {
unitDescription = factory.processGet(pathConfig);
} else if (requestAttributes.isEmpty()) {
- RoboReference> unit = context.getReference(pathConfig.getRoboUnit().getId());
+ RoboReference> unit = context.getReference(pathConfig.getRoboUnit().id());
PathAttributeListDTO pathAttributes = new PathAttributeListDTO();
unit.getKnownAttributes().forEach(a -> {
PathAttributeDTO attributeDescriptor = new PathAttributeDTO();
- attributeDescriptor.setName(a.getAttributeName());
- attributeDescriptor.setValue(a.getAttributeType().getCanonicalName());
+ attributeDescriptor.setName(a.attributeName());
+ attributeDescriptor.setValue(a.attributeType().getCanonicalName());
pathAttributes.addAttribute(attributeDescriptor);
});
unitDescription = ReflectUtils.createJson(pathAttributes);
} else {
- RoboReference> unit = context.getReference(pathConfig.getRoboUnit().getId());
+ RoboReference> unit = context.getReference(pathConfig.getRoboUnit().id());
List attributes = new ArrayList<>();
for (AttributeDescriptor attr : unit.getKnownAttributes()) {
- if (requestAttributes.contains(attr.getAttributeName())) {
+ if (requestAttributes.contains(attr.attributeName())) {
PathAttributeDTO attribute = new PathAttributeDTO();
String valueString = String.valueOf(unit.getAttribute(attr).get());
attribute.setValue(valueString);
- attribute.setName(attr.getAttributeName());
+ attribute.setName(attr.attributeName());
attributes.add(attribute);
}
}
@@ -131,7 +131,7 @@ public HttpResponseProcess call() throws Exception {
if (pathConfig.getPath().equals(UTF8_SOLIDUS)) {
resultBuilder.setCode(StatusCode.BAD_REQUEST);
} else {
- resultBuilder.setTarget(pathConfig.getRoboUnit().getId());
+ resultBuilder.setTarget(pathConfig.getRoboUnit().id());
Object respObj = factory.processPost(pathConfig.getRoboUnit(), decoratedRequest.getMessage());
if (respObj == null) {
resultBuilder.setCode(StatusCode.BAD_REQUEST);
diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java
index 9ee800c5..4d3876c3 100644
--- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java
+++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/request/RoboRequestFactory.java
@@ -64,8 +64,8 @@ public Object processGet(RoboContext context) {
if (!context.getUnits().isEmpty()) {
final List unitList = context.getUnits().stream()
- .map(u -> new ResponseUnitDTO(u.getId(), u.getState())).collect(Collectors.toList());
- unitList.add(0, new ResponseUnitDTO(context.getId(), context.getState()));
+ .map(u -> new ResponseUnitDTO(u.id(), u.getState())).collect(Collectors.toList());
+ unitList.addFirst(new ResponseUnitDTO(context.getId(), context.getState()));
return JsonUtil.toJsonArray(unitList);
} else {
LOGGER.error("internal error: no units available");
@@ -85,11 +85,11 @@ public Object processGet(ServerPathConfig pathConfig) {
try {
Object val = unitRef.getAttribute(d).get();
ResponseAttributeDTO attributeDTO = new ResponseAttributeDTO();
- attributeDTO.setId(d.getAttributeName());
- attributeDTO.setType(d.getAttributeType().getTypeName());
+ attributeDTO.setId(d.attributeName());
+ attributeDTO.setType(d.attributeType().getTypeName());
attributeDTO.setValue(String.valueOf(val));
- if (d.getAttributeName().equals(HttpServerUnit.ATTR_PATHS)) {
+ if (d.attributeName().equals(HttpServerUnit.ATTR_PATHS)) {
attributeDTO.setType("java.util.ArrayList");
}
return attributeDTO;
@@ -106,7 +106,7 @@ public Object processGet(ServerPathConfig pathConfig) {
} else {
final ResponseDecoderUnitDTO result = new ResponseDecoderUnitDTO();
- result.setId(unitRef.getId());
+ result.setId(unitRef.id());
result.setCodec(decoder.getDecodedClass().getName());
result.setMethods(GET_POST_METHODS);
return ReflectUtils.createJson(result);
diff --git a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpServerUnit.java b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpServerUnit.java
index c5f2cb84..c1eacf18 100644
--- a/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpServerUnit.java
+++ b/robo4j-socket-http/src/main/java/com/robo4j/socket/http/units/HttpServerUnit.java
@@ -114,15 +114,15 @@ public void stop() {
@SuppressWarnings("unchecked")
@Override
protected R onGetAttribute(AttributeDescriptor descriptor) {
- if (descriptor.getAttributeName().equals(ATTR_ADDRESS) && descriptor.getAttributeType() == String.class) {
+ if (descriptor.attributeName().equals(ATTR_ADDRESS) && descriptor.attributeType() == String.class) {
return (R) serverAddress;
}
- if (descriptor.getAttributeName().equals(ATTR_PORT) && descriptor.getAttributeType() == Integer.class) {
+ if (descriptor.attributeName().equals(ATTR_PORT) && descriptor.attributeType() == Integer.class) {
return (R) serverPort;
}
- if (descriptor.getAttributeName().equals(ATTR_PATHS) && descriptor.getAttributeType() == String.class) {
+ if (descriptor.attributeName().equals(ATTR_PATHS) && descriptor.attributeType() == String.class) {
return (R) JsonUtil.toJsonArray(paths);
}
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java
index 2d81c632..a2781a95 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramClientTest.java
@@ -16,7 +16,6 @@
*/
package com.robo4j.socket.http.test.units;
-import com.robo4j.RoboContext;
import com.robo4j.RoboReference;
import com.robo4j.socket.http.test.units.config.StringConsumer;
import com.robo4j.util.SystemUtil;
@@ -28,7 +27,7 @@
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit;
-import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
/**
* Testing Datagram client/server decorated messages
@@ -38,20 +37,20 @@
*/
class RoboDatagramClientTest {
private static final Logger LOGGER = LoggerFactory.getLogger(RoboDatagramClientTest.class);
- private static final int TIMEOUT = 10;
- private static final TimeUnit TIME_UNIT = TimeUnit.HOURS;
+ private static final int TIMEOUT_SEC = 10;
+ private static final TimeUnit TIME_UNIT = TimeUnit.SECONDS;
private static final int MAX_NUMBER = 42;
private static final int DEFAULT_TIMEOUT = 5;
@Disabled
- @Test
- void datagramClientServerTest() throws Exception {
+ @Test
+ void datagramClientServerTest() throws Exception {
- var producerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_producer_text.xml");
- var consumerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_consumer_text.xml");
+ var producerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_producer_text.xml");
+ var consumerSystem = RoboContextUtils.loadRoboContextByXml("robo_datagram_client_request_consumer_text.xml");
- consumerSystem.start();
- producerSystem.start();
+ consumerSystem.start();
+ producerSystem.start();
LOGGER.info(SystemUtil.printStateReport(consumerSystem));
LOGGER.info(SystemUtil.printStateReport(producerSystem));
@@ -63,15 +62,16 @@ void datagramClientServerTest() throws Exception {
CountDownLatch countDownLatchStringProducer = stringConsumerProducer
.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_LATCH).get(DEFAULT_TIMEOUT, TimeUnit.MINUTES);
- countDownLatchStringProducer.await(TIMEOUT, TIME_UNIT);
+ var produced = countDownLatchStringProducer.await(TIMEOUT_SEC, TimeUnit.SECONDS);
final int consumerTotalNumber = stringConsumerProducer
.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get(DEFAULT_TIMEOUT, TimeUnit.MINUTES);
producerSystem.shutdown();
consumerSystem.shutdown();
- assertNotNull(consumerTotalNumber);
+ assertTrue(produced);
+ assertTrue(consumerTotalNumber > 0);
- }
+ }
}
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java
index 6c6c5faa..cb6d917a 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboDatagramPingPongTest.java
@@ -37,7 +37,10 @@
import java.util.concurrent.TimeUnit;
import static com.robo4j.socket.http.test.units.HttpUnitTests.CODECS_UNITS_TEST_PACKAGE;
-import static com.robo4j.socket.http.util.RoboHttpUtils.*;
+import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_CODEC_PACKAGES;
+import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_HOST;
+import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_SOCKET_PORT;
+import static com.robo4j.socket.http.util.RoboHttpUtils.PROPERTY_UNIT_PATHS_CONFIG;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -56,7 +59,7 @@ class RoboDatagramPingPongTest {
@Test
void datagramPingPongTest() throws Exception {
- var pongSystem = configurePongSystem(TOTAL_NUMBER);
+ var pongSystem = configurePongSystem();
var pingSystem = configurePingSystem();
pongSystem.start();
@@ -78,7 +81,7 @@ void datagramPingPongTest() throws Exception {
udpClient.sendMessage(request);
}
- totalMessageLatch.await(TIMEOUT, TIME_UNIT);
+ var messagesReceived = totalMessageLatch.await(TIMEOUT, TIME_UNIT);
final int pongConsumerTotalNumber = pongStringConsumerReference.getAttribute(StringConsumer.DESCRIPTOR_MESSAGES_TOTAL).get();
@@ -89,6 +92,7 @@ void datagramPingPongTest() throws Exception {
pingSystem.shutdown();
pongSystem.shutdown();
+ assertTrue(messagesReceived);
assertTrue(pongConsumerTotalNumber > 0 && pongConsumerTotalNumber <= TOTAL_NUMBER);
}
@@ -113,13 +117,13 @@ private RoboContext configurePingSystem() throws Exception {
* @return roboContext
* @throws Exception exception
*/
- private RoboContext configurePongSystem(int totalNumberOfMessage) throws Exception {
+ private RoboContext configurePongSystem() throws Exception {
RoboBuilder builder = new RoboBuilder();
Configuration config = new ConfigurationBuilder().addString(PROPERTY_CODEC_PACKAGES, CODECS_UNITS_TEST_PACKAGE)
.addString(PROPERTY_UNIT_PATHS_CONFIG, "[{\"roboUnit\":\"stringConsumer\",\"filters\":[]}]").build();
builder.add(DatagramServerUnit.class, config, UDP_SERVER);
- config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, totalNumberOfMessage).build();
+ config = new ConfigurationBuilder().addInteger(StringConsumer.PROP_TOTAL_NUMBER_MESSAGES, RoboDatagramPingPongTest.TOTAL_NUMBER).build();
builder.add(StringConsumer.class, config, StringConsumer.NAME);
return builder.build();
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java
index 337bef0a..bd39f868 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/RoboHttpDynamicTests.java
@@ -59,7 +59,7 @@
*/
class RoboHttpDynamicTests {
private static final Logger LOGGER = LoggerFactory.getLogger(RoboHttpDynamicTests.class);
- private static final int TIMEOUT = 20;
+ private static final int TIMEOUT_MIN = 1;
private static final TimeUnit TIME_UNIT = TimeUnit.HOURS;
private static final String ID_HTTP_SERVER = "http";
private static final int PORT = 8025;
@@ -98,10 +98,10 @@ void simpleHttpNonUnitTest() throws Exception {
// TODO: review how to receiving attributes
var countDownLatchDecoratedProducer = getAttributeOrTimeout(decoratedProducer, SocketMessageDecoratedProducerUnit.DESCRIPTOR_MESSAGES_LATCH);
- var messagesProduced = countDownLatchDecoratedProducer.await(TIMEOUT, TIME_UNIT);
+ var messagesProduced = countDownLatchDecoratedProducer.await(TIMEOUT_MIN, TIME_UNIT);
var stringConsumer = mainSystem.getReference(StringConsumer.NAME);
var countDownLatch = getAttributeOrTimeout(stringConsumer, StringConsumer.DESCRIPTOR_MESSAGES_LATCH);
- var messagesReceived = countDownLatch.await(TIMEOUT, TIME_UNIT);
+ var messagesReceived = countDownLatch.await(TIMEOUT_MIN, TIME_UNIT);
var receivedMessages = getAttributeOrTimeout(stringConsumer, StringConsumer.DESCRIPTOR_MESSAGES_TOTAL);
clientSystem.shutdown();
@@ -203,11 +203,12 @@ private RoboContext getClientRoboSystem() throws Exception {
return result;
}
+ // TODO: maybe some duplication
private static R getAttributeOrTimeout(RoboReference roboReference, AttributeDescriptor attributeDescriptor) throws InterruptedException, ExecutionException, TimeoutException {
- var attribute = roboReference.getAttribute(attributeDescriptor).get(TIMEOUT, TimeUnit.MINUTES);
+ var attribute = roboReference.getAttribute(attributeDescriptor).get(TIMEOUT_MIN, TimeUnit.MINUTES);
if (attribute == null) {
- attribute = roboReference.getAttribute(attributeDescriptor).get(TIMEOUT, TimeUnit.MINUTES);
- LOGGER.error("roboReference:{}, no attribute:{}", roboReference.getId(), attributeDescriptor.getAttributeName());
+ attribute = roboReference.getAttribute(attributeDescriptor).get(TIMEOUT_MIN, TimeUnit.MINUTES);
+ LOGGER.error("roboReference:{}, no attribute:{}", roboReference.id(), attributeDescriptor.attributeName());
}
return attribute;
}
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpOneAttributeGetController.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpOneAttributeGetController.java
index 5390bb1f..7dbe14dd 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpOneAttributeGetController.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpOneAttributeGetController.java
@@ -24,9 +24,9 @@
import com.robo4j.configuration.Configuration;
import com.robo4j.socket.http.test.units.config.enums.TestCommandEnum;
-import java.util.Arrays;
import java.util.Collection;
import java.util.Collections;
+import java.util.List;
/**
* HttpOneAttributeGetController contains attributes that are exposed over HttpSocket
@@ -41,9 +41,9 @@ public class HttpOneAttributeGetController extends RoboUnit {
.create(Integer.class, ATTR_NUMBER);
public static final Collection> KNOWN_ATTRIBUTES = Collections
- .unmodifiableCollection(Arrays.asList(DESCRIPTOR_NUMBER));
+ .unmodifiableCollection(List.of(DESCRIPTOR_NUMBER));
- private int number = 42;
+ private final int number = 42;
private String target;
public HttpOneAttributeGetController(RoboContext context, String id) {
@@ -66,8 +66,8 @@ public void onMessage(TestCommandEnum message) {
@SuppressWarnings("unchecked")
@Override
protected R onGetAttribute(AttributeDescriptor descriptor) {
- if (descriptor.getAttributeName().equals(ATTR_NUMBER)
- && descriptor.getAttributeType() == Integer.class) {
+ if (descriptor.attributeName().equals(ATTR_NUMBER)
+ && descriptor.attributeType() == Integer.class) {
return (R) Integer.valueOf(number);
}
return null;
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpTwoAttributesGetController.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpTwoAttributesGetController.java
index 07b58254..7181daed 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpTwoAttributesGetController.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/HttpTwoAttributesGetController.java
@@ -47,8 +47,8 @@ public class HttpTwoAttributesGetController extends RoboUnit {
public static final Collection> KNOWN_ATTRIBUTES = Collections
.unmodifiableCollection(Arrays.asList(DESCRIPTOR_NUMBER, DESCRIPTOR_TEXT));
- private int number = 42;
- private String text = "Some magic text";
+ private final int number = 42;
+ private final String text = "Some magic text";
private String target;
public HttpTwoAttributesGetController(RoboContext context, String id) {
@@ -71,10 +71,10 @@ public void onMessage(TestCommandEnum message) {
@SuppressWarnings("unchecked")
@Override
protected R onGetAttribute(AttributeDescriptor descriptor) {
- if (AttributeUtils.validateAttributeByNameAndType(descriptor, ATTR_NUMBER, DESCRIPTOR_NUMBER.getAttributeType())) {
+ if (AttributeUtils.validateAttributeByNameAndType(descriptor, ATTR_NUMBER, DESCRIPTOR_NUMBER.attributeType())) {
return (R) Integer.valueOf(number);
}
- if(AttributeUtils.validateAttributeByNameAndType(descriptor, ATTR_TEXT, DESCRIPTOR_TEXT.getAttributeType())){
+ if(AttributeUtils.validateAttributeByNameAndType(descriptor, ATTR_TEXT, DESCRIPTOR_TEXT.attributeType())){
return (R) text;
}
return null;
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/PropertyListBuilder.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/PropertyListBuilder.java
index 9cd27429..f74843cb 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/PropertyListBuilder.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/PropertyListBuilder.java
@@ -26,7 +26,7 @@
*/
public class PropertyListBuilder {
- private List list;
+ private final List list;
private PropertyListBuilder() {
this.list = new LinkedList<>();
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java
index c6f10176..c023e28a 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/SocketMessageDecoratedProducerUnit.java
@@ -138,16 +138,16 @@ private HttpDecoratedRequest getHttpRequest(ClientPathConfig pathConfig) {
@SuppressWarnings("unchecked")
@Override
public synchronized R onGetAttribute(AttributeDescriptor attribute) {
- if (attribute.getAttributeName().equals(ATTR_TOTAL_SENT_MESSAGES)
- && attribute.getAttributeType() == Integer.class) {
+ if (attribute.attributeName().equals(ATTR_TOTAL_SENT_MESSAGES)
+ && attribute.attributeType() == Integer.class) {
return (R) (Integer) counter.get();
}
- if (attribute.getAttributeName().equals(ATTR_MESSAGES_LATCH)
- && attribute.getAttributeType() == CountDownLatch.class) {
+ if (attribute.attributeName().equals(ATTR_MESSAGES_LATCH)
+ && attribute.attributeType() == CountDownLatch.class) {
return (R) messagesLatch;
}
- if (attribute.getAttributeName().equals(ATTR_SETUP_LATCH)
- && attribute.getAttributeType() == CountDownLatch.class) {
+ if (attribute.attributeName().equals(ATTR_SETUP_LATCH)
+ && attribute.attributeType() == CountDownLatch.class) {
return (R) setupLatch;
}
return null;
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringConsumer.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringConsumer.java
index 56723451..33eba703 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringConsumer.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringConsumer.java
@@ -75,16 +75,16 @@ public void onMessage(String message) {
@SuppressWarnings("unchecked")
@Override
public synchronized R onGetAttribute(AttributeDescriptor attribute) {
- if (attribute.getAttributeName().equals(ATTR_MESSAGES_TOTAL)
- && attribute.getAttributeType() == Integer.class) {
+ if (attribute.attributeName().equals(ATTR_MESSAGES_TOTAL)
+ && attribute.attributeType() == Integer.class) {
return (R) Integer.valueOf(counter.get());
}
- if (attribute.getAttributeName().equals(ATTR_RECEIVED_MESSAGES)
- && attribute.getAttributeType() == List.class) {
+ if (attribute.attributeName().equals(ATTR_RECEIVED_MESSAGES)
+ && attribute.attributeType() == List.class) {
return (R) receivedMessages;
}
- if (attribute.getAttributeName().equals(ATTR_MESSAGES_LATCH)
- && attribute.getAttributeType() == CountDownLatch.class) {
+ if (attribute.attributeName().equals(ATTR_MESSAGES_LATCH)
+ && attribute.attributeType() == CountDownLatch.class) {
return (R) messagesLatch;
}
return null;
diff --git a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java
index 7f9e67f4..6cf1b0cd 100644
--- a/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java
+++ b/robo4j-socket-http/src/test/java/com/robo4j/socket/http/test/units/config/StringProducerUnit.java
@@ -38,8 +38,8 @@ public class StringProducerUnit extends RoboUnit {
private String target;
/**
- * @param context
- * @param id
+ * @param context RoboContext
+ * @param id RoboId
*/
public StringProducerUnit(RoboContext context, String id) {
super(String.class, context, id);
@@ -68,8 +68,8 @@ public void onMessage(String message) {
@SuppressWarnings("unchecked")
@Override
public synchronized R onGetAttribute(AttributeDescriptor