diff --git a/client_s2/.idea/.gitignore b/client_s2/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/client_s2/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/client_s2/.idea/client_s2.iml b/client_s2/.idea/client_s2.iml new file mode 100644 index 0000000..d0876a7 --- /dev/null +++ b/client_s2/.idea/client_s2.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/client_s2/.idea/inspectionProfiles/Project_Default.xml b/client_s2/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..9277599 --- /dev/null +++ b/client_s2/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,12 @@ + + + + \ No newline at end of file diff --git a/client_s2/.idea/inspectionProfiles/profiles_settings.xml b/client_s2/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/client_s2/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/client_s2/.idea/misc.xml b/client_s2/.idea/misc.xml new file mode 100644 index 0000000..d56657a --- /dev/null +++ b/client_s2/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/client_s2/.idea/modules.xml b/client_s2/.idea/modules.xml new file mode 100644 index 0000000..8417644 --- /dev/null +++ b/client_s2/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/client_s2/.idea/saveactions_settings.xml b/client_s2/.idea/saveactions_settings.xml new file mode 100644 index 0000000..b7b3f34 --- /dev/null +++ b/client_s2/.idea/saveactions_settings.xml @@ -0,0 +1,15 @@ + + + + + + \ No newline at end of file diff --git a/client_s2/.idea/vcs.xml b/client_s2/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/client_s2/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/client_s2/CompLib.proto b/client_s2/CompLib.proto index 77a202d..1bb1bd0 100644 --- a/client_s2/CompLib.proto +++ b/client_s2/CompLib.proto @@ -20,17 +20,65 @@ message GenericResponse { Status status = 2; } -message ReadSensorsRequest { +message EncoderReadPositionsRequest { Header header = 1; } -message ReadSensorsResponse { +message EncoderReadPositionsResponse { Header header = 1; Status status = 2; - uint32 ir_1 = 3; - uint32 ir_2 = 4; - uint32 ir_3 = 5; - uint32 ir_4 = 6; - uint32 ir_5 = 7; + repeated int32 positions = 3 [packed = true]; +} + +message EncoderReadVelocitiesRequest { + Header header = 1; +} + +message EncoderReadVelocitiesResponse { + Header header = 1; + Status status = 2; + repeated double velocities = 3 [packed = true]; +} + +message IRSensorsEnableRequest { + Header header = 1; +} + +message IRSensorsDisableRequest { + Header header = 1; +} + +message IRSensorsReadAllRequest { + Header header = 1; +} + +message IRSensorsReadAllResponse { + Header header = 1; + Status status = 2; + repeated uint32 data = 3 [packed = true]; +} + +message MotorsSetPowerRequest { + Header header = 1; + uint32 port = 2; + double power = 3; +} + +message MotorsSetSpeedRequest { + Header header = 1; + uint32 port = 2; + double speed = 3; +} + +message OdometryReadRequest { + Header header = 1; +} + +message OdometryReadResponse { + Header header = 1; + Status status = 2; + double x_position = 3; + double y_position = 4; + double orientation = 5; } diff --git a/client_s2/CompLib_pb2.py b/client_s2/CompLib_pb2.py deleted file mode 100644 index ea548fd..0000000 --- a/client_s2/CompLib_pb2.py +++ /dev/null @@ -1,84 +0,0 @@ -# -*- coding: utf-8 -*- -# Generated by the protocol buffer compiler. DO NOT EDIT! -# source: CompLib.proto -"""Generated protocol buffer code.""" -from google.protobuf import descriptor as _descriptor -from google.protobuf import descriptor_pool as _descriptor_pool -from google.protobuf import message as _message -from google.protobuf import reflection as _reflection -from google.protobuf import symbol_database as _symbol_database -# @@protoc_insertion_point(imports) - -_sym_db = _symbol_database.Default() - - - - -DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rCompLib.proto\x12\x07\x43ompLib\"\x1e\n\x06Header\x12\x14\n\x0cmessage_type\x18\x01 \x01(\t\"3\n\x06Status\x12\x12\n\nsuccessful\x18\x01 \x01(\x08\x12\x15\n\rerror_message\x18\x02 \x01(\t\"1\n\x0eGenericRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"S\n\x0fGenericResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\"5\n\x12ReadSensorsRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"\x9d\x01\n\x13ReadSensorsResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x0c\n\x04ir_1\x18\x03 \x01(\r\x12\x0c\n\x04ir_2\x18\x04 \x01(\r\x12\x0c\n\x04ir_3\x18\x05 \x01(\r\x12\x0c\n\x04ir_4\x18\x06 \x01(\r\x12\x0c\n\x04ir_5\x18\x07 \x01(\rb\x06proto3') - - - -_HEADER = DESCRIPTOR.message_types_by_name['Header'] -_STATUS = DESCRIPTOR.message_types_by_name['Status'] -_GENERICREQUEST = DESCRIPTOR.message_types_by_name['GenericRequest'] -_GENERICRESPONSE = DESCRIPTOR.message_types_by_name['GenericResponse'] -_READSENSORSREQUEST = DESCRIPTOR.message_types_by_name['ReadSensorsRequest'] -_READSENSORSRESPONSE = DESCRIPTOR.message_types_by_name['ReadSensorsResponse'] -Header = _reflection.GeneratedProtocolMessageType('Header', (_message.Message,), { - 'DESCRIPTOR' : _HEADER, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.Header) - }) -_sym_db.RegisterMessage(Header) - -Status = _reflection.GeneratedProtocolMessageType('Status', (_message.Message,), { - 'DESCRIPTOR' : _STATUS, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.Status) - }) -_sym_db.RegisterMessage(Status) - -GenericRequest = _reflection.GeneratedProtocolMessageType('GenericRequest', (_message.Message,), { - 'DESCRIPTOR' : _GENERICREQUEST, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.GenericRequest) - }) -_sym_db.RegisterMessage(GenericRequest) - -GenericResponse = _reflection.GeneratedProtocolMessageType('GenericResponse', (_message.Message,), { - 'DESCRIPTOR' : _GENERICRESPONSE, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.GenericResponse) - }) -_sym_db.RegisterMessage(GenericResponse) - -ReadSensorsRequest = _reflection.GeneratedProtocolMessageType('ReadSensorsRequest', (_message.Message,), { - 'DESCRIPTOR' : _READSENSORSREQUEST, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.ReadSensorsRequest) - }) -_sym_db.RegisterMessage(ReadSensorsRequest) - -ReadSensorsResponse = _reflection.GeneratedProtocolMessageType('ReadSensorsResponse', (_message.Message,), { - 'DESCRIPTOR' : _READSENSORSRESPONSE, - '__module__' : 'CompLib_pb2' - # @@protoc_insertion_point(class_scope:CompLib.ReadSensorsResponse) - }) -_sym_db.RegisterMessage(ReadSensorsResponse) - -if _descriptor._USE_C_DESCRIPTORS == False: - - DESCRIPTOR._options = None - _HEADER._serialized_start=26 - _HEADER._serialized_end=56 - _STATUS._serialized_start=58 - _STATUS._serialized_end=109 - _GENERICREQUEST._serialized_start=111 - _GENERICREQUEST._serialized_end=160 - _GENERICRESPONSE._serialized_start=162 - _GENERICRESPONSE._serialized_end=245 - _READSENSORSREQUEST._serialized_start=247 - _READSENSORSREQUEST._serialized_end=300 - _READSENSORSRESPONSE._serialized_start=303 - _READSENSORSRESPONSE._serialized_end=460 -# @@protoc_insertion_point(module_scope) diff --git a/client_s2/__pycache__/CompLib_pb2.cpython-39.pyc b/client_s2/__pycache__/CompLib_pb2.cpython-39.pyc deleted file mode 100644 index 53c4d1a..0000000 Binary files a/client_s2/__pycache__/CompLib_pb2.cpython-39.pyc and /dev/null differ diff --git a/client_s2/compLib/CompLibClient.py b/client_s2/compLib/CompLibClient.py new file mode 100644 index 0000000..0dea16b --- /dev/null +++ b/client_s2/compLib/CompLibClient.py @@ -0,0 +1,45 @@ +import socket + +import compLib.CompLib_pb2 as CompLib_pb2 + +UNIX_SOCKET_PATH = "/tmp/compLib" +TCP_SOCKET_HOST = "192.168.0.151" +TCP_SOCKET_PORT = 9090 + + +class CompLibClient(object): + + @staticmethod + def send(data: bytes, size: int) -> bytes: + # with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + # sock.connect((TCP_SOCKET_HOST, TCP_SOCKET_PORT)) + + with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock: + sock.connect(UNIX_SOCKET_PATH) + + sock.sendall(size.to_bytes(1, byteorder='big')) + sock.sendall(data) + + response_size_bytes = sock.recv(1) + response_size = int.from_bytes(response_size_bytes, byteorder="big") + # print(response_size) + + response_bytes = sock.recv(response_size) + # print(response_bytes.hex()) + # print(len(response_bytes)) + + CompLibClient.check_response(response_bytes) + + return response_bytes + + @staticmethod + def check_response(response_bytes: bytes) -> bool: + print(f"{response_bytes}") + res = CompLib_pb2.GenericResponse() + res.ParseFromString(response_bytes) + + if res.status.successful: + return True + + # TODO: Log error message if unsuccessful + return False diff --git a/client_s2/compLib/CompLib_pb2.py b/client_s2/compLib/CompLib_pb2.py new file mode 100644 index 0000000..eaa5fe8 --- /dev/null +++ b/client_s2/compLib/CompLib_pb2.py @@ -0,0 +1,190 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: CompLib.proto +"""Generated protocol buffer code.""" +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rCompLib.proto\x12\x07\x43ompLib\"\x1e\n\x06Header\x12\x14\n\x0cmessage_type\x18\x01 \x01(\t\"3\n\x06Status\x12\x12\n\nsuccessful\x18\x01 \x01(\x08\x12\x15\n\rerror_message\x18\x02 \x01(\t\"1\n\x0eGenericRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"S\n\x0fGenericResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\">\n\x1b\x45ncoderReadPositionsRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"w\n\x1c\x45ncoderReadPositionsResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x15\n\tpositions\x18\x03 \x03(\x05\x42\x02\x10\x01\"?\n\x1c\x45ncoderReadVelocitiesRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"y\n\x1d\x45ncoderReadVelocitiesResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x16\n\nvelocities\x18\x03 \x03(\x01\x42\x02\x10\x01\"9\n\x16IRSensorsEnableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsDisableRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\":\n\x17IRSensorsReadAllRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"n\n\x18IRSensorsReadAllResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x10\n\x04\x64\x61ta\x18\x03 \x03(\rB\x02\x10\x01\"U\n\x15MotorsSetPowerRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05power\x18\x03 \x01(\x01\"U\n\x15MotorsSetSpeedRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x0c\n\x04port\x18\x02 \x01(\r\x12\r\n\x05speed\x18\x03 \x01(\x01\"6\n\x13OdometryReadRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\"\x95\x01\n\x14OdometryReadResponse\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1f\n\x06status\x18\x02 \x01(\x0b\x32\x0f.CompLib.Status\x12\x12\n\nx_position\x18\x03 \x01(\x01\x12\x12\n\ny_position\x18\x04 \x01(\x01\x12\x13\n\x0borientation\x18\x05 \x01(\x01\x62\x06proto3') + + + +_HEADER = DESCRIPTOR.message_types_by_name['Header'] +_STATUS = DESCRIPTOR.message_types_by_name['Status'] +_GENERICREQUEST = DESCRIPTOR.message_types_by_name['GenericRequest'] +_GENERICRESPONSE = DESCRIPTOR.message_types_by_name['GenericResponse'] +_ENCODERREADPOSITIONSREQUEST = DESCRIPTOR.message_types_by_name['EncoderReadPositionsRequest'] +_ENCODERREADPOSITIONSRESPONSE = DESCRIPTOR.message_types_by_name['EncoderReadPositionsResponse'] +_ENCODERREADVELOCITIESREQUEST = DESCRIPTOR.message_types_by_name['EncoderReadVelocitiesRequest'] +_ENCODERREADVELOCITIESRESPONSE = DESCRIPTOR.message_types_by_name['EncoderReadVelocitiesResponse'] +_IRSENSORSENABLEREQUEST = DESCRIPTOR.message_types_by_name['IRSensorsEnableRequest'] +_IRSENSORSDISABLEREQUEST = DESCRIPTOR.message_types_by_name['IRSensorsDisableRequest'] +_IRSENSORSREADALLREQUEST = DESCRIPTOR.message_types_by_name['IRSensorsReadAllRequest'] +_IRSENSORSREADALLRESPONSE = DESCRIPTOR.message_types_by_name['IRSensorsReadAllResponse'] +_MOTORSSETPOWERREQUEST = DESCRIPTOR.message_types_by_name['MotorsSetPowerRequest'] +_MOTORSSETSPEEDREQUEST = DESCRIPTOR.message_types_by_name['MotorsSetSpeedRequest'] +_ODOMETRYREADREQUEST = DESCRIPTOR.message_types_by_name['OdometryReadRequest'] +_ODOMETRYREADRESPONSE = DESCRIPTOR.message_types_by_name['OdometryReadResponse'] +Header = _reflection.GeneratedProtocolMessageType('Header', (_message.Message,), { + 'DESCRIPTOR' : _HEADER, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.Header) + }) +_sym_db.RegisterMessage(Header) + +Status = _reflection.GeneratedProtocolMessageType('Status', (_message.Message,), { + 'DESCRIPTOR' : _STATUS, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.Status) + }) +_sym_db.RegisterMessage(Status) + +GenericRequest = _reflection.GeneratedProtocolMessageType('GenericRequest', (_message.Message,), { + 'DESCRIPTOR' : _GENERICREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.GenericRequest) + }) +_sym_db.RegisterMessage(GenericRequest) + +GenericResponse = _reflection.GeneratedProtocolMessageType('GenericResponse', (_message.Message,), { + 'DESCRIPTOR' : _GENERICRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.GenericResponse) + }) +_sym_db.RegisterMessage(GenericResponse) + +EncoderReadPositionsRequest = _reflection.GeneratedProtocolMessageType('EncoderReadPositionsRequest', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADPOSITIONSREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadPositionsRequest) + }) +_sym_db.RegisterMessage(EncoderReadPositionsRequest) + +EncoderReadPositionsResponse = _reflection.GeneratedProtocolMessageType('EncoderReadPositionsResponse', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADPOSITIONSRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadPositionsResponse) + }) +_sym_db.RegisterMessage(EncoderReadPositionsResponse) + +EncoderReadVelocitiesRequest = _reflection.GeneratedProtocolMessageType('EncoderReadVelocitiesRequest', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADVELOCITIESREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadVelocitiesRequest) + }) +_sym_db.RegisterMessage(EncoderReadVelocitiesRequest) + +EncoderReadVelocitiesResponse = _reflection.GeneratedProtocolMessageType('EncoderReadVelocitiesResponse', (_message.Message,), { + 'DESCRIPTOR' : _ENCODERREADVELOCITIESRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.EncoderReadVelocitiesResponse) + }) +_sym_db.RegisterMessage(EncoderReadVelocitiesResponse) + +IRSensorsEnableRequest = _reflection.GeneratedProtocolMessageType('IRSensorsEnableRequest', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSENABLEREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsEnableRequest) + }) +_sym_db.RegisterMessage(IRSensorsEnableRequest) + +IRSensorsDisableRequest = _reflection.GeneratedProtocolMessageType('IRSensorsDisableRequest', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSDISABLEREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsDisableRequest) + }) +_sym_db.RegisterMessage(IRSensorsDisableRequest) + +IRSensorsReadAllRequest = _reflection.GeneratedProtocolMessageType('IRSensorsReadAllRequest', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSREADALLREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsReadAllRequest) + }) +_sym_db.RegisterMessage(IRSensorsReadAllRequest) + +IRSensorsReadAllResponse = _reflection.GeneratedProtocolMessageType('IRSensorsReadAllResponse', (_message.Message,), { + 'DESCRIPTOR' : _IRSENSORSREADALLRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.IRSensorsReadAllResponse) + }) +_sym_db.RegisterMessage(IRSensorsReadAllResponse) + +MotorsSetPowerRequest = _reflection.GeneratedProtocolMessageType('MotorsSetPowerRequest', (_message.Message,), { + 'DESCRIPTOR' : _MOTORSSETPOWERREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.MotorsSetPowerRequest) + }) +_sym_db.RegisterMessage(MotorsSetPowerRequest) + +MotorsSetSpeedRequest = _reflection.GeneratedProtocolMessageType('MotorsSetSpeedRequest', (_message.Message,), { + 'DESCRIPTOR' : _MOTORSSETSPEEDREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.MotorsSetSpeedRequest) + }) +_sym_db.RegisterMessage(MotorsSetSpeedRequest) + +OdometryReadRequest = _reflection.GeneratedProtocolMessageType('OdometryReadRequest', (_message.Message,), { + 'DESCRIPTOR' : _ODOMETRYREADREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.OdometryReadRequest) + }) +_sym_db.RegisterMessage(OdometryReadRequest) + +OdometryReadResponse = _reflection.GeneratedProtocolMessageType('OdometryReadResponse', (_message.Message,), { + 'DESCRIPTOR' : _ODOMETRYREADRESPONSE, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.OdometryReadResponse) + }) +_sym_db.RegisterMessage(OdometryReadResponse) + +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _ENCODERREADPOSITIONSRESPONSE.fields_by_name['positions']._options = None + _ENCODERREADPOSITIONSRESPONSE.fields_by_name['positions']._serialized_options = b'\020\001' + _ENCODERREADVELOCITIESRESPONSE.fields_by_name['velocities']._options = None + _ENCODERREADVELOCITIESRESPONSE.fields_by_name['velocities']._serialized_options = b'\020\001' + _IRSENSORSREADALLRESPONSE.fields_by_name['data']._options = None + _IRSENSORSREADALLRESPONSE.fields_by_name['data']._serialized_options = b'\020\001' + _HEADER._serialized_start=26 + _HEADER._serialized_end=56 + _STATUS._serialized_start=58 + _STATUS._serialized_end=109 + _GENERICREQUEST._serialized_start=111 + _GENERICREQUEST._serialized_end=160 + _GENERICRESPONSE._serialized_start=162 + _GENERICRESPONSE._serialized_end=245 + _ENCODERREADPOSITIONSREQUEST._serialized_start=247 + _ENCODERREADPOSITIONSREQUEST._serialized_end=309 + _ENCODERREADPOSITIONSRESPONSE._serialized_start=311 + _ENCODERREADPOSITIONSRESPONSE._serialized_end=430 + _ENCODERREADVELOCITIESREQUEST._serialized_start=432 + _ENCODERREADVELOCITIESREQUEST._serialized_end=495 + _ENCODERREADVELOCITIESRESPONSE._serialized_start=497 + _ENCODERREADVELOCITIESRESPONSE._serialized_end=618 + _IRSENSORSENABLEREQUEST._serialized_start=620 + _IRSENSORSENABLEREQUEST._serialized_end=677 + _IRSENSORSDISABLEREQUEST._serialized_start=679 + _IRSENSORSDISABLEREQUEST._serialized_end=737 + _IRSENSORSREADALLREQUEST._serialized_start=739 + _IRSENSORSREADALLREQUEST._serialized_end=797 + _IRSENSORSREADALLRESPONSE._serialized_start=799 + _IRSENSORSREADALLRESPONSE._serialized_end=909 + _MOTORSSETPOWERREQUEST._serialized_start=911 + _MOTORSSETPOWERREQUEST._serialized_end=996 + _MOTORSSETSPEEDREQUEST._serialized_start=998 + _MOTORSSETSPEEDREQUEST._serialized_end=1083 + _ODOMETRYREADREQUEST._serialized_start=1085 + _ODOMETRYREADREQUEST._serialized_end=1139 + _ODOMETRYREADRESPONSE._serialized_start=1142 + _ODOMETRYREADRESPONSE._serialized_end=1291 +# @@protoc_insertion_point(module_scope) diff --git a/client_s2/compLib/Encoder.py b/client_s2/compLib/Encoder.py new file mode 100644 index 0000000..d1cf404 --- /dev/null +++ b/client_s2/compLib/Encoder.py @@ -0,0 +1,35 @@ +import compLib.CompLib_pb2 as CompLib_pb2 +from compLib.CompLibClient import CompLibClient + + +class Encoder(object): + """Class used to read the encoders + """ + + @staticmethod + def read_all_positions(): + """Read all encoder positions. + + :return: Tuple of all current encoder positions + """ + request = CompLib_pb2.EncoderReadPositionsRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + response = CompLib_pb2.EncoderReadPositionsResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + return tuple(i for i in response.positions) + + @staticmethod + def read_all_velocities(): + """Read the velocity of all motors connected. + + :return: Tuple of all current motor velocities + """ + request = CompLib_pb2.EncoderReadVelocitiesRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + response = CompLib_pb2.EncoderReadVelocitiesResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + return tuple(i for i in response.velocities) diff --git a/client_s2/compLib/IRSensor.py b/client_s2/compLib/IRSensor.py new file mode 100644 index 0000000..840c839 --- /dev/null +++ b/client_s2/compLib/IRSensor.py @@ -0,0 +1,39 @@ +import compLib.CompLib_pb2 as CompLib_pb2 +from compLib.CompLibClient import CompLibClient + + +class IRSensor(object): + """Access the different IR Sensors of the robot + """ + + @staticmethod + def read_all(): + """Read all IR sensors at once. + + :return: Tuple of all current ir sensors + """ + request = CompLib_pb2.IRSensorsReadAllRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + response = CompLib_pb2.IRSensorsReadAllResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + return tuple(i for i in response.data) + + @staticmethod + def enable(): + """Turn on all IR emitters + """ + request = CompLib_pb2.IRSensorsEnableRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) + + @staticmethod + def disable(): + """Turn off all IR emitters + """ + request = CompLib_pb2.IRSensorsDisableRequest() + request.header.message_type = request.DESCRIPTOR.full_name + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) diff --git a/client_s2/compLib/Motor.py b/client_s2/compLib/Motor.py new file mode 100644 index 0000000..27ce586 --- /dev/null +++ b/client_s2/compLib/Motor.py @@ -0,0 +1,69 @@ +import compLib.CompLib_pb2 as CompLib_pb2 +from compLib.CompLibClient import CompLibClient + +MOTOR_COUNT = 4 + + +class Motor(object): + """Class used to control the motors + """ + + @staticmethod + def power(port: int, percent: float): + """Set specified motor to percentage power + + :param port: Port, which the motor is connected to. 0-3 + :param percent: Percentage of max speed. between -100 and 100 + :raises: IndexError + """ + + if port < 0 or port >= MOTOR_COUNT: + raise IndexError("Invalid Motor port specified!") + + if percent < -100 or percent > 100: + raise IndexError("Invalid Motor speed specified! Speed is between -100 and 100 percent!") + + request = CompLib_pb2.MotorsSetPowerRequest() + request.header.message_type = request.DESCRIPTOR.full_name + request.port = port + request.power = percent + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) + + @staticmethod + def speed(port: int, speed: float): + """Set specified motor to percentage power + + :param port: Port, which the motor is connected to. 0-3 + :param speed: Speed at which a motor should turn in RPM + :raises: IndexError + """ + + if port < 0 or port >= MOTOR_COUNT: + raise IndexError("Invalid Motor port specified!") + + request = CompLib_pb2.MotorsSetSpeedRequest() + request.header.message_type = request.DESCRIPTOR.full_name + request.port = port + request.speed = speed + + CompLibClient.send(request.SerializeToString(), request.ByteSize()) + + # @staticmethod + # def all_off(): + # """ + # Turns of all motors + # """ + # Logging.get_logger().debug(f"Motor.all_off") + # + # for i in range(1, MOTOR_COUNT + 1): + # Motor.active_break(i) + + # @staticmethod + # def active_break(port: int): + # """ + # Actively break with a specific motor + # + # :param port: Port, which the motor is connected to. 1-4 + # """ + # Motor.pwm(port, 0, MotorMode.BREAK) diff --git a/client_s2/compLib/__init__.py b/client_s2/compLib/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc b/client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc new file mode 100644 index 0000000..e506fdc Binary files /dev/null and b/client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc differ diff --git a/client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc b/client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc new file mode 100644 index 0000000..5e710a8 Binary files /dev/null and b/client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc differ diff --git a/client_s2/compLib/__pycache__/Encoder.cpython-39.pyc b/client_s2/compLib/__pycache__/Encoder.cpython-39.pyc new file mode 100644 index 0000000..737675b Binary files /dev/null and b/client_s2/compLib/__pycache__/Encoder.cpython-39.pyc differ diff --git a/client_s2/compLib/__pycache__/IRSensor.cpython-39.pyc b/client_s2/compLib/__pycache__/IRSensor.cpython-39.pyc new file mode 100644 index 0000000..9f4e309 Binary files /dev/null and b/client_s2/compLib/__pycache__/IRSensor.cpython-39.pyc differ diff --git a/client_s2/compLib/__pycache__/Motor.cpython-39.pyc b/client_s2/compLib/__pycache__/Motor.cpython-39.pyc new file mode 100644 index 0000000..a989671 Binary files /dev/null and b/client_s2/compLib/__pycache__/Motor.cpython-39.pyc differ diff --git a/client_s2/compLib/__pycache__/__init__.cpython-39.pyc b/client_s2/compLib/__pycache__/__init__.cpython-39.pyc new file mode 100644 index 0000000..f848f0a Binary files /dev/null and b/client_s2/compLib/__pycache__/__init__.cpython-39.pyc differ diff --git a/client_s2/main.py b/client_s2/main.py index 5bbd783..dd3e732 100644 --- a/client_s2/main.py +++ b/client_s2/main.py @@ -1,36 +1,48 @@ -import socket -import sys -import os -import CompLib_pb2 - - SOCKET_PATH = "/tmp/compLib" -def send(data, size): - with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock: - sock.connect(SOCKET_PATH) - sock.sendall(size.to_bytes(1, byteorder='big')) - sock.sendall(data) - - responseSizeBytes = sock.recv(1) - responseSize = int.from_bytes(responseSizeBytes, byteorder="big") - print(responseSize) - - responseBytes = sock.recv(responseSize) - genericResponse = CompLib_pb2.GenericResponse() - - genericResponse.ParseFromString(responseBytes) - print(genericResponse) - - # reponseBytes = +# def send(data, size): +# with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock: +# sock.connect(SOCKET_PATH) +# sock.sendall(size.to_bytes(1, byteorder='big')) +# sock.sendall(data) +# +# response_size_bytes = sock.recv(1) +# response_size = int.from_bytes(response_size_bytes, byteorder="big") +# # print(f"Response size: {response_size}") +# +# response_bytes = sock.recv(response_size) +# generic_response = CompLib_pb2.GenericResponse() +# +# generic_response.ParseFromString(response_bytes) +# # print(f"Response: {generic_response}") +# +# # reponseBytes = def main(): - readSensorsRequest = CompLib_pb2.ReadSensorsRequest() - # readSensorsRequest.header = CompLib_pb2.Header() - readSensorsRequest.header.message_type = readSensorsRequest.DESCRIPTOR.full_name - send(readSensorsRequest.SerializeToString(), readSensorsRequest.ByteSize()) + # encoder_read_positions_request = CompLib_pb2.EncoderReadPositionsRequest() + # # readSensorsRequest.header = CompLib_pb2.Header() + # encoder_read_positions_request.header.message_type = encoder_read_positions_request.DESCRIPTOR.full_name + # + # start_time = time.time() + # for i in range(100000): + # send(encoder_read_positions_request.SerializeToString(), encoder_read_positions_request.ByteSize()) + # print("--- %s seconds ---" % (time.time() - start_time)) + + # from compLib.IRSensor import IRSensor + # IRSensor.read_all() + # + from compLib.Encoder import Encoder + Encoder.read_all_positions() + Encoder.read_all_velocities() + + # from compLib.Motor import Motor + # Motor.speed(0, 50) + # Motor.speed(3, -50) + + # time.sleep(10) -main() +if __name__ == '__main__': + main() diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt index 629923a..a5ab68b 100644 --- a/server_v2/CMakeLists.txt +++ b/server_v2/CMakeLists.txt @@ -10,13 +10,26 @@ if (NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif () -set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") -set(CMAKE_CXX_FLAGS_DEBUG "-g") -set(CMAKE_CXX_FLAGS_RELEASE "-O3") - find_package(Protobuf REQUIRED) find_package(spdlog REQUIRED) -find_package(pigpio REQUIRED) + +if (APPLE) + set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") + set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -gdwarf-3") + set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Wall -Wextra -gdwarf-3") + + set(LIBRARIES "spdlog::spdlog") +elseif (UNIX) + message("Running on Raspberry Pi") + add_definitions(-DIS_RASPI) + set(CMAKE_CXX_FLAGS_RELEASE "-O3") + + find_package(pigpio REQUIRED) + set(LIBRARIES "pigpio" "spdlog::spdlog") + set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") +endif () + +message("CMAKE_SYSTEM_PROCESSOR = ${CMAKE_SYSTEM_PROCESSOR}") set(PROTO_FILES protos/CompLib.proto @@ -35,7 +48,8 @@ set(SRC_FILES src/Odometry.cpp src/communication/MessageProcessor.cpp src/communication/MessageBuilder.cpp - src/communication/UnixSocketServer.cpp) + src/communication/UnixSocketServer.cpp + src/communication/TCPSocketServer.cpp) set(HDR_FILES include/spi.hpp @@ -52,7 +66,8 @@ set(HDR_FILES include/Odometry.hpp include/communication/MessageProcessor.hpp include/communication/MessageBuilder.hpp - include/communication/UnixSocketServer.hpp) + include/communication/UnixSocketServer.hpp + include/communication/TCPSocketServer.hpp) include_directories(third_party/asio) @@ -63,4 +78,4 @@ target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PUBLIC ${PROTOBUF_INCLUDE_DIRS} ) -target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES} pigpio spdlog::spdlog) +target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES} ${LIBRARIES}) diff --git a/server_v2/include/communication/MessageBuilder.hpp b/server_v2/include/communication/MessageBuilder.hpp index 93912f4..37106bc 100644 --- a/server_v2/include/communication/MessageBuilder.hpp +++ b/server_v2/include/communication/MessageBuilder.hpp @@ -1,6 +1,8 @@ #ifndef COMPLIB_SERVER_MESSAGEBUILDER_HPP #define COMPLIB_SERVER_MESSAGEBUILDER_HPP +#include +#include #include "CompLib.pb.h" #include "include/Robot.hpp" diff --git a/server_v2/include/communication/TCPSocketServer.hpp b/server_v2/include/communication/TCPSocketServer.hpp new file mode 100644 index 0000000..a1c0384 --- /dev/null +++ b/server_v2/include/communication/TCPSocketServer.hpp @@ -0,0 +1,20 @@ +#ifndef COMPLIB_SERVER_TCPSOCKETSERVER_HPP +#define COMPLIB_SERVER_TCPSOCKETSERVER_HPP + +#include + +#define SOCKET_PATH "/tmp/compLib" +#define SOCKET_BUFFER_SIZE 128 + +class TCPSocketServer { +public: + TCPSocketServer(); + +private: + [[noreturn]] void server_loop(); + + std::thread server_thread; +}; + + +#endif //COMPLIB_SERVER_TCPSOCKETSERVER_HPP diff --git a/server_v2/include/communication/UnixSocketServer.hpp b/server_v2/include/communication/UnixSocketServer.hpp index c734db9..bae3642 100644 --- a/server_v2/include/communication/UnixSocketServer.hpp +++ b/server_v2/include/communication/UnixSocketServer.hpp @@ -4,7 +4,7 @@ #include #define SOCKET_PATH "/tmp/compLib" -#define SOCKET_BUFFER_SIZE 64 +#define SOCKET_BUFFER_SIZE 128 class UnixSocketServer { public: diff --git a/server_v2/include/reset.hpp b/server_v2/include/reset.hpp index 07f1580..b8e9cc3 100644 --- a/server_v2/include/reset.hpp +++ b/server_v2/include/reset.hpp @@ -1,9 +1,6 @@ #ifndef COMPLIB_SERVER_RESET_HPP #define COMPLIB_SERVER_RESET_HPP -#include -#include - #define RESET_PIN 23 #define BOOT_PIN 17 @@ -11,21 +8,33 @@ #define RESET_STARTUP_SLEEP_TIME_US 1000 * 500 namespace Reset { - void reset_robot() { - gpioInitialise(); +#ifdef IS_RASPI - gpioSetMode(BOOT_PIN, PI_OUTPUT); - gpioSetMode(RESET_PIN, PI_OUTPUT); +#include +#include - gpioWrite(BOOT_PIN, 0); - gpioWrite(RESET_PIN, 0); + void reset_robot() { + gpioInitialise(); - usleep(RESET_SLEEP_TIME_US); + gpioSetMode(BOOT_PIN, PI_OUTPUT); + gpioSetMode(RESET_PIN, PI_OUTPUT); - gpioWrite(RESET_PIN, 1); + gpioWrite(BOOT_PIN, 0); + gpioWrite(RESET_PIN, 0); - usleep(RESET_STARTUP_SLEEP_TIME_US); - } + usleep(RESET_SLEEP_TIME_US); + + gpioWrite(RESET_PIN, 1); + + usleep(RESET_STARTUP_SLEEP_TIME_US); + } + +#else + + void reset_robot() { + } + +#endif } diff --git a/server_v2/include/spi.hpp b/server_v2/include/spi.hpp index 376b222..856fff0 100644 --- a/server_v2/include/spi.hpp +++ b/server_v2/include/spi.hpp @@ -136,7 +136,7 @@ private: void clear_buffers(); - uint8_t calculate_checksum(uint8_t *data, uint8_t length); + static uint8_t calculate_checksum(uint8_t *data, uint8_t length); }; diff --git a/server_v2/src/communication/MessageBuilder.cpp b/server_v2/src/communication/MessageBuilder.cpp index da69270..1fd1df5 100644 --- a/server_v2/src/communication/MessageBuilder.cpp +++ b/server_v2/src/communication/MessageBuilder.cpp @@ -1,3 +1,5 @@ +#include + #include "include/communication/MessageBuilder.hpp" using namespace CompLib; @@ -36,17 +38,17 @@ CompLib::EncoderReadPositionsResponse *MessageBuilder::encoder_read_positions_re response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name())); response->set_allocated_status(default_successful_status()); for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - response->set_positions(i, positions.at(i)); + response->add_positions(positions.at(i)); } return response; } CompLib::EncoderReadVelocitiesResponse *MessageBuilder::encoder_read_velocities_response(std::array velocities) { auto response = new EncoderReadVelocitiesResponse(); - response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name())); + response->set_allocated_header(header(EncoderReadVelocitiesResponse::descriptor()->full_name())); response->set_allocated_status(default_successful_status()); for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { - response->set_velocities(i, velocities.at(i)); + response->add_velocities(velocities.at(i)); } return response; } @@ -56,7 +58,7 @@ CompLib::IRSensorsReadAllResponse *MessageBuilder::ir_sensors_read_all_response( response->set_allocated_header(header(IRSensorsReadAllResponse::descriptor()->full_name())); response->set_allocated_status(default_successful_status()); for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) { - response->set_data(i, data.at(i)); + response->add_data(data.at(i)); } return response; } diff --git a/server_v2/src/communication/MessageProcessor.cpp b/server_v2/src/communication/MessageProcessor.cpp index bb5fb03..f5e7898 100644 --- a/server_v2/src/communication/MessageProcessor.cpp +++ b/server_v2/src/communication/MessageProcessor.cpp @@ -13,6 +13,7 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string & CompLib::GenericRequest message; message.ParseFromString(serializedMessage); auto messageTypeName = message.header().message_type(); + spdlog::debug("Got request with type {}", messageTypeName); try { // Encoder diff --git a/server_v2/src/communication/TCPSocketServer.cpp b/server_v2/src/communication/TCPSocketServer.cpp new file mode 100644 index 0000000..405123d --- /dev/null +++ b/server_v2/src/communication/TCPSocketServer.cpp @@ -0,0 +1,74 @@ +#include +#include +#include +#include + +#include + +#include "include/communication/TCPSocketServer.hpp" +#include "include/communication/MessageProcessor.hpp" + +#define SERVER_PORT 9090 +#define TCP_LISTEN_BACKLOG 5 + +TCPSocketServer::TCPSocketServer() { + server_thread = std::thread(&TCPSocketServer::server_loop, this); + server_thread.detach(); +} + +[[noreturn]] void TCPSocketServer::server_loop() { + struct sockaddr_in address; + int socket_file_descriptor = socket(AF_INET, SOCK_STREAM, 0); + if (socket_file_descriptor == 0) { + spdlog::error("TCP socket initialization failed!"); + exit(EXIT_FAILURE); + } + + int opt = 1; + if (int err = setsockopt(socket_file_descriptor, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) { + spdlog::error("TCP setsockopt failed! Err: {}", err); + exit(EXIT_FAILURE); + } + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(SERVER_PORT); + + if (bind(socket_file_descriptor, (struct sockaddr *) &address, sizeof(address)) < 0) { + spdlog::error("TCP bind failed!"); + exit(EXIT_FAILURE); + } + + if (listen(socket_file_descriptor, TCP_LISTEN_BACKLOG) < 0) { + spdlog::error("TCP listen failed!"); + exit(EXIT_FAILURE); + } + + char read_buffer[SOCKET_BUFFER_SIZE]; + char write_buffer[SOCKET_BUFFER_SIZE]; + for (;;) { + int client_file_descriptor = accept(socket_file_descriptor, NULL, NULL); + if (client_file_descriptor < 0) { + spdlog::error("TCP accept failed!"); + continue; + } + + read(client_file_descriptor, read_buffer, 1); + uint8_t message_size = read_buffer[0]; + + read(client_file_descriptor, read_buffer, read_buffer[0]); + std::string string_message; + for (int i{}; i < message_size; i++) { + string_message += read_buffer[i]; + } + + auto response = MessageProcessor::process_message(string_message); + uint8_t response_size = response->ByteSizeLong(); + write_buffer[0] = response_size; + write(client_file_descriptor, write_buffer, 1); + + response->SerializeToArray(write_buffer, SOCKET_BUFFER_SIZE); + write(client_file_descriptor, write_buffer, response_size); + + close(client_file_descriptor); + } +} \ No newline at end of file diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 0dddd15..157dda9 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -1,24 +1,31 @@ #include +#include #include "include/reset.hpp" -#include "include/Encoders.hpp" #include "include/OdometryController.hpp" #include "include/ClosedLoopMotorController.hpp" +#include "include/communication/UnixSocketServer.hpp" +#include "include/communication/TCPSocketServer.hpp" using namespace std; int main() { Reset::reset_robot(); spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v"); + spdlog::set_level(spdlog::level::debug); - ClosedLoopMotorController::getInstance().set_speed(0, -125); - ClosedLoopMotorController::getInstance().set_speed(3, 125); - OdometryController::getInstance().enable(); + UnixSocketServer unixSocketServer; + TCPSocketServer tcpSocketServer; - for (int i = 0; i < 10000; ++i) { - auto odom = OdometryController::getInstance().get(); - spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation()); - usleep(1000); - } +// ClosedLoopMotorController::getInstance().set_speed(0, -125); +// ClosedLoopMotorController::getInstance().set_speed(3, 125); +// OdometryController::getInstance().enable(); + +// for (int i = 0; i < 10000; ++i) { +// auto odom = OdometryController::getInstance().get(); +// spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation()); +// usleep(1000); +// } + std::this_thread::sleep_for(std::chrono::hours(12)); return 0; } diff --git a/server_v2/src/spi.cpp b/server_v2/src/spi.cpp index 9314905..1374925 100644 --- a/server_v2/src/spi.cpp +++ b/server_v2/src/spi.cpp @@ -1,8 +1,3 @@ -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port -#include //Needed for SPI port -#include #include #include @@ -10,104 +5,146 @@ #include "include/mathUtils.hpp" void check_for_error(int error_code, std::string error_message) { - if (error_code < 0) { - spdlog::error(error_message); - exit(1); - } -} - -Spi::Spi() { - int spi_mode = SPI_MODE_0; - int spi_bits_per_workd = SPI_BITS_PER_WORD; - int spi_speed = SPI_SPEED; - - check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ"); + if (error_code < 0) { + spdlog::error(error_message); + exit(1); + } } void Spi::clear_buffers() { - memset(tx_buffer, 0, SPI_BUFFER_SIZE); - memset(rx_buffer, 0, SPI_BUFFER_SIZE); + memset(tx_buffer, 0, SPI_BUFFER_SIZE); + memset(rx_buffer, 0, SPI_BUFFER_SIZE); } -uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) { - int sum = 0; - for (int i = 0; i < length; i++) { - sum += data[i]; - } - return sum % 256; +uint8_t Spi::calculate_checksum(uint8_t *data, uint8_t length) { + int sum = 0; + for (int i = 0; i < length; i++) { + sum += data[i]; + } + return sum % 256; +} + +#ifdef IS_RASPI + +#include //Needed for SPI port +#include //Needed for SPI port +#include //Needed for SPI port +#include //Needed for SPI port + +Spi::Spi() { + int spi_mode = SPI_MODE_0; + int spi_bits_per_workd = SPI_BITS_PER_WORD; + int spi_speed = SPI_SPEED; + + check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ"); } int Spi::read(uint8_t reg, uint8_t length) { - std::lock_guard lock(spi_mutex); + std::lock_guard lock(spi_mutex); - uint8_t read_buffer[SPI_BUFFER_SIZE] = {0}; - read_array(reg, length, read_buffer); + uint8_t read_buffer[SPI_BUFFER_SIZE] = {0}; + read_array(reg, length, read_buffer); - return mathUtils::int_from_bytes(read_buffer, length); + return mathUtils::int_from_bytes(read_buffer, length); } -void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) { - std::lock_guard lock(spi_mutex); +void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) { + std::lock_guard lock(spi_mutex); - clear_buffers(); - tx_buffer[0] = 0; - tx_buffer[1] = reg; - tx_buffer[2] = length; + clear_buffers(); + tx_buffer[0] = 0; + tx_buffer[1] = reg; + tx_buffer[2] = length; - transfer(); + transfer(); - uint8_t checksum = calculate_checksum(rx_buffer, length + 3); - if (checksum != rx_buffer[length + 3]) { - spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length +3], checksum); - } + uint8_t checksum = calculate_checksum(rx_buffer, length + 3); + if (checksum != rx_buffer[length + 3]) { + spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length + 3], checksum); + } - memcpy(data, rx_buffer + 2, length); + memcpy(data, rx_buffer + 2, length); } void Spi::write(uint8_t reg, uint8_t length, int value) { - std::lock_guard lock(spi_mutex); + std::lock_guard lock(spi_mutex); - uint8_t write_buffer[SPI_BUFFER_SIZE] = {0}; - mathUtils::bytes_from_int(value, length, write_buffer); - write_array(reg, length, write_buffer); + uint8_t write_buffer[SPI_BUFFER_SIZE] = {0}; + mathUtils::bytes_from_int(value, length, write_buffer); + write_array(reg, length, write_buffer); } -void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) { - std::lock_guard lock(spi_mutex); +void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) { + std::lock_guard lock(spi_mutex); - clear_buffers(); - tx_buffer[0] = 1; - tx_buffer[1] = reg; - tx_buffer[2] = length; - memcpy(tx_buffer + 3, data, length); + clear_buffers(); + tx_buffer[0] = 1; + tx_buffer[1] = reg; + tx_buffer[2] = length; + memcpy(tx_buffer + 3, data, length); - transfer(); + transfer(); } void Spi::transfer() { - std::lock_guard lock(spi_mutex); - - struct spi_ioc_transfer spi; - memset(&spi, 0, sizeof(spi)); - - spi.tx_buf = (unsigned long) tx_buffer; - spi.rx_buf = (unsigned long) rx_buffer; - spi.len = SPI_BUFFER_SIZE; - spi.delay_usecs = 0; - spi.speed_hz = SPI_SPEED; - spi.bits_per_word = SPI_BITS_PER_WORD; - spi.cs_change = 0; - - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); - check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); + std::lock_guard lock(spi_mutex); - if (tx_buffer[1] != rx_buffer[1]) { - spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]); - } + struct spi_ioc_transfer spi; + memset(&spi, 0, sizeof(spi)); + + spi.tx_buf = (unsigned long) tx_buffer; + spi.rx_buf = (unsigned long) rx_buffer; + spi.len = SPI_BUFFER_SIZE; + spi.delay_usecs = 0; + spi.speed_hz = SPI_SPEED; + spi.bits_per_word = SPI_BITS_PER_WORD; + spi.cs_change = 0; + + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); + check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data"); + + if (tx_buffer[1] != rx_buffer[1]) { + spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]); + } } + +#else + +Spi::Spi() {} + +int Spi::read(uint8_t reg, uint8_t length) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); + + return 0; +} + +void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); + clear_buffers(); + memcpy(data, rx_buffer, length); +} + +void Spi::write(uint8_t reg, uint8_t length, int value) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); +} + +void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); +} + +void Spi::transfer() { + std::lock_guard lock(spi_mutex); + spdlog::warn("Calling SPI without actual interface."); +} + +#endif \ No newline at end of file