From d4c49a0a51b51d40d6a9091978a95651f47f20db Mon Sep 17 00:00:00 2001 From: Konstantin Lampalzer Date: Sat, 11 Jun 2022 23:52:57 +0200 Subject: [PATCH] Add movement class to python --- client_s2/.idea/misc.xml | 3 + client_s2/{ => compLib}/CompLib.proto | 17 ++++++ client_s2/compLib/CompLibClient.py | 8 +-- client_s2/compLib/CompLib_pb2.py | 32 +++++++++- client_s2/compLib/Movement.py | 57 ++++++++++++++++++ .../__pycache__/CompLibClient.cpython-39.pyc | Bin 1302 -> 1311 bytes .../__pycache__/CompLib_pb2.cpython-39.pyc | Bin 4335 -> 5022 bytes .../__pycache__/Movement.cpython-39.pyc | Bin 0 -> 2466 bytes client_s2/main.py | 50 +++++++++++++-- .../include/ClosedLoopMotorController.hpp | 2 + server_v2/include/Encoders.hpp | 4 ++ server_v2/include/Robot.hpp | 13 ++-- server_v2/src/ClosedLoopMotorController.cpp | 50 ++++++++------- server_v2/src/Encoders.cpp | 18 +++++- server_v2/src/GoToController.cpp | 14 +++-- server_v2/src/OdometryController.cpp | 8 +-- server_v2/src/PID.cpp | 8 ++- server_v2/src/main.cpp | 9 ++- 18 files changed, 240 insertions(+), 53 deletions(-) rename client_s2/{ => compLib}/CompLib.proto (80%) create mode 100644 client_s2/compLib/Movement.py create mode 100644 client_s2/compLib/__pycache__/Movement.cpython-39.pyc diff --git a/client_s2/.idea/misc.xml b/client_s2/.idea/misc.xml index d56657a..f6104af 100644 --- a/client_s2/.idea/misc.xml +++ b/client_s2/.idea/misc.xml @@ -1,4 +1,7 @@ + + \ No newline at end of file diff --git a/client_s2/CompLib.proto b/client_s2/compLib/CompLib.proto similarity index 80% rename from client_s2/CompLib.proto rename to client_s2/compLib/CompLib.proto index 1bb1bd0..8185499 100644 --- a/client_s2/CompLib.proto +++ b/client_s2/compLib/CompLib.proto @@ -82,3 +82,20 @@ message OdometryReadResponse { double orientation = 5; } +message DriveDistanceRequest { + Header header = 1; + double distance_m = 2; + double velocity_m_s = 3; +} + +message TurnDegreesRequest { + Header header = 1; + double angle_degrees = 2; + double velocity_rad_s = 3; +} + +message DriveRequest { + Header header = 1; + double linear_velocity_m_s = 2; + double angular_velocity_rad_s = 3; +} \ No newline at end of file diff --git a/client_s2/compLib/CompLibClient.py b/client_s2/compLib/CompLibClient.py index 0dea16b..00599c9 100644 --- a/client_s2/compLib/CompLibClient.py +++ b/client_s2/compLib/CompLibClient.py @@ -11,11 +11,11 @@ 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_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) + # 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) diff --git a/client_s2/compLib/CompLib_pb2.py b/client_s2/compLib/CompLib_pb2.py index eaa5fe8..ebe2d4c 100644 --- a/client_s2/compLib/CompLib_pb2.py +++ b/client_s2/compLib/CompLib_pb2.py @@ -14,7 +14,7 @@ _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') +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\"a\n\x14\x44riveDistanceRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x12\n\ndistance_m\x18\x02 \x01(\x01\x12\x14\n\x0cvelocity_m_s\x18\x03 \x01(\x01\"d\n\x12TurnDegreesRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x15\n\rangle_degrees\x18\x02 \x01(\x01\x12\x16\n\x0evelocity_rad_s\x18\x03 \x01(\x01\"l\n\x0c\x44riveRequest\x12\x1f\n\x06header\x18\x01 \x01(\x0b\x32\x0f.CompLib.Header\x12\x1b\n\x13linear_velocity_m_s\x18\x02 \x01(\x01\x12\x1e\n\x16\x61ngular_velocity_rad_s\x18\x03 \x01(\x01\x62\x06proto3') @@ -34,6 +34,9 @@ _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'] +_DRIVEDISTANCEREQUEST = DESCRIPTOR.message_types_by_name['DriveDistanceRequest'] +_TURNDEGREESREQUEST = DESCRIPTOR.message_types_by_name['TurnDegreesRequest'] +_DRIVEREQUEST = DESCRIPTOR.message_types_by_name['DriveRequest'] Header = _reflection.GeneratedProtocolMessageType('Header', (_message.Message,), { 'DESCRIPTOR' : _HEADER, '__module__' : 'CompLib_pb2' @@ -146,6 +149,27 @@ OdometryReadResponse = _reflection.GeneratedProtocolMessageType('OdometryReadRes }) _sym_db.RegisterMessage(OdometryReadResponse) +DriveDistanceRequest = _reflection.GeneratedProtocolMessageType('DriveDistanceRequest', (_message.Message,), { + 'DESCRIPTOR' : _DRIVEDISTANCEREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.DriveDistanceRequest) + }) +_sym_db.RegisterMessage(DriveDistanceRequest) + +TurnDegreesRequest = _reflection.GeneratedProtocolMessageType('TurnDegreesRequest', (_message.Message,), { + 'DESCRIPTOR' : _TURNDEGREESREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.TurnDegreesRequest) + }) +_sym_db.RegisterMessage(TurnDegreesRequest) + +DriveRequest = _reflection.GeneratedProtocolMessageType('DriveRequest', (_message.Message,), { + 'DESCRIPTOR' : _DRIVEREQUEST, + '__module__' : 'CompLib_pb2' + # @@protoc_insertion_point(class_scope:CompLib.DriveRequest) + }) +_sym_db.RegisterMessage(DriveRequest) + if _descriptor._USE_C_DESCRIPTORS == False: DESCRIPTOR._options = None @@ -187,4 +211,10 @@ if _descriptor._USE_C_DESCRIPTORS == False: _ODOMETRYREADREQUEST._serialized_end=1139 _ODOMETRYREADRESPONSE._serialized_start=1142 _ODOMETRYREADRESPONSE._serialized_end=1291 + _DRIVEDISTANCEREQUEST._serialized_start=1293 + _DRIVEDISTANCEREQUEST._serialized_end=1390 + _TURNDEGREESREQUEST._serialized_start=1392 + _TURNDEGREESREQUEST._serialized_end=1492 + _DRIVEREQUEST._serialized_start=1494 + _DRIVEREQUEST._serialized_end=1602 # @@protoc_insertion_point(module_scope) diff --git a/client_s2/compLib/Movement.py b/client_s2/compLib/Movement.py new file mode 100644 index 0000000..1b672f8 --- /dev/null +++ b/client_s2/compLib/Movement.py @@ -0,0 +1,57 @@ +import compLib.CompLib_pb2 as CompLib_pb2 +from compLib.CompLibClient import CompLibClient + + +class Movement(object): + """High level class to control movement of the robot + """ + + @staticmethod + def drive_distance(distance: float, speed: float): + """ + Drive a given distance with a certain speed. + Positive distance and speed with result in forward motion. Everything else will move backwards. + :param distance: Distance in meters + :param speed: Speed in meters per second + :return: None + """ + request = CompLib_pb2.DriveDistanceRequest() + request.header.message_type = request.DESCRIPTOR.full_name + request.distance_m = distance + request.velocity_m_s = speed + + response = CompLib_pb2.GenericResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + @staticmethod + def turn_degrees(degrees: float, speed: float): + """ + Turn specified degrees with a given speed. + Positive degrees and speed with result in counter-clockwise motion. Everything else will be clockwise + :param degrees: Degrees between -180 and 180 + :param speed: Speed in radians per second + :return: None + """ + request = CompLib_pb2.TurnDegreesRequest() + request.header.message_type = request.DESCRIPTOR.full_name + request.angle_degrees = degrees + request.velocity_rad_s = speed + + response = CompLib_pb2.GenericResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + + @staticmethod + def drive(linear: float, angular: float): + """ + Non-blocking way to perform a linear and angular motion at the same time. + :param linear: Linear speed in meters per second + :param angular: Angular speed in radians per second + :return: None + """ + request = CompLib_pb2.DriveDistanceRequest() + request.header.message_type = request.DESCRIPTOR.full_name + request.linear_velocity_m_s = linear + request.angular_velocity_rad_s = angular + + response = CompLib_pb2.GenericResponse() + response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) diff --git a/client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc b/client_s2/compLib/__pycache__/CompLibClient.cpython-39.pyc index e506fdc9ae9afd48ffded9fd59becc80c966070e..7dc2349dfa01ce2de7835d64207d05cd7acf1ff7 100644 GIT binary patch delta 383 zcmbQnHJ^(&k(ZZ?0SID#ElJYa$ScBFKLN-qVORjfS&Tr;w2-Mkr=a79SUCl|3;+lc^Wq69+y jJR@LE32+SYDB=WZ00rnRw)~{5)Z~&PCagw$V^IJAx|&!u delta 369 zcmbQwHI0imk(ZZ?0SK~|bR{WmhUe7#09=79$WdEoAD?sAZ~QTEJYwvXGIH z0mx#lVa#GnVN794VeVyui!nf1>?w>38EctKII=ivm?3IvS%A9OYk>M|SxdMUa8K@J zlxD1(yp&Pfisu$vaei`kYDpBkqg#BapJzlAcd);+cYJV2kgKC_6nk=hUS4W)$t?kp zTs%n1H6%X3F~noC6jKKy|K!C?Nvy0wj9g5UxtNt1`6lZzFO%W}`r#IHQEG7!Kad3> z1VAj_$?uqpRk=Y-L691jr2PDxB3`&6B;}LaSgbvTK{A{`B8or6IRIw6hktMgia>yW VP{=K|{G=?PLyMTOn)aVX0RR!WSo#0} diff --git a/client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc b/client_s2/compLib/__pycache__/CompLib_pb2.cpython-39.pyc index 5e710a8bba5d7be76f183f6169ecda4a99769213..272a68d45821155879845ef31f0e50a137431b8e 100644 GIT binary patch delta 1514 zcmY+^%TFtH6bJB`!tIrzFQyM#T1s2E(3bbJz`fkxeY7Gbl32h6n@QyyZ;GYGnL?tQ zZe2=dYs{_jA27tViSA8Ij4{NWiQBu^jc0!Sanvrp`TkBnbAHoJ-{ilV3pIj4L!-|R z-+lApk83xfjQ;!0{9nEBc?6Ov?KF>GOHnJxHbHF~M_$jsTlnz&(MLLtK^ih&`uans zvlx6mhqHZM*Q6Lx!emjDqR7HfBFds9MXASRRFN2yvLs~|0VU!r=B1ct5mX|~^AbOZ)vMR|clc-vf zVX-E~8jF|`lPuPySZ5JeBFkb!iVY=tGmubZipi!Vo4jOPi5!b9DYjT7mB_QGNKs*t zQlh|O+Y>$9W-_73G?N{RQ7 z+@;;=cHFDsGvB-Z+q>c5gRw%xh*j<8HCCHWx6wXB_veSjifI}KsDX8n^yT!TZpMtj zHMTlu&F-hxh2hXjT;>Ya<@Pd{R zkT2j}ykn+~Xsg-AhHWwS?i##7)=1FitJZ(NRjY3Q7;*H<|9?syKe vz#gZ2?W7ke_wUe!zwFhr+YVd$q5D&K-F*}GyLzPW<|0}5>&ScV_mRLqCqtvx delta 957 zcmX}rJ8#oa6bJA)C6u&v65Dw=ah^?`_d99RTOMWU*a@*@39ZgSYIp=ST^Nuh29TJD zyduH1cHuII16oWE!;BUx>o3q`Q8X90j`de_dF)5>dy@%zMrUmdugq1K^xKg+*wv@0~RHUe|*j6IWqKdjl zx2ZDOQ6#~nCYRJ$=t?A6)TOAih$xX_(U79SBC3Q>SQ1esUCulFu{;9zvRPnnz@KsFtqp5s2IxlQxDn&0)Cd6wn{HNWd) zD0*m251~XUU>8m5Wl8~iXi;}51ynFjy-F#dh6(C*N&yW_Qg2cUXkm(an^HgrZR%Z0 z0Xic-&(BS~HbQn_#IK&)vK`Gz}^VAW?V}Ji#LM zV@d(evq1f+zZ1F&TA}sFkRDq9+Ma9v8@*;t!tdsbi>r(I+r=VCe_l+R_&@ZY!TOQ^ oKC;s6wG&-x{_ogl|AR5R?~=H5{XfQ3%loJ1(Entv{1fZ+KeF=slmGw# diff --git a/client_s2/compLib/__pycache__/Movement.cpython-39.pyc b/client_s2/compLib/__pycache__/Movement.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1f185a1487878f5eb3cdbb663d26e4f9672c3c26 GIT binary patch literal 2466 zcmchZOK;mo5P(VQMahz#*m2VIw6{WNZM8WRg-{f2?7RTiHEj373$fykrM3A`c2^c; zsHfy-+zutO`dJ=>;xCv>8USFx=ZmZ*s8TT4`-`ljO5gneXnYro?PYrku! zot-2a@5#YVC?Pfhsh3CTVh^=odA6`TN7!Bks!)TPaNarD>#J{UuU_bCsH=;{ThcWO zor}7rXk|KAtY8T3)crm=hX}o1eECuiM=XSM2w4zvt(i$!ki`^x&snLDS>X$57~ug$Hw(VyP7LrinV!V;(>+pgr~KX)nk&m9L-V8ae1Kkl!B9(p5p=_=_*Y z9d7A13ajbbZM$yQ9oN2iRId2DHL~9jFjN3Ki}(Y9|H(4=$5?wpHjpP0Q7OV%fS+%N zJF(AWXLnK`B$F8F+zPPPXH$us|0Dkcz|K)vmLx9V&wU~Lh0_6;Dd1k+`r@n4vk36_ z=AxUsKye{?tpC$CP<5=Pw@!H~To>>w7bg@|Tv3ZxD`eN<@i0Ul@*us{5_5FzYei_N z4haHUt&&(HaSsK;`;hJlUUi?uItd~+7Z30*ZPRXDIQSSf@AI}<^0sV?wADiy16OGS zqnw0XeHh>_ek<#OJ!We_C8owVBXOxJvb`mDss7(sz5eXu53Ed20;#XBr5aF8OckOD$vDfeAS2owJdmCBr um4of211R5yn>S|ie?0l=P)`P@5SZ*Gw|P$8uk&yE2K%3->6~cW?aE(fRGf(b literal 0 HcmV?d00001 diff --git a/client_s2/main.py b/client_s2/main.py index dd3e732..8082ab6 100644 --- a/client_s2/main.py +++ b/client_s2/main.py @@ -33,15 +33,57 @@ def main(): # from compLib.IRSensor import IRSensor # IRSensor.read_all() # - from compLib.Encoder import Encoder - Encoder.read_all_positions() - Encoder.read_all_velocities() + # from compLib.Encoder import Encoder + # print(Encoder.read_all_positions()) + # print(Encoder.read_all_velocities()) # from compLib.Motor import Motor # Motor.speed(0, 50) # Motor.speed(3, -50) + # + # import time + # time.sleep(2) + # + # Motor.speed(0, 0) + # Motor.speed(3, -0) - # time.sleep(10) + # Motor.power(0, 0) + # Motor.power(3, 0) + + # import math + # from compLib.Movement import Movement + # Movement.turn_degrees(90, math.pi * 2) + # Movement.turn_degrees(-90, math.pi * 2) + # + # Movement.turn_degrees(90, math.pi * 2) + # Movement.turn_degrees(90, -math.pi * 2) + # + # Movement.turn_degrees(90, math.pi * 2) + # Movement.turn_degrees(-90, -math.pi * 2) + + # from compLib.Movement import Movement + # Movement.drive_distance(0.1, 0.5) + # Movement.drive_distance(-0.1, 0.5) + # + # Movement.drive_distance(0.1, 0.5) + # Movement.drive_distance(0.1, -0.5) + # + # Movement.drive_distance(0.1, 0.5) + # Movement.drive_distance(-0.1, -0.5) + + from compLib.Movement import Movement + import math + Movement.drive_distance(0.5, 0.5) + Movement.turn_degrees(90, math.pi * 2) + + Movement.drive_distance(0.5, 0.5) + Movement.turn_degrees(90, math.pi * 2) + + Movement.drive_distance(0.5, 0.5) + Movement.turn_degrees(90, math.pi * 2) + + Movement.drive_distance(0.5, 0.5) + Movement.turn_degrees(90, math.pi * 2) if __name__ == '__main__': diff --git a/server_v2/include/ClosedLoopMotorController.hpp b/server_v2/include/ClosedLoopMotorController.hpp index 08ee94d..eb589e7 100644 --- a/server_v2/include/ClosedLoopMotorController.hpp +++ b/server_v2/include/ClosedLoopMotorController.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "include/PID.hpp" #include "include/Robot.hpp" @@ -46,6 +47,7 @@ private: std::array speed_targets{0}; std::thread speed_control_thread; + std::recursive_mutex speed_control_mutex; }; diff --git a/server_v2/include/Encoders.hpp b/server_v2/include/Encoders.hpp index bbde700..5c8eb58 100644 --- a/server_v2/include/Encoders.hpp +++ b/server_v2/include/Encoders.hpp @@ -21,14 +21,18 @@ public: std::array get_velocities_rad_s(); + std::array get_velocities_ticks_s(); + private: Encoders() = default; Cache position_cache{ROBOT_ENCODER_RATE_HZ}; Cache velocity_cache{ROBOT_ENCODER_RATE_HZ}; + Cache ticks_cache{ROBOT_ENCODER_RATE_HZ}; std::array cached_positions = {0}; std::array cached_velocities_rad_s = {0}; + std::array cached_velocities_ticks_s = {0}; }; diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp index 499cd98..a9593f9 100644 --- a/server_v2/include/Robot.hpp +++ b/server_v2/include/Robot.hpp @@ -7,9 +7,9 @@ #define ROBOT_IR_RATE_HZ 250 #define ROBOT_MOTOR_COUNT 4 -#define ROBOT_MOTOR_SPEED_CONTROL_KP 8.5 -#define ROBOT_MOTOR_SPEED_CONTROL_KI 31.0 -#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.085 +#define ROBOT_MOTOR_SPEED_CONTROL_KP 15.0 +#define ROBOT_MOTOR_SPEED_CONTROL_KI 5.0 +#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.0 #define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI #define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250 @@ -20,6 +20,7 @@ #define ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT -1.0 #define ROBOT_ENCODER_RATE_HZ 500 +#define ROBOT_ENCODER_MAX_CHANGE_RAD_S M_PI_4 #define ROBOT_WHEEL_RADIUS_MM 35.5 #define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0) @@ -34,13 +35,13 @@ #define ROBOT_GO_TO_DISTANCE_RATE_HZ 200 #define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0 -#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 10.0 +#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0 #define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target #define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.075 #define ROBOT_GO_TO_ANGLE_RATE_HZ 200 #define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target -#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.5 -#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 2.0) +#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.9 +#define ROBOT_GO_TO_ANGLE_MIN_VEL M_PI_2 #endif //COMPLIB_SERVER_ROBOT_HPP diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp index 2f34fcc..48d670c 100644 --- a/server_v2/src/ClosedLoopMotorController.cpp +++ b/server_v2/src/ClosedLoopMotorController.cpp @@ -11,11 +11,13 @@ #define US_IN_S (1000 * MS_IN_S) void ClosedLoopMotorController::set_power(uint8_t port, double power) { + std::lock_guard lock(speed_control_mutex); control_modes.at(port) = ControlMode::POWER; Motors::set_power(port, power); } void ClosedLoopMotorController::set_speed(uint8_t port, double speed_ms) { + std::lock_guard lock(speed_control_mutex); speed_targets.at(port) = speed_ms; if (control_modes.at(port) != ControlMode::SPEED) { pids.at(port) = PID( @@ -35,15 +37,17 @@ ClosedLoopMotorController::ClosedLoopMotorController() { while (true) { std::this_thread::sleep_for(sleep_duration); + std::lock_guard lock(speed_control_mutex); bool should_control_speed = std::find(control_modes.begin(), control_modes.end(), ControlMode::SPEED) != control_modes.end(); if (!should_control_speed) { continue; } - auto speeds_ms = Encoders::getInstance().get_velocities_rad_s(); + auto speeds_rad_s = Encoders::getInstance().get_velocities_rad_s(); for (int i = 0; i < ROBOT_MOTOR_COUNT; i++) { if (control_modes.at(i) == ControlMode::SPEED) { - double power = pids.at(i)(speed_targets.at(i), speeds_ms.at(i)); + spdlog::debug("CLMC: {}", i); + double power = pids.at(i)(speed_targets.at(i), speeds_rad_s.at(i)); Motors::set_power(i, power); } } @@ -54,7 +58,7 @@ void ClosedLoopMotorController::generate_step_response_data() { auto console = spdlog::stdout_color_mt("generate_step_response_data"); console->set_pattern("%v"); - auto input = 25.0; + auto input = 30.0; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; @@ -70,21 +74,21 @@ void ClosedLoopMotorController::generate_step_response_data() { std::this_thread::sleep_for(std::chrono::seconds(1)); auto start_time = std::chrono::steady_clock::now(); - for (int i = 0; i < 150; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + for (int i = 0; i < 1000; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); + console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } - input = 35.0; + input = 40.0; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - for (int i = 0; i < 150; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + for (int i = 0; i < 1000; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); + console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0; @@ -102,10 +106,10 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() { speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID( - 8.5, 31.0, 0.085, + 5.5, 50.0, 0.0, 4 * M_PI, 100.0); pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID( - 8.5, 31.0, 0.085, + 5.5, 50.0, 0.0, 4 * M_PI, 100.0); control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED; control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED; @@ -113,31 +117,31 @@ void ClosedLoopMotorController::generate_tuned_step_response_data() { std::this_thread::sleep_for(std::chrono::seconds(2)); auto start_time = std::chrono::steady_clock::now(); - for (int i = 0; i < 500; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + for (int i = 0; i < 1500; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); + console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } input = 4 * M_PI; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - for (int i = 0; i < 500; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + for (int i = 0; i < 1500; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); + console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } input = M_PI; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - for (int i = 0; i < 500; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + for (int i = 0; i < 1500; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); - console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); + console->info("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); input += 0.01; speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; @@ -169,8 +173,8 @@ void ClosedLoopMotorController::calibrate_wheel_ticks(double turns, double ticks set_power(port, adjusted_power); - spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};", - ticks, target_ticks, adjusted_power); + spdlog::info("{:03.4f}; {:03.4f}; {:03.4f};", + ticks, target_ticks, adjusted_power); std::this_thread::sleep_for(std::chrono::milliseconds(2)); ticks = Encoders::getInstance().get_positions().at(port); diff --git a/server_v2/src/Encoders.cpp b/server_v2/src/Encoders.cpp index 921d552..110d303 100644 --- a/server_v2/src/Encoders.cpp +++ b/server_v2/src/Encoders.cpp @@ -23,9 +23,25 @@ std::array Encoders::get_velocities_rad_s() { auto velocity_ticks_s = mathUtils::from_bytes(data + i * 2, 2); auto velocity_rot_s = velocity_ticks_s / ROBOT_TICKS_PER_TURN; auto velocity_rad_s = velocity_rot_s * 2 * M_PI; - cached_velocities_rad_s.at(i) = velocity_rad_s; + + auto change = velocity_rad_s - cached_velocities_rad_s.at(i); + change = mathUtils::limit_max(change, -ROBOT_ENCODER_MAX_CHANGE_RAD_S, ROBOT_ENCODER_MAX_CHANGE_RAD_S); + cached_velocities_rad_s.at(i) = cached_velocities_rad_s.at(i) + change; } velocity_cache.update(); } return cached_velocities_rad_s; } + +std::array Encoders::get_velocities_ticks_s() { + if (ticks_cache.is_expired()) { + uint8_t data[ROBOT_MOTOR_COUNT * 2] = {0}; + Spi::getInstance().read_array(Spi::MOTOR_1_VEL_H, 2 * ROBOT_MOTOR_COUNT, data); + for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) { + auto velocity_ticks_s = mathUtils::from_bytes(data + i * 2, 2); + cached_velocities_ticks_s.at(i) = velocity_ticks_s; + } + ticks_cache.update(); + } + return cached_velocities_ticks_s; +} diff --git a/server_v2/src/GoToController.cpp b/server_v2/src/GoToController.cpp index b8390d1..e189d16 100644 --- a/server_v2/src/GoToController.cpp +++ b/server_v2/src/GoToController.cpp @@ -26,8 +26,9 @@ void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s) void GoToController::drive_distance(double distance_m, double v_m_s) { auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_DISTANCE_RATE_HZ); - if (distance_m < 0 && v_m_s > 0) { - v_m_s *= -1; + if (distance_m < 0 || v_m_s < 0) { + distance_m = -fabs(distance_m); + v_m_s = -fabs(v_m_s); } auto was_enabled = OdometryController::getInstance().is_enabled(); @@ -74,8 +75,9 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) { auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0)); auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_GO_TO_ANGLE_RATE_HZ); - if (angle_rad < 0 && v_rad_s > 0) { - v_rad_s *= -1; + if (angle_rad < 0 || v_rad_s < 0) { + angle_rad = -fabs(angle_rad); + v_rad_s = -fabs(v_rad_s); } auto was_enabled = OdometryController::getInstance().is_enabled(); @@ -91,12 +93,12 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) { if (fabs(angle_to_target) > M_PI_2) { diff_drive_inverse_kinematics(0.0, v_rad_s); - spdlog::debug("{:03.4f}; {:03.4f};", current_angle, angle_to_target); + spdlog::debug("TURN: {:03.4f}; {:03.4f};", current_angle, angle_to_target); } else { auto speed_mult = mathUtils::limit_max(angle_to_target * ROBOT_GO_TO_ANGLE_ALPHA_P, -1.0, 1.0); auto adjusted_speed = mathUtils::limit_min(fabs(v_rad_s) * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL); diff_drive_inverse_kinematics(0.0, adjusted_speed); - spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed); + spdlog::debug("TURN: {:03.4f}; {:03.4f}; {:03.4f};", current_angle, angle_to_target, adjusted_speed); } } diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 4dfb48b..9b9bfdb 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -80,10 +80,10 @@ OdometryController::OdometryController() { current_odometry = Odometry(new_x, new_y, new_theta); - spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", - current_position_left, current_position_right, - distance_left, distance_right, - v, w); + spdlog::debug("ODOM: {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", + current_position_left, current_position_right, + distance_left, distance_right, + v, w); } } } diff --git a/server_v2/src/PID.cpp b/server_v2/src/PID.cpp index 8ba5277..a27360c 100644 --- a/server_v2/src/PID.cpp +++ b/server_v2/src/PID.cpp @@ -1,6 +1,7 @@ #include "include/PID.hpp" #include #include +#include #define MS_IN_S 1000.0 #define US_IN_S (1000.0 * MS_IN_S) @@ -35,12 +36,15 @@ double PID::operator()(double setpoint, double process_variable) { // Tustin transform of the integral part double integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev); double derivative = D * ((error - error_prev) / delta_seconds); + integral = std::clamp(integral, -limit, limit); double output = proportional + integral + derivative; // anti-windup - limit the output variable - output = std::min(std::max(-limit, output), limit); - // spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds); + output = std::clamp(output, -limit, limit); + spdlog::debug("PID: E{:03.4f} P{:03.4f} I{:03.4f} D{:03.4f} O{:03.4f} EP{:03.4f} DS{:03.4f}, SP{:03.4f} PV{:03.4f}", + error, proportional, integral, derivative, output, error_prev, delta_seconds, setpoint, process_variable + ); integral_prev = integral; setpoint_prev = setpoint; diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 91c38f4..5c83c54 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -14,7 +14,7 @@ using namespace std; int main(int argc, char *argv[]) { Reset::reset_robot(); spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v"); - spdlog::set_level(spdlog::level::debug); + spdlog::set_level(spdlog::level::info); UnixSocketServer unixSocketServer; TCPSocketServer tcpSocketServer; @@ -39,7 +39,11 @@ int main(int argc, char *argv[]) { // GoToGoalController::go_to_point(2, 0, 0.2); // GoToController::drive_distance(-4, 0.2); -// GoToController::turn_degrees(180, M_PI); +// GoToController::turn_degrees(90, M_PI); +// this_thread::sleep_for(std::chrono::milliseconds(100)); +// GoToController::turn_degrees(-90, M_PI); + + // while (true) { // this_thread::sleep_for(std::chrono::milliseconds(10)); // ClosedLoopMotorController::getInstance().set_power(0, 100); @@ -69,6 +73,7 @@ int main(int argc, char *argv[]) { // ClosedLoopMotorController::getInstance().generate_step_response_data(); // ClosedLoopMotorController::getInstance().generate_tuned_step_response_data(); +// this_thread::sleep_for(std::chrono::milliseconds(1000)); std::this_thread::sleep_for(std::chrono::hours(12)); return 0; }