Commit 0e9bcbe5 authored by Ibrahim's avatar Ibrahim
Browse files

converted to relative imports; motor simulation;

parent 4cf5fd7e
......@@ -2,7 +2,7 @@ from dataclasses import dataclass
import numpy as np
from simulation import Multirotor
from .simulation import Multirotor
......
......@@ -3,7 +3,7 @@ from typing import Callable, Iterable, Tuple
import numpy as np
from scipy.optimize import fsolve
from vehicle import PropellerParams, VehicleParams
from .vehicle import PropellerParams, VehicleParams
......
import scipy.integrate
import numpy as np
from vehicle import MotorParams, SimulationParams
"""
This Script simulates a motor object class that can receive a voltage or current amount and
convert voltage into an angular speed based on the specifications of the
Tarot T18 Octocopter motor model.
"""
class Motor:
def __init__(self, params: MotorParams, simulation: SimulationParams):
self.params = params
self.simulation = simulation
self.reset()
def step(self, voltage: float) -> float:
# ----------------------------
# UPDATE
# Returns a motor's angular velocity moving one step in time
# with a given voltage. Takes in as parameters, voltage and sample rate
# ----------------------------
self.t += self.simulation.dt
self.v = voltage
self.ode.set_initial_value(self.omega, 0)
self.speed = self.ode.integrate(self.ode.t + self.simulation.dt)
return self.speed
def omega_dot_i(self, time, state):
# ----------------------------
# OMEGA_DOT_I
# Helper Method to calculate omega_dot for our ode integrator.
# Can be written as a lambda function inside update for other shorter motors
# ----------------------------
rpm = self.speed * 30 / np.pi
t1 = self.motor_constant / self.params.r * self.v
t2 = -((2.138e-08)*rpm**2 + (-1.279e-05)*rpm)
t3 = -(self.params.k_m * self.params.k_q / self.params.r * self.speed)
dspeed = (t1 + t2 + t3) / self.params.moment_of_inertia
return dspeed
def reset(self):
self.ode = scipy.integrate.ode(self.omega_dot_i).set_integrator('vode', method='bdf')
self.t = 0.
self.speed = 0.
return self.speed
......@@ -83,13 +83,13 @@ def apply_forces_torques(
zI = x[2]
ub = x[3] # linear velocity along body-frame-x-axis
vb = x[4] # linear velocity along body-frame-y-axis
wb = x[5] # linear velocity along body-frame-z-axis (down is positive)
wb = x[5] # linear velocity along body-frame-z-axis
phi = x[6] # Roll
theta = x[7] # Pitch
psi = x[8] # Yaw
p = x[9] # body-frame-x-axis rotation rate
q = x[10] # body-frame-y-axis rotation rate
r = x[11] # body-frame-z-axis rotation rate
q = x[10] # body-frame-y-axis rotation rate
r = x[11] # body-frame-z-axis rotation rate
# Pre-calculate trig values
cphi = np.cos(phi); sphi = np.sin(phi) # roll
......@@ -123,6 +123,6 @@ def apply_forces_torques(
# Angular rate
gyro = np.cross(x[9:12], I @ x[9:12])
xdot[9:12] = I_inv @ (torques - gyro) # TODO: verify whether it is + or - gyro
xdot[9:12] = I_inv @ (torques - gyro)
return xdot
......@@ -4,10 +4,10 @@ import numpy as np
from numpy import (cos, sin)
from scipy.integrate import odeint
from vehicle import MotorParams, PropellerParams, SimulationParams, VehicleParams
from coords import body_to_inertial, direction_cosine_matrix, rotating_frame_derivative, angular_to_euler_rate
from physics import thrust, torque, apply_forces_torques
from helpers import control_allocation_matrix
from .vehicle import MotorParams, PropellerParams, SimulationParams, VehicleParams
from .coords import body_to_inertial, direction_cosine_matrix, rotating_frame_derivative, angular_to_euler_rate
from .physics import thrust, torque, apply_forces_torques
from .helpers import control_allocation_matrix
......
......@@ -8,6 +8,15 @@ import numpy as np
@dataclass
class MotorParams:
r: float
"Motor resistance"
moment_of_inertia: float
"Moment of inertia about rotational axis"
d_f: float
"Viscous damping coefficient"
static_friction: float
# See: http://learningrc.com/motor-kv/, http://web.mit.edu/first/scooter/motormath.pdf
k_m: float
"Motor constant, where omega = k_m * back_emf, and k_m = 1/k_e"
......
......@@ -11,9 +11,9 @@ from matplotlib.animation import FuncAnimation
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Line3D
from coords import body_to_inertial, direction_cosine_matrix
from helpers import vehicle_params_factory
from simulation import Multirotor
from .coords import body_to_inertial, direction_cosine_matrix
from .helpers import vehicle_params_factory
from .simulation import Multirotor
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment