generated from shayanshafquat/DroneControllerMLiS
-
Notifications
You must be signed in to change notification settings - Fork 0
/
drone.py
103 lines (74 loc) · 3.69 KB
/
drone.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
import numpy as np
from typing import Tuple
class Drone():
def __init__(self):
self.x = 0
self.y = 0
self.t = 0
self.thrust_left = 0.5
self.thrust_right = 0.5
self.velocity_y = 0
self.velocity_x = 0
self.velocity_drag = 1.0
self.pitch = 0
self.pitch_velocity = 0
self.pitch_drag_constant = 0.3
self.target_coordinates = []
self.max_thrust = 1.0
self.turning_constant = 1.0
self.mass = 1.0
self.g = -1.0/self.mass
self.t = 0
self.game_target_size = 0.1
self.has_reached_target_last_update = False
def add_target_coordinate(self, point: Tuple[float, float]):
self.target_coordinates.append(point)
def get_pitch(self):
return self.pitch
def set_thrust(self, thrust_percentage: Tuple[float, float]):
assert(len(thrust_percentage) == 2)
assert(0<=thrust_percentage[0]<=1)
assert(0<=thrust_percentage[1]<=1)
self.thrust_left = thrust_percentage[0] * self.max_thrust
self.thrust_right = thrust_percentage[1] * self.max_thrust
def get_next_target(self) -> Tuple[float, float]:
return (0,0) if len(self.target_coordinates)==0 else self.target_coordinates[0]
def step_simulation(self, delta_time: float):
# Set the target reached flag to false
self.has_reached_target_last_update = False
self.t += delta_time
thrust_vec_x = np.sin(self.pitch)
thrust_vec_y = np.cos(self.pitch)
velocity_size = np.sqrt(self.velocity_x*self.velocity_x+self.velocity_y*self.velocity_y)
total_thrust = self.thrust_left+self.thrust_right
total_torque = (self.thrust_left - self.thrust_right)*self.turning_constant
acc_y_h = (total_thrust * thrust_vec_y) / self.mass + self.g - self.velocity_drag * self.velocity_y
acc_x_h = (total_thrust * thrust_vec_x) / self.mass - self.velocity_drag * self.velocity_x
theta_acc_h = (total_torque) / self.mass - self.pitch_drag_constant * np.abs(self.pitch_velocity)
vel_x_h = self.velocity_x + acc_x_h * delta_time/2
vel_y_h = self.velocity_y + acc_y_h * delta_time/2
theta_vel_h = self.pitch_velocity + theta_acc_h * delta_time / 2
x_h = self.x + vel_x_h * delta_time / 2
y_h = self.y + vel_y_h * delta_time / 2
theta_h = self.pitch + theta_vel_h * delta_time / 2
thrust_vec_x = np.sin(theta_h)
thrust_vec_y = np.cos(theta_h)
velocity_size = np.sqrt(vel_x_h*vel_x_h+vel_y_h*vel_y_h)
acc_y_f = (total_thrust * thrust_vec_y) / self.mass + self.g - self.velocity_drag * self.velocity_y
acc_x_f = (total_thrust * thrust_vec_x) / self.mass - self.velocity_drag * self.velocity_x
theta_acc_f = (total_torque) / self.mass - self.pitch_drag_constant * np.abs(self.pitch_velocity)
self.velocity_x = vel_x_h + acc_x_f * delta_time/2
self.velocity_y = vel_y_h + acc_y_f * delta_time/2
self.pitch_velocity = theta_vel_h + theta_acc_f * delta_time / 2
self.x = x_h + self.velocity_x * delta_time / 2
self.y = y_h + self.velocity_y * delta_time / 2
self.pitch = theta_h + self.pitch_velocity * delta_time / 2
# Updates the target list
target_point = self.get_next_target()
distance_x = self.x - target_point[0]
distance_y = self.y - target_point[1]
distance_to_target = np.sqrt(distance_x*distance_x+distance_y*distance_y)
if distance_to_target < self.game_target_size:
if len(self.target_coordinates) > 0:
self.target_coordinates.pop(0)
self.has_reached_target_last_update = True