-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrectangle.py
125 lines (94 loc) · 4.89 KB
/
rectangle.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
from utils import ARUCO_DICT, print_coordinates , plot, plot_velo
from arucoTracking import PositionTracker
from Communication import Drone
from pidaxischanged import PIDController
import time as tm
import numpy as np
from math import fabs
#IMP
#checkpoints format: [ [ [x,y,z] , time_alloted ] , ... ]
def visit_checkpoints( checkpoints, x_permissible_error = 0.07 , y_permissible_error = 0.07, z_permissible_error = 0.05 , permissible_rms_velocity=0.05, id=0 ):
# setup values..........................................
commands_data=[]
coords_data=[]
velocity_arr = []
z_rot = []
aruco_dict_type = ARUCO_DICT["DICT_4X4_250"]
calibration_matrix_path = "calibration_data/calibration_matrix.npy"
distortion_coefficients_path = "calibration_data/distortion_coefficients.npy"
k_values = [[250 , 160, 160],
[ 0.05, 0.1, 0.1],
[ 8500, 8500 , 8500]] # PID, TPRs
range = [[1300, 1900], [1300, 1700], [1300, 1700]] # TPR #maximum values for state
k = np.load(calibration_matrix_path)
d = np.load(distortion_coefficients_path)
#........................................................
#initialize tracking and Drone communication.............
pos_tracker = PositionTracker(aruco_dict_type, k, d, camera_src=2, wait_time=1 , smoothing=[3, 3, 10])
pos_tracker.start()
drone = Drone("192.168.4.1", 23, 5, debug=True) #creates drone object, set debug to true to get console output on every action.
drone.connect() #starts looping in a separate thread
init_pos = np.array(pos_tracker.read_smooth_position(id))
while( len(init_pos)==0 ):
print("Detecting Drone")
init_pos = np.array(pos_tracker.read_smooth_position(id))
print("Initial Position",init_pos)
tm.sleep(1)
print("Origin Set")
pos_tracker.set_scaling_params([2.36, 2.4375, 2.56])
pos_tracker.set_origin(np.array(pos_tracker.read_smooth_position(id))) #sets the initial position as origin
tm.sleep(2)
pid = PIDController([0, 0, -0.4], k_values, range)
pid.calculate_state(np.array(pos_tracker.read_smooth_position(id)))
init_time = tm.time()
drone.disarm()
drone.takeoff()
tm.sleep(0.2)
for i in checkpoints:
pid.set_target(i[0])
curr_err = pid.get_error()
start = tm.time()
# while( curr_err[0]>x_permissible_error or curr_err[1]>y_permissible_error or curr_err[2]>z_permissible_error or pos_tracker.get_velocity(id)>permissible_rms_velocity):
while(tm.time()-start<i[1]):
new_pos = np.array(pos_tracker.read_smooth_position(id)) #getting latest position
new_z_rot = pos_tracker.read_z_rotation(id) #getting current yaw state
print(new_pos)
if(len(new_pos) == 0): #terminates the current loop if tracking fails for more than the wait_time(defined earlier)
break
# print(new_pos)
calculated_state = pid.calculate_state(new_pos) #calculates state values using PID
#storing data for plotting
commands_data.append(calculated_state)
coords_data.append( new_pos )
velocity_arr.append(pos_tracker.get_velocity(id))
z_rot.append(fabs(new_z_rot))
#...................................................
drone.set_state(calculated_state[0], calculated_state[1], calculated_state[2]) #setting the new state values
tm.sleep(0.03)
curr_err = pid.get_error()
print("Final Error",curr_err)
print("Checkpoint_reached")
pos_tracker.stop()
drone.land()
drone.disconnect()
total_flight_duration = tm.time()-init_time
plot(commands_data,coords_data,z_rot,total_flight_duration,all=True) #plots the recorded data
# plot_velo(velocity_arr,total_flight_duration)
return
if __name__ == "__main__":
testing = [[[0,0,-0.5],10]]
hover = [[[0,0,-0.4],12]] #checkpoint for hover test
x = -2
y = 1
z = -0.4
cp_time = 6 #checkpoint time
hover_time = 8
rectangle = [ [[0,0,z] , hover_time] , #checkpoints for rectangle test
[[x/4,0,z] , cp_time], [[x/2,0,z] , cp_time], [[x*3/4,0,z] , cp_time], [[x,0,z] , cp_time],
[[x,y/3,z] ,cp_time], [[x,y*2/3,z] , cp_time] , [[x,y,z] , cp_time] ,
[[x*3/4,y,z] , cp_time], [[x/2,y,z] , cp_time], [[x/4,y,z] , cp_time], [[0,y,z] , cp_time],
[ [0,y*2/3,z],cp_time] , [[0,y/3,z],cp_time], [[0,0,z],cp_time] ]
x_translate_checkpoints = [ [[0,0,z] , 13] , [[-0.5,0,z] , 10], [[-1,0,z] , 10], [[-1.5,0,z] , 10], [[-2,0,z] , 10]]
x_y_translate = [ [[0,0,z] , 13] , [[-0.5,0,z] , 10], [[-1,0,z] , 10], [[-1,0.4,z] , 10], [[-1,0.8,z] , 10] ]
visit_checkpoints(testing)
# visit_checkpoints(rectangle)