From 960761f3bcdcc215087c47aae2cbdcfc37c05ebe Mon Sep 17 00:00:00 2001 From: ilithebutterfly Date: Thu, 9 May 2024 15:00:30 -0400 Subject: [PATCH] Changed api Fixed remote/local ID --- src/rove_control_board/api/api.h | 42 +++++++++---------- .../rove_control_board/api.py | 24 +++++------ .../rove_control_board/canutils.py | 40 ++++++++++++++++-- .../control_board_bridge.py | 19 +++++++++ 4 files changed, 89 insertions(+), 36 deletions(-) diff --git a/src/rove_control_board/api/api.h b/src/rove_control_board/api/api.h index 0d4928c..501491d 100644 --- a/src/rove_control_board/api/api.h +++ b/src/rove_control_board/api/api.h @@ -5,12 +5,12 @@ #include -enum StatusCode : euint32_t +enum StatusCode : euint16_t { STNone = 0, }; -enum ErrorCode : euint32_t +enum ErrorCode : euint16_t { ERNone = 0, ERAdapterNotInit = 1, @@ -91,10 +91,10 @@ static_assert(sizeof(Float) == 4); struct Vector2D { - eint32_t x; - eint32_t y; + eint16_t x; + eint16_t y; }; -static_assert(sizeof(Vector2D) == 8); +static_assert(sizeof(Vector2D) == 4); struct RGB { @@ -107,10 +107,10 @@ static_assert(sizeof(RGB) == 3); struct Report { Vector2D pos; - euint32_t statusCode; - euint32_t errorCode; + euint16_t statusCode; + euint16_t errorCode; }; -static_assert(sizeof(Report) == 16); +static_assert(sizeof(Report) == 8); struct Bounds { @@ -127,7 +127,7 @@ ULong hashCheck(Void); static_assert((sizeof(Void)+1) == 2); Bool_ setServoPosition(Vector2D); -static_assert((sizeof(Vector2D)+1) == 9); +static_assert((sizeof(Vector2D)+1) == 5); Vector2D getServoPosition(Void); static_assert((sizeof(Void)+1) == 2); @@ -136,21 +136,21 @@ Bool_ setServoPositionZero(Void); static_assert((sizeof(Void)+1) == 2); Bool_ setServoSpeed(Vector2D); -static_assert((sizeof(Vector2D)+1) == 9); +static_assert((sizeof(Vector2D)+1) == 5); Vector2D getServoSpeed(Void); static_assert((sizeof(Void)+1) == 2); -Int getServoPositionX(Void); +Short getServoPositionX(Void); static_assert((sizeof(Void)+1) == 2); -Int getServoPositionY(Void); +Short getServoPositionY(Void); static_assert((sizeof(Void)+1) == 2); -Int getServoSpeedX(Void); +Short getServoSpeedX(Void); static_assert((sizeof(Void)+1) == 2); -Int getServoSpeedY(Void); +Short getServoSpeedY(Void); static_assert((sizeof(Void)+1) == 2); Bool_ setServoXAcc(Byte); @@ -194,10 +194,10 @@ static BaseFunction_ptr commands[] = { new Function(&setServoPositionZero), new Function(&setServoSpeed), new Function(&getServoSpeed), - new Function(&getServoPositionX), - new Function(&getServoPositionY), - new Function(&getServoSpeedX), - new Function(&getServoSpeedY), + new Function(&getServoPositionX), + new Function(&getServoPositionY), + new Function(&getServoSpeedX), + new Function(&getServoSpeedY), new Function(&setServoXAcc), new Function(&setServoYAcc), new Function(&getServoXAcc), @@ -211,6 +211,6 @@ static BaseFunction_ptr commands[] = { new Function(&getReport), }; #define COMMANDS_COUNT 22 -#define MAX_DECODED_SIZE 17 -#define MAX_ENCODED_SIZE 25 -#define API_HASH 9203740187175752639UL \ No newline at end of file +#define MAX_DECODED_SIZE 9 +#define MAX_ENCODED_SIZE 13 +#define API_HASH 6660753000537812969UL \ No newline at end of file diff --git a/src/rove_control_board/rove_control_board/api.py b/src/rove_control_board/rove_control_board/api.py index 8b9d79e..b42dedd 100644 --- a/src/rove_control_board/rove_control_board/api.py +++ b/src/rove_control_board/rove_control_board/api.py @@ -6,11 +6,11 @@ # manager = comm.SerialCommandManager() manager = CanBusCommandManager() -@manager.enum('I') +@manager.enum('H') class StatusCode(Enum): STNone = 0 -@manager.enum('I') +@manager.enum('H') class ErrorCode(Enum): ERNone = 0b00000000 ERAdapterNotInit = 0b00000001 @@ -23,7 +23,7 @@ class ServoControlMode(Enum): SCMPosition = 1 SCMSpeed = 2 -@manager.struct('ii') +@manager.struct('hh') class Vector2D(comm.BinaryData): def __init__(self, x:int=0, y:int=0): super().__init__(x=x, y=y) @@ -38,7 +38,7 @@ def __init__(self, r:int=0,g:int=0,b:int=0): self.g:int self.b:int -@manager.struct('_II') +@manager.struct('_HH') class Report(comm.BinaryData): def __init__(self, pos:Vector2D=Vector2D(), statusCode:int=0, errorCode:int=0): super().__init__(pos=pos, statusCode=statusCode, errorCode=errorCode) @@ -73,20 +73,20 @@ def setServoSpeed(speed:Vector2D) -> comm.Bool_: def getServoSpeed() -> Vector2D: pass -@manager.command(comm.Void, comm.Int) -def getServoPositionX() -> comm.Int: +@manager.command(comm.Void, comm.Short) +def getServoPositionX() -> comm.Short: pass -@manager.command(comm.Void, comm.Int) -def getServoPositionY() -> comm.Int: +@manager.command(comm.Void, comm.Short) +def getServoPositionY() -> comm.Short: pass -@manager.command(comm.Void, comm.Int) -def getServoSpeedX() -> comm.Int: +@manager.command(comm.Void, comm.Short) +def getServoSpeedX() -> comm.Short: pass -@manager.command(comm.Void, comm.Int) -def getServoSpeedY() -> comm.Int: +@manager.command(comm.Void, comm.Short) +def getServoSpeedY() -> comm.Short: pass @manager.command(comm.Byte, comm.Bool_) diff --git a/src/rove_control_board/rove_control_board/canutils.py b/src/rove_control_board/rove_control_board/canutils.py index 21ca6c8..cb7490d 100644 --- a/src/rove_control_board/rove_control_board/canutils.py +++ b/src/rove_control_board/rove_control_board/canutils.py @@ -33,7 +33,7 @@ ] class CanBusStream(RawIOBase, can.Listener): - def __init__(self, bus:can.BusABC, maxLenth:int, notifier:can.Notifier, timeout:float=1, remoteID:int=0x446): + def __init__(self, bus:can.BusABC, maxLenth:int, notifier:can.Notifier, timeout:float=1, remoteID:int=0x103, localID:int=0x446): RawIOBase.__init__(self) can.Listener.__init__(self) @@ -41,6 +41,7 @@ def __init__(self, bus:can.BusABC, maxLenth:int, notifier:can.Notifier, timeout: self._notifier = notifier self._timeout = timeout self._remoteID = remoteID + self._localID = localID self._readqueue = collections.deque(maxlen=maxLenth) self._writequeue = collections.deque(maxlen=maxLenth) self._notifier.add_listener(self) @@ -48,7 +49,7 @@ def __init__(self, bus:can.BusABC, maxLenth:int, notifier:can.Notifier, timeout: def on_message_received(self, msg: can.Message) -> None: if msg.is_error_frame: print(f"Error: {msg}") - if msg.arbitration_id == self._remoteID: + if msg.arbitration_id == self._localID: for b in msg.data: self._readqueue.append(b) @@ -128,6 +129,9 @@ def __init__(self): self._notifier:can.Notifier = None self._inUse = False self._bitrate = None + self._remoteID = 0x103 + self._localID = 0x446 + self._timeout = 1 @property def bitrate(self) -> int: @@ -160,6 +164,36 @@ def notifier(self) -> can.Notifier: @notifier.setter def notifier(self, noti:can.Notifier): self._notifier = noti + + @property + def remoteID(self) -> int: + return self._remoteID + + @remoteID.setter + def remoteID(self, id:int): + if self._stream is not None: + self._stream._remoteID = id + self._remoteID = id + + @property + def timeout(self) -> float: + return self._timeout + + @timeout.setter + def timeout(self, timeout:float): + if self._stream is not None: + self._stream._timeout = timeout + self._timeout = timeout + + @property + def localID(self) -> int: + return self._localID + + @localID.setter + def localID(self, id:int): + if self._stream is not None: + self._stream._localID = id + self._localID = id def _default(self): if self._bitrate is None: @@ -176,7 +210,7 @@ def _maxSize(self): def __enter__(self) -> IOBase: if self._stream is None: self._default() - self._stream = CanBusStream(self._bus, self._maxSize() * 2, self._notifier) + self._stream = CanBusStream(self._bus, self._maxSize() * 2, self._notifier, self._timeout, self._remoteID, self._localID) return self._stream.__enter__() def __exit__(self, *args, **kwargs): 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 e8a9aa2..b1c940e 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 @@ -2,6 +2,7 @@ from math import isinf, isnan, nan import math import os +from random import randint from time import sleep from types import TracebackType from typing import Any, Callable, NamedTuple, NoReturn, Tuple @@ -84,6 +85,9 @@ def __init__(self): self.led_strobe = self.create_publisher(Bool, 'led_strobe', 0) self.led_strobe_set = self.create_subscription(Bool, 'led_strobe_set', self.setLEDStrobe, 3) self.create_timer(1/30, self.statusReport) + self.create_timer(1/30, self.ping) + + # self.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) self._led_front = False self._led_back = False @@ -189,6 +193,12 @@ def statusReport(self): self.led_back.publish(self.getLEDBack()) self.led_strobe.publish(self.getLEDStrobe()) + @debugFunc + def ping(self): + from rove_control_board.api import manager + p = manager.ping() + self.get_logger().info(f"Ping: {round(p*1000, 2)} us") + def finddir(directory:str, prefix:str): directory = directory.removesuffix('/') lst = os.listdir(directory) @@ -335,7 +345,16 @@ def openSocket(): manager.interface = 'socketcan' manager.channel = chan manager.bitrate = CANBUS_BITRATE + manager.remoteID = 0x103 + manager.localID = 0x446 + manager.timeout = 2 p = manager.ping() + # pp = randint(-1000, 1000) + # print(f"Sending {pp}") + # manager._pingcmd(comm.Int(pp)) + + + print(f"Ping: {round(p*1000, 2)} us on {chan}") if not isnan(p): break