Skip to content

Commit f200682

Browse files
HyperToken9HyperToken9
committed
Fixed Controlling... Again
1 parent abd2b34 commit f200682

File tree

1 file changed

+70
-46
lines changed

1 file changed

+70
-46
lines changed

‎main.py

+70-46
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,8 @@ def __init__(self, spawn_at=[750, 100], offset_angle=-2):
5858
# Images
5959
self.image_path = "Flight-Control-Bot/Plane1.png"
6060
self.image_aligned = pygame.image.load(self.image_path)
61+
self.true_rect = self.image_aligned.get_rect()
62+
6163
self.image = pygame.image.load(self.image_path)
6264
self.rect = self.image.get_rect()
6365
# self.img_pure = Image.open(self.Image_Path)
@@ -68,10 +70,10 @@ def __init__(self, spawn_at=[750, 100], offset_angle=-2):
6870
self.flying = True
6971

7072
# Position
71-
self.position = spawn_at # [random.randint(0, 1920/2), random.randint(0, 540)]
73+
self.position = [300, 300]#spawn_at # [random.randint(0, 1920/2), random.randint(0, 540)]
7274

7375
# Angle In Degrees
74-
self.angle = self.angle_to_center() # random.randint(0, 180)
76+
self.angle = self.angle_to_center() # random.randint(0, 180)
7577
print(f"{self.angle}")
7678

7779
self.OffsetAngle = offset_angle
@@ -95,17 +97,23 @@ def __init__(self, spawn_at=[750, 100], offset_angle=-2):
9597
self.red_dots.append(dot)
9698
self.turn(0)
9799

100+
98101
# # Turns the plane by a certain bank angle
99102
def turn(self, bank):
100103
self.angle += bank
101104
self.image = pygame.transform.rotate(self.image_aligned, self.OffsetAngle + self.angle)
102105
self.rect = self.image.get_rect()
106+
self.rect.center = self.position
107+
# print(dir(self.rect))
108+
# print(self.rect)
103109

104110
# Turns the plane to a Certain Angle
105111
def turn_to(self, angle):
106112
self.angle = angle
107113
self.image = pygame.transform.rotate(self.image_aligned, self.OffsetAngle + self.angle)
108114
self.rect = self.image.get_rect()
115+
self.rect.center = self.position
116+
109117

110118
# Starts to fade the sprite on approaching landing
111119

@@ -114,6 +122,7 @@ def update(self, click_pos, click_status):
114122
self.draw() # DRAWs the Game Sprite to NEW Location
115123
self.Towing(click_pos, click_status)
116124

125+
117126
def fade(self):
118127
self.img_og.fill((255, 255, 255, int(self.alpha)), special_flags=pygame.BLEND_RGBA_MULT)
119128
self.alpha /= 1.2
@@ -124,31 +133,30 @@ def fade(self):
124133
# self.Pos[1] + 5]
125134
# self.img_og = pygame.transform.scale(self.img_og, (size, size))
126135
# self.size = size
127-
128-
def Stanley_Control(self):
129-
k = 100
130-
131-
distances_to_waypoints = list(map(math.dist, [self.position] * len(self.way_points), self.way_points))
132-
# print(distances_to_waypoints)
133-
smallest_distance = min(distances_to_waypoints)
134-
closest_point_index = distances_to_waypoints.index(smallest_distance)
135-
136-
prev_wp = self.way_points[closest_point_index - 1]
137-
next_wp = self.way_points[closest_point_index]
138-
139-
# cross track error
140-
cte_num = (next_wp[0] - prev_wp[0]) * (prev_wp[1] - self.position[1]) - (prev_wp[0] - self.position[0]) * (next_wp[1] - prev_wp[1])
141-
142-
cte_den = math.dist(prev_wp, next_wp)
143-
cross_track_error = cte_num / (cte_den + 1)
144-
145-
theta_track = math.atan2(next_wp[1] - prev_wp[1], next_wp[0] - prev_wp[0])
146-
147-
heading_error = theta_track - self.angle
148-
149-
delta = heading_error + math.atan2(self.speed, k * cross_track_error)
150-
151-
return delta
136+
# def Stanley_Control(self):
137+
# k = 100
138+
#
139+
# distances_to_waypoints = list(map(math.dist, [self.position] * len(self.way_points), self.way_points))
140+
# # print(distances_to_waypoints)
141+
# smallest_distance = min(distances_to_waypoints)
142+
# closest_point_index = distances_to_waypoints.index(smallest_distance)
143+
#
144+
# prev_wp = self.way_points[closest_point_index - 1]
145+
# next_wp = self.way_points[closest_point_index]
146+
#
147+
# # cross track error
148+
# cte_num = (next_wp[0] - prev_wp[0]) * (prev_wp[1] - self.position[1]) - (prev_wp[0] - self.position[0]) * (next_wp[1] - prev_wp[1])
149+
#
150+
# cte_den = math.dist(prev_wp, next_wp)
151+
# cross_track_error = cte_num / (cte_den + 1)
152+
#
153+
# theta_track = math.atan2(next_wp[1] - prev_wp[1], next_wp[0] - prev_wp[0])
154+
#
155+
# heading_error = theta_track - self.angle
156+
#
157+
# delta = heading_error + math.atan2(self.speed, k * cross_track_error)
158+
#
159+
# return delta
152160

