diff --git a/.gitignore b/.gitignore index 14f300a..6b75623 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,2 @@ -slprj .idea -__pycache__ +venv \ No newline at end of file diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..3659d2e --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "RadioControlProtocolPy"] + path = RadioControlProtocolPy + url = git@github.com:ToolboxPlane/RadioControlProtocolPy.git diff --git a/README.md b/README.md deleted file mode 100644 index 6f26d68..0000000 --- a/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# ControlSimulation -Simulation of the feedback-control using python. diff --git a/RadioControlProtocolPy b/RadioControlProtocolPy new file mode 160000 index 0000000..9981549 --- /dev/null +++ b/RadioControlProtocolPy @@ -0,0 +1 @@ +Subproject commit 99815493c51cdbbf11eba81887faaafe60c925b5 diff --git a/config.py b/config.py deleted file mode 100644 index 9e41b76..0000000 --- a/config.py +++ /dev/null @@ -1,31 +0,0 @@ -import numpy as np - -RHO_AIR = 1.2041 # Density of Air at 20°C -G = 9.81 - -M_PLANE = 1.117 + 0.506 + 2 * 0.192 # Mass of the plane -WIDTH = 2 # Maximal width of the plane -LENGTH = 0.9 # Maximal length of the plane -HEIGHT = 0.2 # Maximal height of the plane -CW = 0.2 # Drag coefficient (GUESSED) -A_FRONT = 157448 / 1000 ** 2 # Frontal area (pixels/(pixel/meter)^2) -A_SIDE = 63256 / 1000 ** 2 # Side area -A_TOP = 717182 / 1000 ** 2 # Top area - -# M = I * a'' <=> I = M / a'' = (F * r) / a'' = (m*g*r) / a'' -# Real M: M - M_mass = M - m * r^2 -MOMENT_INERTIA_ROLL = (0.197 * G * 0.5) / (185 / 180 * np.pi) - (0.197 * 0.5 ** 2) -MOMENT_INERTIA_PITCH = (0.197 * G * 0.42) / (275 / 180 * np.pi) - (0.197 * 0.42 ** 2) - -KV = 900 # Speed of the motor -V_BAT = 16.8 # Battery voltage -PROP_RADIUS = 10 / 2 * 0.0254 # Radius of the propeller in meter -PROP_PITCH = 4.5 * 0.0254 # Stroke of the propeller -PROP_EFF = 0.7 # Efficiency of the propeller (GUESSED) - -SERVO_MAX_ANGLE = 35 * np.pi / 180 # Max deflection angle of the elevon -ELEVON_AREA = 0.43 * 0.07 # Surface area of the elevon - -# Distance from elevon centre to plane centre -ELEVON_DIST_X = 0.3 -ELEVON_DIST_Y = 0.6 diff --git a/controller.py b/controller.py deleted file mode 100644 index 7bc7848..0000000 --- a/controller.py +++ /dev/null @@ -1,14 +0,0 @@ -import planestate -import input - - -class Controller: - def get(self, x: planestate.PlaneState, t: float) -> input.Input: - output = input.Input() - - if t < 10: - output.power = 1 - else: - output.power = 0 - - return output diff --git a/input.py b/input.py deleted file mode 100644 index 3c46604..0000000 --- a/input.py +++ /dev/null @@ -1,8 +0,0 @@ -class Input: - def __init__(self): - self.power = 0 - self.elevon_l = 0 - self.elevon_r = 0 - - def __str__(self): - return "Power=%.1f, L=%.1f, R=%.1f" % (self.power, self.elevon_l, self.elevon_r) \ No newline at end of file diff --git a/main.py b/main.py index 383e90c..8c72ead 100644 --- a/main.py +++ b/main.py @@ -1,44 +1,12 @@ -import numpy as np -import time +import socket +import RadioControlProtocolPy.rcLib as rcLib -from planestate import PlaneState -from input import Input -from plot import plot -import system -import controller +sock = socket.socket(socket.AF_INET, socket.SOCK_RAW, 253) +while True: + data = sock.recv(1024) + pkg = rcLib.Package(0, 0) -def main(): - dt = 0.01 - max_t = 100 - ts = np.arange(0, max_t, dt) - - control = controller.Controller() - - states = [PlaneState()] - states[0].x = 0 - states[0].y = 0 - states[0].speed = 0 - states[0].yaw = np.pi / 4 - states[0].roll = 0 - states[0].pitch = 0 - states[0].omega_roll = 0 - states[0].omega_pitch = 0 - - last_percent = 0 - for t in ts[1:]: - u = control.get(states[-1], t) - new_state = system.f(states[-1], u, dt) - states.append(new_state) - # time.sleep(dt) - - progress = t/max_t*100 - if int(progress) > last_percent: - print("Time: %.1f (%.d%%)\t%s" % (t, int(progress), str(new_state))) - last_percent = int(progress) - - plot(ts, states) - - -if __name__ == "__main__": - main() + for d in data: + if pkg.decode(d): + print(pkg.channel_data) diff --git a/planestate.py b/planestate.py deleted file mode 100644 index 4aa6707..0000000 --- a/planestate.py +++ /dev/null @@ -1,18 +0,0 @@ -class PlaneState: - def __init__(self): - self.speed = 0 - self.x = 0 - self.y = 0 - self.z = 0 - self.yaw = 0 - self.pitch = 0 - self.omega_pitch = 0 - self.roll = 0 - self.omega_roll = 0 - - def __str__(self): - return "Pos=(%.1f, %.1f, %.1f)\t Rot=(%.1f, %.1f, %.1f)\t V=%.1f\t d/dt Rot=(%.1f, %.1f, %.1f)" \ - % ( - self.x, self.y, self.z, self.roll, self.pitch, self.yaw, self.speed, self.omega_roll, - self.omega_pitch, - 0) diff --git a/plot.py b/plot.py deleted file mode 100644 index 51613a7..0000000 --- a/plot.py +++ /dev/null @@ -1,46 +0,0 @@ -import matplotlib.pyplot as plt -import matplotlib.lines -import numpy as np - - -def plot(ts: np.ndarray, states: list): - plt.figure() - - ax = plt.subplot(2, 2, 1) - xs = list(map(lambda x: x.x, states)) - ys = list(map(lambda x: x.y, states)) - traj = matplotlib.lines.Line2D(xs, ys) - ax.add_line(traj) - ax.set_xlim(min(xs), max(xs)) - ax.set_ylim(min(ys), max(ys)) - plt.title("Position") - plt.xlabel("x") - plt.ylabel("y") - - plt.subplot(2, 2, 2) - alts = list(map(lambda x: x.z, states)) - plt.plot(ts, alts) - plt.title("Altitude") - plt.xlabel("t") - plt.ylabel("z") - - plt.subplot(2, 2, 3) - speeds = list(map(lambda x: x.speed, states)) - plt.plot(ts, speeds) - plt.title("Speed") - plt.xlabel("t") - plt.ylabel("v") - - plt.subplot(2, 2, 4) - rolls = list(map(lambda x: x.roll/np.pi * 180, states)) - pitchs = list(map(lambda x: x.pitch/np.pi * 180, states)) - yaws = list(map(lambda x: x.yaw/np.pi * 180, states)) - plt.plot(ts, rolls) - plt.plot(ts, pitchs) - plt.plot(ts, yaws) - plt.title("Rotation") - plt.xlabel("t") - plt.ylabel("angle (deg)") - plt.legend(["Roll", "Pitch", "Yaw"]) - - plt.show() diff --git a/system.py b/system.py deleted file mode 100644 index a43dcd3..0000000 --- a/system.py +++ /dev/null @@ -1,70 +0,0 @@ -import math -import numpy as np - -import config -import planestate -import input - - -def fix_angle(alpha: float) -> float: - alpha = math.fmod(alpha, np.pi * 2) - if alpha > np.pi: - alpha -= np.pi * 2 - - if alpha < -np.pi: - alpha += np.pi * 2 - - return alpha - - -def f(x: planestate.PlaneState, u: input.Input, dt: float) -> planestate.PlaneState: - res = planestate.PlaneState() - # Motor - motor_rps = config.KV / 60 * config.V_BAT * u.power # Motor dynamic is not modelled - - # Propeller - motor_air_speed = motor_rps * config.PROP_PITCH - relative_motor_air_speed = motor_air_speed - x.speed - volume_flow = relative_motor_air_speed * np.pi * config.PROP_RADIUS**2 * config.PROP_EFF - mass_flow = volume_flow * config.RHO_AIR - thrust = mass_flow * motor_air_speed - - # Speed - air_resistance = config.CW * config.A_FRONT * 0.5 * config.RHO_AIR * x.speed ** 2 - gravitation = config.G * config.M_PLANE * np.sin(x.pitch) - acc_force = thrust - air_resistance - gravitation - acc = acc_force / config.M_PLANE - res.speed = x.speed + acc * dt - - # Position - res.x = x.x + np.cos(x.pitch) * np.cos(x.yaw) * (x.speed * dt + acc * 0.5 * dt ** 2) - res.y = x.y + np.cos(x.pitch) * np.sin(x.yaw) * (x.speed * dt + acc * 0.5 * dt ** 2) - res.z = x.z + np.sin(x.pitch) * (x.speed * dt + acc * 0.5 * dt ** 2) - - # Elevons - elevon_angle_l = u.elevon_l * config.SERVO_MAX_ANGLE - elevon_angle_r = u.elevon_r * config.SERVO_MAX_ANGLE - - elevon_in_thrust = 0.5 * x.speed * config.ELEVON_AREA * x.speed - - elevon_l_vert_force = elevon_in_thrust * np.sin(elevon_angle_l) - elevon_r_vert_force = elevon_in_thrust * np.sin(elevon_angle_r) - - pitch_torque = elevon_r_vert_force * config.ELEVON_DIST_X + elevon_l_vert_force * config.ELEVON_DIST_X - roll_torque = elevon_r_vert_force * config.ELEVON_DIST_Y - elevon_l_vert_force * config.ELEVON_DIST_Y - - acc_pitch = pitch_torque / config.MOMENT_INERTIA_PITCH - acc_roll = roll_torque / config.MOMENT_INERTIA_ROLL - res.omega_pitch = acc_pitch * dt - res.omega_roll = acc_roll * dt - - # Rotation - res.yaw = x.yaw - res.pitch = x.pitch + x.omega_pitch * dt + 0.5 * acc_pitch * dt ** 2 - res.roll = x.roll + x.omega_roll * dt + 0.5 * acc_roll * dt ** 2 - - res.yaw = fix_angle(res.yaw) - res.pitch = fix_angle(res.pitch) - res.roll = fix_angle(res.roll) - - return res