def load_data(self, param_file):
        with open(param_file, 'r') as inputf:
            self.parameters = json.load(inputf)

        # Set imported parameters as properties
        for parameter in self.parameters:
            setattr(self, parameter, self.parameters[parameter])
        # use for threadsafe commnications with the GUI thread
        self.mutex = QMutex()

        self.atmosphere = Atmosphere()

        # TODO Error analysis vs. ticksize
        self.ticksize = 0.001

        self.time = 0
        self.height = self.launch_height
        self.velocity = 0
        self.acceleration = 0

        self.max_height = 0
        self.max_velocity = 0
        self.max_acceleration = 0

        self.mass = self.launch_mass

        self.thruster = Thruster()
        self.data = {}
        self.data['time'] = []
        self.data['height'] = []
        self.data['velocity'] = []
        self.data['acceleration'] = []
Ejemplo n.º 2
0
    def __init__(self):
        '''
        Initialize the thrusters and PID controllers on Perseverance.

        Parameters:
            error_publisher: A MechOS publisher to send the publish the current error

        Returns:
            N/A
        '''
        #Get the mechos network parameters
        configs = MechOS_Network_Configs(
            MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #Initialize parameter server client to get and set parameters related to
        #the PID controller. This includes update time and PID contstants for
        #each degree of freedom.
        self.param_serv = mechos.Parameter_Server_Client(
            configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #Initialize serial connection to the maestro
        com_port = self.param_serv.get_param("COM_Ports/maestro")
        maestro_serial_obj = serial.Serial(com_port, 9600)

        #Initialize all 8 thrusters (max thrust 80%)
        max_thrust = float(self.param_serv.get_param("Control/max_thrust"))

        self.thrusters = [None, None, None, None, None, None, None, None]
        self.thrusters[0] = Thruster(maestro_serial_obj, 1, [0, 0, 1],
                                     [1, -1, 0], max_thrust, True)
        self.thrusters[1] = Thruster(maestro_serial_obj, 2, [0, 1, 0],
                                     [1, 0, 0], max_thrust, False)
        self.thrusters[2] = Thruster(maestro_serial_obj, 3, [0, 0, 1],
                                     [1, 1, 0], max_thrust, False)
        self.thrusters[3] = Thruster(maestro_serial_obj, 4, [1, 0, 0],
                                     [0, 1, 0], max_thrust, False)
        self.thrusters[4] = Thruster(maestro_serial_obj, 5, [0, 0, 1],
                                     [-1, 1, 0], max_thrust, True)
        self.thrusters[5] = Thruster(maestro_serial_obj, 6, [0, 1, 0],
                                     [-1, 0, 0], max_thrust, False)
        self.thrusters[6] = Thruster(maestro_serial_obj, 7, [0, 0, 1],
                                     [-1, -1, 0], max_thrust, False)
        self.thrusters[7] = Thruster(maestro_serial_obj, 8, [1, 0, 0],
                                     [0, -1, 0], max_thrust, False)

        #Initialize the PID controllers for control system
        self.set_up_PID_controllers()
Ejemplo n.º 3
0
    def __init__(self, config_filename):
        # Load vehicle configuration file
        with open(config_filename) as f:
            self.vehicle = yaml.load(f)

        # Initialize thrusters based off of configuration file
        self.thrusters = []
        for t_dict in self.vehicle['thrusters']:
            t = Thruster(t_dict['pos'], t_dict['rpy'], t_dict['flipped'])
            self.thrusters.append(t)

        # Creating wrench matrix with column vectors equal to the force and torque of each thruster
        self.wrenchmat = np.empty((6, len(self.thrusters)))
        for i, t in enumerate(self.thrusters):
            self.wrenchmat[0][i] = t.force_hat[0]
            self.wrenchmat[1][i] = t.force_hat[1]
            self.wrenchmat[2][i] = t.force_hat[2]
            self.wrenchmat[3][i] = t.torque[0]
            self.wrenchmat[4][i] = t.torque[1]
            self.wrenchmat[5][i] = t.torque[2]

        self.wrenchmat_pinv = np.linalg.pinv(
            self.wrenchmat)  # Calculate pseudoinverse of wrench matrix
Ejemplo n.º 4
0
	def __init__(self, pos):
		""" Them thar thrusters be lookin' dodgy. """
		
		self.state = "alive"
		
		self.pos = pos
		self.velocity = [0,0]
		self.omega = 0
		self.mass = 5.0
		self.reactionmass = 1.0
		self.density = 0
		self.rotation = 0
		#self.points = [[-10,-50],[5,-50],[5,-20],[10,-20],[10,30],[8,30],[12,50],[-12,50],[-8,30],[-12,30],[-12,-20],[-10,-20]]
		self.points = [[0,-50],[20,50],[-20,50]]
		self.radius = 30
		self.hull = 1.0
		
		self.target = 0
		
		s = 2.0
		
		self.thrusters = []
		
		self.thruster_fore_backward = Thruster(self,[0,-50],[0*s,1*s],1)
		self.thrusters.append(self.thruster_fore_backward)
		
		self.thruster_fore_right = Thruster(self,[0,-50],[-1*s,0*s],1)
		self.thrusters.append(self.thruster_fore_right)
		
		self.thruster_fore_left = Thruster(self,[0,-50],[1*s,0*s],1)
		self.thrusters.append(self.thruster_fore_left)
		
		self.thruster_rear_forward = Thruster(self,[0,50],[0*s,-1*s],1)
		self.thrusters.append(self.thruster_rear_forward)
		
		self.thruster_rear_right = Thruster(self,[0,50],[-1*s,0*s],1)
		self.thrusters.append(self.thruster_rear_right)
		
		self.thruster_rear_left = Thruster(self,[0,50],[1*s,0*s],1)
		self.thrusters.append(self.thruster_rear_left)
class RocketSimulator(QtCore.QObject):
    R_EARTH = 6371000 # meters
    G_0 = -9.80665 # m/s^2 (at sea level)

    new_data  = QtCore.pyqtSignal(object)


    def __init__(self, ticksize, param_file='parameters.json'):
        QtCore.QObject.__init__(self)
        self.load_data(param_file)


    def load_data(self, param_file):
        with open(param_file, 'r') as inputf:
            self.parameters = json.load(inputf)

        # Set imported parameters as properties
        for parameter in self.parameters:
            setattr(self, parameter, self.parameters[parameter])
        # use for threadsafe commnications with the GUI thread
        self.mutex = QMutex()

        self.atmosphere = Atmosphere()

        # TODO Error analysis vs. ticksize
        self.ticksize = 0.001

        self.time = 0
        self.height = self.launch_height
        self.velocity = 0
        self.acceleration = 0

        self.max_height = 0
        self.max_velocity = 0
        self.max_acceleration = 0

        self.mass = self.launch_mass

        self.thruster = Thruster()
        self.data = {}
        self.data['time'] = []
        self.data['height'] = []
        self.data['velocity'] = []
        self.data['acceleration'] = []


    def run_simulation(self):

        while self.height >= self.launch_height:
            self.run_tick()
        print(self.max_height, self.max_velocity, self.max_acceleration)

    def run_tick(self):

        self.height += self.velocity * self.ticksize
        self.velocity += self.acceleration * self.ticksize

        force = self.thrust_force() + self.drag_force() + self.gravity_force()
        self.acceleration = force / self.mass

        locked = False
        if self.mutex.tryLock(10):

            self.new_data.emit([self.time, self.height, self.velocity, self.acceleration])
            self.mutex.unlock()

        self.data['time'].append(self.time)
        self.data['height'].append(self.height)
        self.data['velocity'].append(self.velocity)
        self.data['acceleration'].append(self.acceleration)

        self.update_mass()
        self.update_max_values()
        self.time += self.ticksize

    def drag_force(self):
        pressure = self.atmosphere.get_density_by_height(self.height)
        # Rocket is heading up
        if self.velocity >= 0:
            drag_coef = self.rocket_drag_coef
            area = self.cross_sectional_area
        # Rocket is falling with parachute deployed
        else:
            drag_coef = self.parachute_drag_coef
            area = self.parachute_area

        # Drag force is the opposite direction of velocity
        if self.velocity > 0:
            direction = -1
        else:
             direction = 1

        # TODO use increased parachute area
        return (direction * drag_coef * pressure * self.velocity**2 * area ) / 2


    def gravity_force(self):
        return self.mass * self.get_g_at_alitude(self.height)

    def get_g_at_alitude(self, height):
        return self.G_0 * ((height + self.R_EARTH) / self.R_EARTH)**2


    def thrust_force(self):
        if self.time < self.burn_length:
            return self.thruster.get_thrust_at_time(self.time)
        else:
            return 0

    def update_mass(self):
        if self.time > self.burn_length:
            return
        else:
            self.mass -= (self.propellent_mass / self.burn_length) * self.ticksize

    def update_max_values(self):
        if self.height > self.max_height:
            self.max_height = self.height

        if self.velocity > self.max_velocity:
            self.max_velocity = self.velocity

        if self.acceleration > self.max_acceleration:
            self.max_acceleration = self.acceleration
Ejemplo n.º 6
0
    def __init__(self):
        '''
        Initialize the thrusters and PID controllers on Perseverance.

        Parameters:
            N/A

        Returns:
            N/A
        '''
        #Get the mechos network parameters
        configs = MechOS_Network_Configs(
            MECHOS_CONFIG_FILE_PATH)._get_network_parameters()

        #Initialize parameter server client to get and set parameters related to
        #the PID controller. This includes update time and PID contstants for
        #each degree of freedom.
        self.param_serv = mechos.Parameter_Server_Client(
            configs["param_ip"], configs["param_port"])
        self.param_serv.use_parameter_database(configs["param_server_path"])

        #Initialize serial connection to the maestro
        com_port = self.param_serv.get_param("COM_Ports/maestro")
        maestro_serial_obj = serial.Serial(com_port, 9600)

        #Initialize all 8 thrusters (max thrust 80%)
        max_thrust = float(self.param_serv.get_param("Control/max_thrust"))

        self.thrusters = [None, None, None, None, None, None, None, None]
        self.thrusters[0] = Thruster(maestro_serial_obj, 1, [0, 0, 1],
                                     [1, -1, 0], max_thrust, True)
        self.thrusters[1] = Thruster(maestro_serial_obj, 2, [0, 1, 0],
                                     [1, 0, 0], max_thrust, True)
        self.thrusters[2] = Thruster(maestro_serial_obj, 3, [0, 0, 1],
                                     [1, 1, 0], max_thrust, False)
        self.thrusters[3] = Thruster(maestro_serial_obj, 4, [1, 0, 0],
                                     [0, 1, 0], max_thrust, False)
        self.thrusters[4] = Thruster(maestro_serial_obj, 5, [0, 0, 1],
                                     [-1, 1, 0], max_thrust, True)
        self.thrusters[5] = Thruster(maestro_serial_obj, 6, [0, 1, 0],
                                     [-1, 0, 0], max_thrust, True)
        self.thrusters[6] = Thruster(maestro_serial_obj, 7, [0, 0, 1],
                                     [-1, -1, 0], max_thrust, False)
        self.thrusters[7] = Thruster(maestro_serial_obj, 8, [1, 0, 0],
                                     [0, -1, 0], max_thrust, True)

        #Used to notify when to hold depth based on the remote control trigger for depth.
        self.remote_desired_depth = 0
        self.remote_depth_recorded = False
        self.remote_yaw_min_thrust = float(
            self.param_serv.get_param("Control/Remote/yaw_min"))
        self.remote_yaw_max_thrust = float(
            self.param_serv.get_param("Control/Remote/yaw_max"))
        self.remote_x_min_thrust = float(
            self.param_serv.get_param("Control/Remote/x_min"))
        self.remote_x_max_thrust = float(
            self.param_serv.get_param("Control/Remote/x_max"))
        self.remote_y_min_thrust = float(
            self.param_serv.get_param("Control/Remote/y_min"))
        self.remote_y_max_thrust = float(
            self.param_serv.get_param("Control/Remote/y_max"))

        #Initialize the PID controllers for control system
        self.set_up_PID_controllers(True)
Ejemplo n.º 7
0
motor_kv = 920  # rpm / V
bus_voltage = 0.9 * 14  # V, 90% of full charge
max_rpm = motor_kv * bus_voltage

# Propeller and aero characteristics
prop_dia = 0.2  # m
effectiveness = 1e-7  # N/(rpm^2)
thrust_from_effort = lambda rpm: effectiveness * np.abs(rpm) * rpm  # N
effort_from_thrust = lambda thrust: np.sign(thrust) * np.sqrt(
    np.abs(thrust) / effectiveness)  # rpm
reaction_coeff = 0.25 * prop_dia  # (N*m)/N

# Quadcopter thrusters (sign of reaction_coeff determines handedness)
quad_thrusters = OrderedDict([
    ("front",
     Thruster((r, 0, 0.02), thrust_from_effort, effort_from_thrust,
              reaction_coeff, max_rpm)),
    ("left",
     Thruster((0, r, 0.02), thrust_from_effort, effort_from_thrust,
              -reaction_coeff, max_rpm)),
    ("back",
     Thruster((-r, 0, 0.02), thrust_from_effort, effort_from_thrust,
              reaction_coeff, max_rpm)),
    ("right",
     Thruster((0, -r, 0.02), thrust_from_effort, effort_from_thrust,
              -reaction_coeff, max_rpm))
])

# Multicopter thrusters (sign of reaction_coeff determines handedness)
R = np.array([[np.cos(2 * np.pi / 12), -np.sin(2 * np.pi / 12), 0],
              [np.sin(2 * np.pi / 12),
               np.cos(2 * np.pi / 12), 0], [0, 0, 1]])
Ejemplo n.º 8
0
'''
Author: David Pierce Walker-Howell
Date: 07/05/2019
Description: Quick and Dirty script made to test a single thruster on
            thruster id 1. This was used to test the esc's when the sub
            flooded. Worked well.
'''
import serial
from thruster import Thruster
import time

ser = serial.Serial("/dev/ttyACM0", 9600)
thruster_1 = Thruster(ser, 1, [0, 0, 0], [0, 0, 0], 40)
thruster_1.set_thrust(20)
time.sleep(2)
thruster_1.set_thrust(0)