-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy patharucoTracking.py
118 lines (100 loc) · 4.31 KB
/
arucoTracking.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
from djitellopy import Tello
import cv2
import numpy as np
from time import sleep
def trackArucoX(myDrone, twist, pid, pError):
"""PID controller implemented for moving forward, yaw_axis, and left right velocity
To Do: need to implement up and down and I think we can use direction for that"""
"""trim speeds to 30 because indoors and don't want a crazy velocity into the wall,
also because frames don't update quick enough, scared it might not update and just keep flying into the wall"""
# Sends RC command based on distance of translation vector
if np.all(twist[1]) != None:
if np.all(twist[1][0][0]) != 0:
# PID for left_right_forwards backwards
error = twist[1][0][0][0]
speed = pid[0] * error + pid[1] * (error - pError)
speed = int(np.clip(speed, -30, 30))
# error2 = twist[1][0][0][1]
# speed2 = pid2[0] * error2 + pid2[1] * (error2 - pError2)
# speed2 = int(np.clip(speed2, -30, 30))
# # PID for forwards backwards
# error3 = twist[1][0][0][2]
# speed3 = pid3[0] * error3 + pid3[1] * (error3 - pError3)
# speed3 = int(np.clip(speed2, -30, 30))
# speed2 = 0
# speed3 = 0
# error2 = 0
# error3 = 0
# print('speed', speed,'\n')
# print('speed2', speed2)
# print('speed3', speed3, '\n')
myDrone.left_right_velocity = speed
myDrone.for_back_velocity = 0 #speed3
myDrone.up_down_velocity = 0 #speed2
myDrone.yaw_velocity = 0
else:
myDrone.for_back_velocity = 0
myDrone.left_right_velocity = 0
myDrone.up_down_velocity = 0
myDrone.yaw_velocity = 0
error = 0
error2 = 0
error3 = 0
speed = 0
speed2 = 0
speed3 = 0
# if myDrone.send_rc_control:
myDrone.send_rc_control(myDrone.left_right_velocity,
myDrone.for_back_velocity,
myDrone.up_down_velocity,
myDrone.yaw_velocity)
# return errors for previous errors and PID controller
return error, speed
def trackArucoY(myDrone, twist, pid, pError):
"""PID controller implemented for moving forward, yaw_axis, and left right velocity
To Do: need to implement up and down and I think we can use direction for that"""
"""trim speeds to 30 because indoors and don't want a crazy velocity into the wall,
also because frames don't update quick enough, scared it might not update and just keep flying into the wall"""
# Sends RC command based on distance of translation vector
if np.all(twist[1]) != None:
if np.all(twist[1][0][0]) != 0:
# PID for left_right_forwards backwards
# error = twist[1][0][0][0]
# speed = pid[0] * error + pid[1] * (error - pError)
# speed = int(np.clip(speed, -30, 30))
error = twist[1][0][0][1]
speed = pid[0] * error + pid[1] * (error - pError)
speed = int(np.clip(speed, -30, 30) * -1)
# # PID for forwards backwards
# error3 = twist[1][0][0][2]
# speed3 = pid3[0] * error3 + pid3[1] * (error3 - pError3)
# speed3 = int(np.clip(speed2, -30, 30))
# speed2 = 0
# speed3 = 0
# error2 = 0
# error3 = 0
# print('speed', speed,'\n')
# print('speed2', speed2)
# print('speed3', speed3, '\n')
myDrone.left_right_velocity = 0
myDrone.for_back_velocity = 0 #speed3
myDrone.up_down_velocity = speed #speed2
myDrone.yaw_velocity = 0
else:
myDrone.for_back_velocity = 0
myDrone.left_right_velocity = 0
myDrone.up_down_velocity = 0
myDrone.yaw_velocity = 0
error = 0
error2 = 0
error3 = 0
speed = 0
speed2 = 0
speed3 = 0
# if myDrone.send_rc_control:
myDrone.send_rc_control(myDrone.left_right_velocity,
myDrone.for_back_velocity,
myDrone.up_down_velocity,
myDrone.yaw_velocity)
# return errors for previous errors and PID controller
return error, speed