-
Notifications
You must be signed in to change notification settings - Fork 0
/
car.py
149 lines (123 loc) · 4.3 KB
/
car.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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#Car Model class
#global positions dictionary
# from ast import Pass
# from re import S
from traceback import print_tb
# from turtle import pos
# from torch import R
from car_commands import *
import numpy
from cell import Cell
from passenger import Passenger
class Car:
#Init methods
def __init__(self, id, socket,color):
#FOR TESTING ONLY, REMOVE LATER
self.goal_id = 8
self.sending = 0
self.color = color
self.id = id
self.path = None
self.isMoving = False
self.allowedToMove = True
self.socket = socket
#self.socket.send('g'.encode())
self.last_dir = 'g'
#self.socket.send('h'.encode())
self.dir = 0
self.local_goal = None
self.pos = None
self.passengers = []
self.currentPassenger: Passenger = None
self.cellsToVisit = [] #List of cells to visit
self.last_goal = self.pos
def set_path(self, path):
self.path = path
self.set_local_goal()
def set_local_goal(self):
if not (self.path is None) and len(self.path) > 0:
self.local_goal = (self.path[0].x, self.path[0].y)
self.last_goal = (self.path[len(self.path)-1].x,self.path[len(self.path)-1].y)
self.drive()
else:
self.local_goal = self.get_pos()
self.stopDrive()
def position(self, pos):
self.pos = pos
if self.path is None:
self.last_goal = self.get_pos()
def direction(self, dir):
self.dir = dir
def get_pos(self):
return (int(self.pos[0]), int(self.pos[1]))
#TODO: Finish dijkstra
#Drive methods
def stopDrive(self):
self.allowedToMove=False
if self.isMoving:
# print("stopping!")
self.socket.send('h'.encode())
self.isMoving = False
def continueDrive(self):
self.allowedToMove=True
def drive(self):
self.sending = (self.sending + 1) % 5
if (not self.local_goal is None) and ((self.pos[0] - self.local_goal[0]) ** 2 + (self.pos[1] - self.local_goal[1])**2)**.5 < 1:
if len(self.path) > 0:
self.path.pop(0)
self.set_local_goal()
if self.allowedToMove:
if not self.local_goal is None:
if not self.isMoving or self.sending % 5 == 0:
# print("driving", self.sending)
self.socket.send('d'.encode())
self.isMoving = True
goal_vector = sub(self.local_goal,self.pos)
car_angle = self.dir
goal_angle = 360-angle(goal_vector[0],goal_vector[1])
direction = dir(car_angle,goal_angle,0)
if self.last_dir != direction or self.sending % 5 == 0:
self.last_dir = direction
self.socket.send(direction.encode())
else:
if self.isMoving:
print("stop from that other place (line 96)")
self.socket.send('h'.encode())
self.isMoving = False
#Updates the car's driving angle and moves the car
#TODO Alexander
#def update():
# if self.isMoving:
# socket.send('d')
# else:
# socket.send('s')
# local_goal = cellsToVisit[0]
# if(near local_goal):
# cellsToVist.pop()
# else:
# adjust_angle()
# pass
def add_passenger_destination(self, newPassenger: Passenger, path):
"""
Adds the given passenger to the car's list of passengers.
Appends the shortest path from the last passenger to the newly added passenger to the car's cellsToVisit
"""
self.passengers.append(newPassenger)
self.cellsToVisit += path
def lastPassenger(self):
"""
Returns the last passenger the car will visit
"""
return self.passengers[-1]
def cells_to_visit_count(self):
"""
Returns the total cells the car has to visit
"""
return len(self.passengers)
#Car Attributes
#Alex writes the private methods
#Cars should have access to do the socket
#Socket is a socket object,
#Connection outputs:
#Arduino Speed = Integer (100 - 200)
#Turning Angle = Integer ()