Skip to content

Commit

Permalink
Changed api
Browse files Browse the repository at this point in the history 
Fixed remote/local ID
  • Loading branch information
@IliTheButterfly
IliTheButterfly committed May 9, 2024
1 parent f71f9e5 commit 960761f
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 36 deletions.
42 changes: 21 additions & 21 deletions src/rove_control_board/api/api.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,12 @@

#include <capra_comm.h>

enum StatusCode : euint32_t
enum StatusCode : euint16_t
{
STNone = 0,
};

enum ErrorCode : euint32_t
enum ErrorCode : euint16_t
{
ERNone = 0,
ERAdapterNotInit = 1,
Expand Down Expand Up @@ -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
{
Expand All @@ -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
{
Expand All @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -194,10 +194,10 @@ static BaseFunction_ptr commands[] = {
new Function<Bool_, Void>(&setServoPositionZero),
new Function<Bool_, Vector2D>(&setServoSpeed),
new Function<Vector2D, Void>(&getServoSpeed),
new Function<Int, Void>(&getServoPositionX),
new Function<Int, Void>(&getServoPositionY),
new Function<Int, Void>(&getServoSpeedX),
new Function<Int, Void>(&getServoSpeedY),
new Function<Short, Void>(&getServoPositionX),
new Function<Short, Void>(&getServoPositionY),
new Function<Short, Void>(&getServoSpeedX),
new Function<Short, Void>(&getServoSpeedY),
new Function<Bool_, Byte>(&setServoXAcc),
new Function<Bool_, Byte>(&setServoYAcc),
new Function<Byte, Void>(&getServoXAcc),
Expand All @@ -211,6 +211,6 @@ static BaseFunction_ptr commands[] = {
new Function<Report, Void>(&getReport),
};
#define COMMANDS_COUNT 22
#define MAX_DECODED_SIZE 17
#define MAX_ENCODED_SIZE 25
#define API_HASH 9203740187175752639UL
#define MAX_DECODED_SIZE 9
#define MAX_ENCODED_SIZE 13
#define API_HASH 6660753000537812969UL
24 changes: 12 additions & 12 deletions src/rove_control_board/rove_control_board/api.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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_)
Expand Down
40 changes: 37 additions & 3 deletions src/rove_control_board/rove_control_board/canutils.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,22 +33,23 @@
]

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)

self._bus = bus
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)

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)

Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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:
Expand All @@ -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):
Expand Down
19 changes: 19 additions & 0 deletions src/rove_control_board/rove_control_board/control_board_bridge.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 960761f

Please sign in to comment.