153161
def fly(self):
154162
self.tick += 1
@@ -174,13 +182,11 @@ def fly(self):
174182
turn_direction = orientation(head, tail, goal) * fine_bank
175183
# print("Ore", orientation(head, tail, dest))
176184
# print("turnDir", turnDir)
177-
self.turn(self.Stanley_Control())
185+
self.turn(turn_direction)
178186

179-
if math.dist(mid, self.way_points[0]) < 2:
187+
if math.dist(head, self.way_points[0]) < 2 or math.dist(mid,self.way_points[0]) < 8:
180188
reached = self.way_points.pop(0)
181189
self.red_dots.pop(0)
182-
# green_dots.append(reached)
183-
# print("Reached", reached)
184190

185191
self.restrict_fly_zone()
186192

@@ -194,11 +200,16 @@ def restrict_fly_zone(self):
194200
if self.tick < 500:
195201
return
196202

203+
if self.is_out_of_bounds():
204+
self.turn_to(self.angle_to_center())
205+
print("Out of Bounds")
206+
207+
# Checks if plane has left screen
208+
def is_out_of_bounds(self):
197209
if -25 < self.position[0] < screen_width - 10 and -25 < self.position[1] < screen_height - 10:
198-
return
210+
return False
211+
return True
199212

200-
self.turn_to(self.angle_to_center())
201-
print("Out of Bounds")
202213

203214
def land(self):
204215
if len(self.LandLoc) == 1:
@@ -237,10 +248,11 @@ def Towing(self, mouse_position, click_status):
237248
# Give angle to center from current position
238249
def angle_to_center(self):
239250
screen_center = [screen_width / 2, screen_height / 2]
240-
return self.angle(self.position, screen_center)
251+
# print(type(self.position), type(screen_center))
252+
return self.angle_of_points(self.position, screen_center)
241253

242254
@staticmethod
243-
def angle(coord1, coord2):
255+
def angle_of_points(coord1, coord2):
244256

245257
numerator = coord1[1] - coord2[1]
246258
denominator = coord2[0] - coord1[0]
@@ -250,20 +262,24 @@ def angle(coord1, coord2):
250262

251263
def head_pose(self):
252264

253-
head_loc = (self.position[0] + 30 + (27 * math.cos(math.radians(self.angle))
254-
- 2 * math.sin(math.radians(self.angle))),
255-
self.position[1] + 30 + (-27 * math.sin(math.radians(self.angle))
256-
- 2 * math.cos(math.radians(self.angle))))
265+
w, _ = self.true_rect.size
266+
267+
delta = self.rotate(np.array([w/2, 0]), degrees= -1 * self.angle)
268+
269+
head_loc = self.position + delta
257270

258271
# red_dots.append(head_loc)
259-
return np.array(head_loc)
272+
return head_loc
260273

261274
def tail_pose(self):
262-
tail_loc = (self.position[0] + 30 + (-29 * math.cos(math.radians(self.angle))),
263-
self.position[1] + 30 + (29 * math.sin(math.radians(self.angle))))
275+
w, _ = self.true_rect.size
276+
277+
delta = self.rotate(np.array([-w / 2, 0]), degrees=-1 * self.angle)
264278

265-
# red_dots.append(tail_loc)
266-
return np.array(tail_loc)
279+
tail_loc = self.position + delta
280+
281+
# red_dots.append(head_loc)
282+
return tail_loc
267283

268284
# def Spin(self):
269285
# self.Angle += 1
@@ -273,6 +289,14 @@ def path_vis(self):
273289
for dot in self.red_dots:
274290
screen.blit(red_dot, (dot[0] - 4, dot[1] - 4))
275291

292+
@staticmethod
293+
def rotate(p, origin=(0, 0), degrees=0):
294+
angle = np.deg2rad(degrees)
295+
R = np.array([[np.cos(angle), -np.sin(angle)],
296+
[np.sin(angle), np.cos(angle)]])
297+
o = np.atleast_2d(origin)
298+
p = np.atleast_2d(p)
299+
return np.squeeze((R @ (p.T - o.T) + o.T).T)
276300

277301
class Fleet:
278302

@@ -337,7 +361,7 @@ def spawn(self):
337361
if event.type == pygame.QUIT:
338362
running = False
339363

340-
test_fleet.Spawner()
364+
# test_fleet.Spawner()
341365
test_fleet.Manage()
342366

343367
# DEBUGGING TOOLS

0 commit comments

Comments
 (0)