@@ -49,6 +49,9 @@ def __init__(self):
49
49
self .right_tof = None
50
50
self .top_tof = None
51
51
self .bottom_tof = None
52
+ self .linear_velocity = None
53
+ self .angular_velocity = None
54
+ self .last_ack = ''
52
55
self .version = [None , None , None ]
53
56
54
57
def begin (self ) -> int :
@@ -84,6 +87,24 @@ def _stop_update_thread(cls):
84
87
"""
85
88
cls ._update_thread_running = False
86
89
90
+ def rotate (self , angle : float ):
91
+ """
92
+ Rotates the robot by given angle
93
+ :param angle:
94
+ :return:
95
+ """
96
+ self .packeter .packetC1F (ord ('R' ), angle )
97
+ uart .write (self .packeter .msg [0 :self .packeter .msg_size ])
98
+
99
+ def move (self , distance : float ):
100
+ """
101
+ Moves the robot by given distance
102
+ :param distance:
103
+ :return:
104
+ """
105
+ self .packeter .packetC1F (ord ('G' ), distance )
106
+ uart .write (self .packeter .msg [0 :self .packeter .msg_size ])
107
+
87
108
def stop (self ):
88
109
"""
89
110
Stops all Alvik operations
@@ -174,6 +195,13 @@ def drive(self, linear_velocity: float, angular_velocity: float):
174
195
self .packeter .packetC2F (ord ('V' ), linear_velocity , angular_velocity )
175
196
uart .write (self .packeter .msg [0 :self .packeter .msg_size ])
176
197
198
+ def get_drive_speed (self ) -> (float , float ):
199
+ """
200
+ Returns linear and angular velocity of the robot
201
+ :return: linear_velocity, angular_velocity
202
+ """
203
+ return self .linear_velocity , self .angular_velocity
204
+
177
205
def set_servo_positions (self , a_position : int , b_position : int ):
178
206
"""
179
207
Sets A/B servomotor angle
@@ -184,6 +212,13 @@ def set_servo_positions(self, a_position: int, b_position: int):
184
212
self .packeter .packetC2B (ord ('S' ), a_position & 0xFF , b_position & 0xFF )
185
213
uart .write (self .packeter .msg [0 :self .packeter .msg_size ])
186
214
215
+ def get_ack (self ):
216
+ """
217
+ Resets and returns last acknowledgement
218
+ :return:
219
+ """
220
+ return self .last_ack
221
+
187
222
# def send_ack(self):
188
223
# self.packeter.packetC1B(ord('X'), ACK_)
189
224
# uart.write(self.packeter.msg[0:self.packeter.msg_size])
@@ -286,9 +321,15 @@ def _parse_message(self) -> int:
286
321
elif code == ord ('q' ):
287
322
# imu position
288
323
_ , self .roll , self .pitch , self .yaw = self .packeter .unpacketC3F ()
289
- if code == ord ('w' ):
324
+ elif code == ord ('w' ):
290
325
# wheels position
291
326
_ , self .left_wheel ._position , self .right_wheel ._position = self .packeter .unpacketC2F ()
327
+ elif code == ord ('v' ):
328
+ # robot velocity
329
+ _ , self .linear_velocity , self .angular_velocity = self .packeter .unpacketC2F ()
330
+ elif code == ord ('x' ):
331
+ # robot ack
332
+ _ , self .last_ack = self .packeter .unpacketC1B ()
292
333
elif code == 0x7E :
293
334
# firmware version
294
335
_ , * self .version = self .packeter .unpacketC3B ()
0 commit comments