diff --git a/src/rove_control_board/api/api.h b/src/rove_control_board/api/api.h index 4b1c5f2..839e47c 100644 --- a/src/rove_control_board/api/api.h +++ b/src/rove_control_board/api/api.h @@ -262,106 +262,44 @@ static_assert((sizeof(RGBLed)+1) == 6); Bool_ configure(Configuration); static_assert((sizeof(Configuration)+1) == 9); -// 0 -// 1 -// 2 -// 3 -// 4 -// 5 -// 6 -// 7 -// 8 -// 9 - - - -// 0x00 : 00 -// 0x01 : 01 -// 0x02 : 02 -// 0x03 : 03 -// 0x04 : 04 -// 0x05 : 05 -// 0x06 : 06 -// 0x07 : 07 -// 0x08 : 08 -// 0x09 : 09 -// 0x0A : 10 -// 0x0B : 11 -// 0x0C : 12 -// 0x0D : 13 -// 0x0E : 14 -// 0x0F : 15 -// 0x10 : 16 -// 0x11 : 17 -// 0x12 : 18 -// 0x13 : 19 -// 0x14 : 20 -// 0x15 : 21 -// 0x16 : 22 -// 0x17 : 23 -// 0x18 : 24 -// 0x19 : 25 -// 0x1A : 26 -// 0x1B : 27 -// 0x1C : 28 -// 0x1D : 29 -// 0x1E : 30 -// 0x1F : 31 -// 0x10 : 32 -// 0x11 : 33 -// 0x12 : 34 -// 0x13 : 35 -// 0x14 : 36 -// 0x15 : 37 -// 0x16 : 38 -// 0x17 : 39 -// 0x18 -// 0x19 -// 0x1A -// 0x1B -// 0x1C -// 0x1D -// 0x1E -// 0x1F - static BaseFunction_ptr commands[] = { - new Function(&ping), // 0x00 : 00 - new Function(&hashCheck), // 0x01 : 01 - new Function(&setServoPosition), // 0x02 : 02 - new Function(&getServoPosition), // 0x03 : 03 - new Function(&setServoPositionZero), // 0x04 : 04 - new Function(&setServoSpeed), // 0x05 : 05 - new Function(&getServoSpeed), // 0x06 : 06 - new Function(&getServoPositionX), // 0x07 : 07 - new Function(&getServoPositionY), // 0x08 : 08 - new Function(&getServoSpeedX), // 0x09 : 09 - new Function(&getServoSpeedY), // 0x0A : 10 - new Function(&setServoXAcc), // 0x0B : 11 - new Function(&setServoYAcc), // 0x0C : 12 - new Function(&getServoXAcc), // 0x0D : 13 - new Function(&getServoYAcc), // 0x0E : 14 - new Function(&setLEDFront), // 0x0F : 15 - new Function(&setLEDBack), // 0x10 : 16 - new Function(&setLEDStrobe), // 0x11 : 17 - new Function(&getLEDFront), // 0x12 : 18 - new Function(&getLEDBack), // 0x13 : 19 - new Function(&getLEDStrobe), // 0x14 : 20 - new Function(&getReport), // 0x15 : 21 - new Function(&getWinchMode), // 0x16 : 22 - new Function(&setWinchMode), // 0x17 : 23 - new Function(&getWinchLock), // 0x18 : 24 - new Function(&setWinchLock), // 0x19 : 25 - new Function(&getServoControlMode), // 0x1A : 26 - new Function(&setServoControlMode), // 0x1B : 27 - new Function(&getGPIO1), // 0x1C : 28 - new Function(&setGPIO1), // 0x1D : 29 - new Function(&getGPIO2), // 0x1E : 30 - new Function(&setGPIO2), // 0x1F : 31 - new Function(&getGPIO3), // 0x10 : 32 - new Function(&setGPIO3), // 0x11 : 33 - new Function(&getRGBLed), // 0x12 : 34 - new Function(&setRGBLed), // 0x13 : 35 - new Function(&configure), // 0x14 : 36 + new Function(&ping), + new Function(&hashCheck), + new Function(&setServoPosition), + new Function(&getServoPosition), + new Function(&setServoPositionZero), + new Function(&setServoSpeed), + new Function(&getServoSpeed), + new Function(&getServoPositionX), + new Function(&getServoPositionY), + new Function(&getServoSpeedX), + new Function(&getServoSpeedY), + new Function(&setServoXAcc), + new Function(&setServoYAcc), + new Function(&getServoXAcc), + new Function(&getServoYAcc), + new Function(&setLEDFront), + new Function(&setLEDBack), + new Function(&setLEDStrobe), + new Function(&getLEDFront), + new Function(&getLEDBack), + new Function(&getLEDStrobe), + new Function(&getReport), + new Function(&getWinchMode), + new Function(&setWinchMode), + new Function(&getWinchLock), + new Function(&setWinchLock), + new Function(&getServoControlMode), + new Function(&setServoControlMode), + new Function(&getGPIO1), + new Function(&setGPIO1), + new Function(&getGPIO2), + new Function(&setGPIO2), + new Function(&getGPIO3), + new Function(&setGPIO3), + new Function(&getRGBLed), + new Function(&setRGBLed), + new Function(&configure), }; #define COMMANDS_COUNT 37 #define MAX_DECODED_SIZE 9 diff --git a/src/rove_control_board/rove_control_board/control_board_bridge.py b/src/rove_control_board/rove_control_board/control_board_bridge.py index 4da29bc..6086c83 100644 --- a/src/rove_control_board/rove_control_board/control_board_bridge.py +++ b/src/rove_control_board/rove_control_board/control_board_bridge.py @@ -214,7 +214,7 @@ def __init__(self): self._last_send = 0 # Setup status report - self.heartbeat = self.create_timer(1/15, callThreadsafe(self.statusReport)) + self.heartbeat = self.create_timer(1/30, callThreadsafe(self.statusReport)) # self.create_timer(1/100, self.ping) self.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) @@ -466,24 +466,24 @@ def statusReport(self): r = api.getReport(comm.Void()) - if r.errorCode != api.ErrorCode.ERNone.value: - try: - self.printErrors(r.errorCode) - except ValueError as e: - self.get_logger().error(str(e)) - if r.statusCode == api.StatusCode.STNotInitialized.value: - self.get_logger().warning("Not initialized") - # sleep(2) - return - if r.statusCode == api.StatusCode.STInitialized.value and not self._configured: - if self._confIterator is None: - self.get_logger().info("Configuring") - self._confIterator = iter(self.sendConfig()) - # sleep(2) - next(self._confIterator) - if self._configured: - self.get_logger().info("Done") - return + # if r.errorCode != api.ErrorCode.ERNone.value: + # try: + # self.printErrors(r.errorCode) + # except ValueError as e: + # self.get_logger().error(str(e)) + # if r.statusCode == api.StatusCode.STNotInitialized.value: + # self.get_logger().warning("Not initialized") + # # sleep(2) + # return + # if r.statusCode == api.StatusCode.STInitialized.value and not self._configured: + # if self._confIterator is None: + # self.get_logger().info("Configuring") + # self._confIterator = iter(self.sendConfig()) + # # sleep(2) + # next(self._confIterator) + # if self._configured: + # self.get_logger().info("Done") + # return self._pos[0] = r.pos.x self._pos[1] = r.pos.y