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 __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()
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
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
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)
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]])
'''
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)