-
Notifications
You must be signed in to change notification settings - Fork 2
/
movement.py
129 lines (90 loc) · 4.06 KB
/
movement.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
'''
this class deals with drone movement
'''
class movement:
# gets the relevant objects needed upon initialization
def __init__(self, drone_obj, tof_obj, velocity_obj, threshold_distance=1500):
self.drone = drone_obj
self.tof = tof_obj
self.velocity = velocity_obj
self.velocities = self.velocity.calc_vel_FBLR()
# this sets the closest the drone can get to an obstacle before velocity commands are terminated
self.threshold = threshold_distance
'''the following functions command the drone to move in their respective
directions. times is just the number of times the send_global_velocity will be called
Check the drone class' send rate to determine the number of seconds (default 0.5)
The drone will always check if it is about to crash using the corresponding tof sensor
If the TOF detects the distance is less than 1500cm (Default), it will attempt to abort the command
By default, the TOF sensors get weird readings of less than 800mm once the distance exceeds 4m (the limit)
thus there is a hardcoded minimum below which the TOF will ignore readings. This is only possible because
our drone was never meant to operate closer than 1.5m any wall.
This is just a warning to those who may want to change the threshold to be lower than 800mm
'''
def move_forward(self, times=1):
for i in range(times):
if self.tof.dist_list[0] > threshold:
self.drone.send_global_velocity(*velocities[0],0,1)
else:
self.drone.send_global_velocity(0,0,0,1)
self.stop()
def move_backward(self, times=1)
for i in range(times):
if self.tof.dist_list[1] > threshold:
self.drone.send_global_velocity(*velocities[1],0,times)
else:
self.drone.send_global_velocity(0,0,0,1)
self.stop()
def move_left(self, times=1)
for i in range(times):
if self.tof.dist_list[2] > threshold:
self.drone.send_global_velocity(*velocities[2],0,times)
else:
self.drone.send_global_velocity(0,0,0,1)
self.stop()
def move_right(self, times=1)
for i in range(times):
if self.tof.dist_list[3] > threshold:
self.drone.send_global_velocity(*velocities[3],0,times)
else:
self.drone.send_global_velocity(0,0,0,1)
self.stop()
# stops all drone velocity for x seconds
# seconds assumes the update rate for send_global_velocity is 0.5s
# THIS DOES NOT STOP THE DRONE FROM DRIFTING DUE TO SENSORS
def stop(self, seconds=1):
self.drone.send_global_velocity(0,0,0,seconds*2)
# Yaw movement in degrees relative to the drone
def yaw(self, angle):
self.drone.condition_yaw(angle, True)
# give the drone appropriate time to execute the yaw
stop_time = int(angle/5) + 2
self.stop(stop_time)
# this is a subroutine to call the drone to move at an angle relative to itself (0 is forward)
# the default velocity is 0.1m/s
# down corresponds to the z axis, postive means the drone moves down
'''
do note that there is no way to check for obstacles when the drone isnt flying in
the four main directions, thus there is no TOF check for these movements
'''
def move_angle(self, angle, velocity=0.1, down=0, times=1):
velocity_tuple = self.velocity.calc_vel_from_angle(angle, velocity)
for i in times:
self.drone.send_global_velocity(*velocity_tuple, down, 1)
# this is the raw movement call, do not use it unless you are sure of what you are doing
# the drone movement is in accordance to the North East Down convention
# combine the vectors to obtain the correct moevemtn in correct direction
def move_vels(self, North, East, Down=0, times=1)
self.drone.send_global_velocity(North, East, Down, times)
'''the following are raw directional movement calls without TOF checks'''
def move_forward_raw(self, times=1):
for i in range(times):
self.drone.send_global_velocity(*velocities[0],0,1)
def move_backward(self, times=1)
for i in range(times):
self.drone.send_global_velocity(*velocities[1],0,times)
def move_left(self, times=1)
for i in range(times):
self.drone.send_global_velocity(*velocities[2],0,times)
def move_right(self, times=1)
for i in range(times):
self.drone.send_global_velocity(*velocities[3],0,times)