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

converted to relative imports; motor simulation;

parent 4cf5fd7e

control.py

+1 −1
Original line number Diff line number Diff line
@@ -2,7 +2,7 @@ from dataclasses import dataclass 


import numpy as np



from simulation import Multirotor

from .simulation import Multirotor

helpers.py

+1 −1
Original line number Diff line number Diff line
@@ -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

motor.py

0 → 100755
+60 −0
Original line number Diff line number Diff line 
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

physics.py

+4 −4
Original line number Diff line number Diff line
@@ -83,7 +83,7 @@ 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
@@ -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

simulation.py

+4 −4
Original line number Diff line number Diff line
@@ -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