def main(args):
com = Communicator(module_name=args.module_name)
#incoming_packet = {"forward/backward": 0.0, "right/left": 0.0, "up/down": 0.0, "yaw": 0.0, "roll": 0.0, "pitch": 0.0}
last_packet_time = 0
while True:
incoming_packet = com.get_last_message("movement/fuzzification")
if incoming_packet and incoming_packet['timestamp'] > last_packet_time:
last_packet_time = incoming_packet['timestamp']
try:
outgoing_packet = {"front_left": 0.0, "front_right": 0.0, "middle_left": 0.0, "middle_right": 0.0, "back_left": 0.0, "back_right": 0.0}
# Possible TODO: Scale all values except for forward/backward by some constant so yaw, strafe, etc, are easier to control
outgoing_packet["front_left"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] + incoming_packet["Fuzzy_Sets"]["right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["front_right"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] - incoming_packet["Fuzzy_Sets"]["right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_left"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] - incoming_packet["Fuzzy_Sets"]["right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_right"] = incoming_packet["Fuzzy_Sets"]["forward/backward"] + incoming_packet["Fuzzy_Sets"]["right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["middle_left"] = incoming_packet["Fuzzy_Sets"]["up/down"] + incoming_packet["Fuzzy_Sets"]["roll"]
outgoing_packet["middle_right"] = incoming_packet["Fuzzy_Sets"]["up/down"] - incoming_packet["Fuzzy_Sets"]["roll"]
for key in outgoing_packet.keys():
if outgoing_packet[key] > 1.0: outgoing_packet[key] = 1.0
if outgoing_packet[key] < -1.0: outgoing_packet[key] = -1.0
print outgoing_packet
com.publish_message({"Defuzzified_Sets": outgoing_packet})
except KeyError as e:
pass
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name)
fuzzy_sets = settings[args.module_name]["fuzzy_sets"]
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
# Expected packet sent by this module:
# packet = {
# 'is_left': 0.0,
# 'is_right': 0.0,
# 'is_back': 0.0,
# 'is_forward': 0.0,
# 'is_low': 0.0,
# 'is_high': 0.0,
# 'is_rotated_left': 0.0,
# 'is_rotated_right': 0.0}
# Received missive is of the form:
# {"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
# "desired_orientation": {"yaw": 0.0, "pitch": 0.0, "roll": 0.0},
# "desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
# "face_of_power": self.face_of_power,
# "timestamp": time.time()}
last_timestamp = 0.0
while True:
# TODO: Figure out AI module name
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
packet = {}
packet['is_left'] = get_membership(missive['desired_offset']['x'],
fuzzy_sets['is_left'])
packet['is_right'] = get_membership(missive['desired_offset']['x'],
fuzzy_sets['is_right'])
packet['is_back'] = get_membership(missive['desired_offset']['y'],
fuzzy_sets['is_back'])
packet['is_forward'] = get_membership(
missive['desired_offset']['y'], fuzzy_sets['is_forward'])
packet['is_low'] = get_membership(missive['desired_offset']['z'],
fuzzy_sets['is_low'])
packet['is_high'] = get_membership(missive['desired_offset']['z'],
fuzzy_sets['is_high'])
packet['is_rotated_left'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_left'])
packet['is_rotated_right'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_right'])
com.publish_message(packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name)
fuzzy_sets = settings[args.module_name]["fuzzy_sets"]
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
# Expected packet sent by this module:
# packet = {
# 'is_left': 0.0,
# 'is_right': 0.0,
# 'is_back': 0.0,
# 'is_forward': 0.0,
# 'is_low': 0.0,
# 'is_high': 0.0,
# 'is_rotated_left': 0.0,
# 'is_rotated_right': 0.0}
# Received missive is of the form:
# {"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
# "desired_orientation": {"yaw": 0.0, "pitch": 0.0, "roll": 0.0},
# "desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
# "face_of_power": self.face_of_power,
# "timestamp": time.time()}
last_timestamp = 0.0
while True:
# TODO: Figure out AI module name
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
packet = {}
packet['is_left'] = get_membership(
missive['desired_offset']['x'], fuzzy_sets['is_left'])
packet['is_right'] = get_membership(
missive['desired_offset']['x'], fuzzy_sets['is_right'])
packet['is_back'] = get_membership(
missive['desired_offset']['y'], fuzzy_sets['is_back'])
packet['is_forward'] = get_membership(
missive['desired_offset']['y'], fuzzy_sets['is_forward'])
packet['is_low'] = get_membership(
missive['desired_offset']['z'], fuzzy_sets['is_low'])
packet['is_high'] = get_membership(
missive['desired_offset']['z'], fuzzy_sets['is_high'])
packet['is_rotated_left'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_left'])
packet['is_rotated_right'] = get_membership(
missive['desired_orientation']['yaw'],
fuzzy_sets['is_rotated_right'])
com.publish_message(packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name,
settings_path=args.settings_path)
# TODO We need to settle on a good convention for these vectors.
# For now, I'm using these conventions:
# vector:
# x is left and right
# y is forward and backward
# z is up and down.
# rotation:
# x is pitch
# y is roll
# z is yaw. Negative yaws left, positive yaws right.
packet = {
'vector': {
'x': 0.0,
'y': 0.0,
'z': 0.0
},
'rotation': {
'x': 0.0,
'y': 0.0,
'z': 0.0
}
}
last_packet_time = 0.0
while True:
directive_packet = com.get_last_message("movement/directive")
if directive_packet and directive_packet[
'timestamp'] > last_packet_time:
last_packet_time = directive_packet['timestamp']
# Kludges to handle keyboard inputs.
tx_packet = deepcopy(packet)
if directive_packet['is_left'] == 1.0:
tx_packet['vector']['x'] = 1.0
elif directive_packet['is_right'] == 1.0:
tx_packet['vector']['x'] = -1.0
elif directive_packet['is_back'] == 1.0:
tx_packet['vector']['y'] = 1.0
elif directive_packet['is_forward'] == 1.0:
tx_packet['vector']['y'] = -1.0
elif directive_packet['is_low'] == 1.0:
tx_packet['vector']['z'] = 1.0
elif directive_packet['is_high'] == 1.0:
tx_packet['vector']['z'] = -1.0
elif directive_packet['is_rotated_left'] == 1.0:
tx_packet['rotation']['z'] = 1.0
elif directive_packet['is_rotated_right'] == 1.0:
tx_packet['rotation']['z'] = -1.0
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(
module_name=args.module_name,
settings_path=args.settings_path)
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
packet = {
'is_left': 0.0,
'is_right': 0.0,
'is_back': 0.0,
'is_forward': 0.0,
'is_low': 0.0,
'is_high': 0.0,
'is_rotated_left': 0.0,
'is_rotated_right': 0.0}
missive = {
"desired_offset": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_orientation": {"x": 0.0, "y": 0.0, "z": 0.0},
"desired_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
"timestamp": time.time()}
last_timestamp = 0.0
while True:
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
tx_packet = deepcopy(packet)
if (missive['desired_offset']['y'] == 9001.0 and
missive['desired_velocity']['y'] == 1.0):
tx_packet['is_back'] = 1.0
elif (missive['desired_offset']['y'] == -9001 and
missive['desired_velocity']['y'] == -1.0):
tx_packet['is_forward'] = 1.0
elif missive['desired_orientation']['z'] == 3 * math.pi / 2:
tx_packet['is_rotated_right'] = 1.0
elif missive['desired_orientation']['z'] == math.pi / 2:
tx_packet['is_rotated_left'] = 1.0
elif (missive['desired_offset']['z'] == 9001 and
missive['desired_velocity']['z'] == 1.0):
tx_packet['is_low'] = 1.0
elif (missive['desired_offset']['z'] == -9001 and
missive['desired_velocity']['z'] == -1.0):
tx_packet['is_high'] = 1.0
print tx_packet
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name)
#incoming_packet = {"forward/backward": 0.0, "right/left": 0.0, "up/down": 0.0, "yaw": 0.0, "roll": 0.0, "pitch": 0.0}
last_packet_time = 0
while True:
incoming_packet = com.get_last_message("movement/fuzzification")
if incoming_packet and incoming_packet['timestamp'] > last_packet_time:
last_packet_time = incoming_packet['timestamp']
try:
outgoing_packet = {
"front_left": 0.0,
"front_right": 0.0,
"middle_left": 0.0,
"middle_right": 0.0,
"back_left": 0.0,
"back_right": 0.0
}
# Possible TODO: Scale all values except for forward/backward by some constant so yaw, strafe, etc, are easier to control
outgoing_packet["front_left"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] + incoming_packet["Fuzzy_Sets"][
"right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["front_right"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] - incoming_packet["Fuzzy_Sets"][
"right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_left"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] - incoming_packet["Fuzzy_Sets"][
"right/left"] + incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["back_right"] = incoming_packet["Fuzzy_Sets"][
"forward/backward"] + incoming_packet["Fuzzy_Sets"][
"right/left"] - incoming_packet["Fuzzy_Sets"]["yaw"]
outgoing_packet["middle_left"] = incoming_packet["Fuzzy_Sets"][
"up/down"] + incoming_packet["Fuzzy_Sets"]["roll"]
outgoing_packet[
"middle_right"] = incoming_packet["Fuzzy_Sets"][
"up/down"] - incoming_packet["Fuzzy_Sets"]["roll"]
for key in outgoing_packet.keys():
if outgoing_packet[key] > 1.0: outgoing_packet[key] = 1.0
if outgoing_packet[key] < -1.0: outgoing_packet[key] = -1.0
print outgoing_packet
com.publish_message({"Defuzzified_Sets": outgoing_packet})
except KeyError as e:
pass
time.sleep(args.epoch)
def main(args):
com = Communicator("movement/physical")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/stabilization")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
last_packet_time = rx_packet['timestamp']
tx_packet = {
'vector': rx_packet['vector'],
'rotation': rx_packet['rotation']}
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator("movement/physical")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/stabilization")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
last_packet_time = rx_packet['timestamp']
tx_packet = {
'vector': rx_packet['vector'],
'rotation': rx_packet['rotation']
}
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
com = Communicator("movement/translation")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/defuzzification")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
tx_packet = {'Thruster_Values': {}}
last_packet_time = rx_packet['timestamp']
for k in rx_packet['Defuzzified_Sets']:
tx_packet['Thruster_Values'][k] = \
translate(k, rx_packet['Defuzzified_Sets'][k])
com.publish_message(tx_packet)
print tx_packet
time.sleep(args.epoch)
def main(args):
com = Communicator("movement/translation")
last_packet_time = 0.0
while True:
rx_packet = com.get_last_message("movement/defuzzification")
if rx_packet and rx_packet['timestamp'] > last_packet_time:
tx_packet = {'Thruster_Values': {}}
last_packet_time = rx_packet['timestamp']
for k in rx_packet['Defuzzified_Sets']:
tx_packet['Thruster_Values'][k] = \
translate(k, rx_packet['Defuzzified_Sets'][k])
com.publish_message(tx_packet)
print tx_packet
time.sleep(args.epoch)
def main(args):
com = Communicator(module_name=args.module_name)
# TODO We need to settle on a good convention for these vectors.
# For now, I'm using these conventions:
# vector:
# x is left and right
# y is forward and backward
# z is up and down.
# rotation:
# x is pitch
# y is roll
# z is yaw. Negative yaws left, positive yaws right.
packet = {"vector": {"x": 0.0, "y": 0.0, "z": 0.0}, "rotation": {"yaw": 0.0, "pitch": 0.0, "roll": 0.0}}
last_packet_time = 0.0
while True:
directive_packet = com.get_last_message("movement/directive")
if directive_packet and directive_packet["timestamp"] > last_packet_time:
last_packet_time = directive_packet["timestamp"]
# Kludges to handle keyboard inputs.
tx_packet = deepcopy(packet)
if directive_packet["is_left"] > 0.0:
tx_packet["vector"]["x"] = 1.0
elif directive_packet["is_right"] > 0.0:
tx_packet["vector"]["x"] = -1.0
elif directive_packet["is_back"] > 0.0:
tx_packet["vector"]["y"] = 1.0
elif directive_packet["is_forward"] > 0.0:
tx_packet["vector"]["y"] = -1.0
elif directive_packet["is_low"] > 0.0:
tx_packet["vector"]["z"] = 1.0
elif directive_packet["is_high"] > 0.0:
tx_packet["vector"]["z"] = -1.0
elif directive_packet["is_rotated_left"] > 0.0:
tx_packet["rotation"]["yaw"] = 1.0
elif directive_packet["is_rotated_right"] > 0.0:
tx_packet["rotation"]["yaw"] = -1.0
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
# Someone SHOULD complain about this.
global DEBUG
DEBUG = args.debug
com = Communicator("microcontroller")
accel_com = Communicator('datafeed/raw/accelerometer')
gyro_com = Communicator('datafeed/raw/gyroscope')
compass_com = Communicator('datafeed/raw/compass')
depth_com = Communicator('datafeed/raw/depth')
battery_voltage_com = Communicator('datafeed/raw/battery_voltage')
# If we want to insert a mock module for these sensors, we want to prevent
# multiple input sources from inserting messages into the pipe.
disabled_publish = lambda *args: None
if args.disable_accel_com:
accel_com.publish_message = disabled_publish
if args.disable_gyro_com:
gyro_com.publish_message = disabled_publish
if args.disable_compass_com:
compass_com.publish_message = disabled_publish
if args.disable_depth_com:
depth_com.publish_message = disabled_publish
if args.disable_battery_voltage_com:
battery_voltage_com.publish_message = disabled_publish
if not DEBUG:
ser = serial.Serial()
cmd_thruster.ser = ser
# this may change, depending on what port the OS gives the
# microcontroller
ser.port = args.port
# the baudrate may change in the future
ser.baudrate = args.baudrate
# attempt to open the serial port (there is no guard code, I'm assuming
# this does not fail)
ser.open()
get_lock(ser, com) # get in sync with the stream
prev_gv_timestamp = 0.0
while True:
gv_packet = com.get_last_message("movement/translation")
# handle values from grapevine.py
if gv_packet is not None and gv_packet['timestamp'] > prev_gv_timestamp:
cmd_thruster(THRUSTER_BOW_PORT,
gv_packet["Thruster_Values"]["front_left"])
cmd_thruster(THRUSTER_BOW_SB,
gv_packet["Thruster_Values"]["front_right"])
cmd_thruster(THRUSTER_DEPTH_PORT,
gv_packet["Thruster_Values"]["middle_left"])
cmd_thruster(THRUSTER_DEPTH_SB,
gv_packet["Thruster_Values"]["middle_right"])
cmd_thruster(THRUSTER_STERN_PORT,
gv_packet["Thruster_Values"]["back_left"])
cmd_thruster(THRUSTER_STERN_SB,
gv_packet["Thruster_Values"]["back_right"])
prev_gv_timestamp = gv_packet['timestamp']
# handle values from uC, USB port.
if not DEBUG:
uC_packet = get_packet(ser, com)
respond_to_serial_packet(uC_packet, accel_com, gyro_com,
compass_com, depth_com,
battery_voltage_com)
time.sleep(args.epoch)
if not DEBUG:
ser.close()
def main(args):
com = Communicator(module_name=args.module_name,
settings_path=args.settings_path)
# These values represent the submarine's membership in 8 fuzzy sets.
# These sets come in pairs (left/right, back/forward, etc.) and represent
# where the submarine is in relation to where it wants to be.
# The roll and yaw sets are not available to directive module since
# they should be controlled by either the stabilization module, or by
# the awesome balancing skills of the mech-e's.
packet = {
'is_left': 0.0,
'is_right': 0.0,
'is_back': 0.0,
'is_forward': 0.0,
'is_low': 0.0,
'is_high': 0.0,
'is_rotated_left': 0.0,
'is_rotated_right': 0.0
}
missive = {
"desired_offset": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"desired_orientation": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"desired_velocity": {
"x": 0.0,
"y": 0.0,
"z": 0.0
},
"timestamp": time.time()
}
last_timestamp = 0.0
while True:
missive = com.get_last_message("decision")
if missive and missive['timestamp'] > last_timestamp:
last_timestamp = missive['timestamp']
tx_packet = deepcopy(packet)
if (missive['desired_offset']['y'] == 9001.0
and missive['desired_velocity']['y'] == 1.0):
tx_packet['is_back'] = 1.0
elif (missive['desired_offset']['y'] == -9001
and missive['desired_velocity']['y'] == -1.0):
tx_packet['is_forward'] = 1.0
elif missive['desired_orientation']['z'] == 3 * math.pi / 2:
tx_packet['is_rotated_right'] = 1.0
elif missive['desired_orientation']['z'] == math.pi / 2:
tx_packet['is_rotated_left'] = 1.0
elif (missive['desired_offset']['z'] == 9001
and missive['desired_velocity']['z'] == 1.0):
tx_packet['is_low'] = 1.0
elif (missive['desired_offset']['z'] == -9001
and missive['desired_velocity']['z'] == -1.0):
tx_packet['is_high'] = 1.0
print tx_packet
com.publish_message(tx_packet)
time.sleep(args.epoch)
def main(args):
# Someone SHOULD complain about this.
global DEBUG
DEBUG = args.debug
com = Communicator("microcontroller")
accel_com = Communicator('datafeed/raw/accelerometer')
gyro_com = Communicator('datafeed/raw/gyroscope')
compass_com = Communicator('datafeed/raw/compass')
depth_com = Communicator('datafeed/raw/depth')
battery_voltage_com = Communicator('datafeed/raw/battery_voltage')
# If we want to insert a mock module for these sensors, we want to prevent
# multiple input sources from inserting messages into the pipe.
disabled_publish = lambda *args: None
if args.disable_accel_com:
accel_com.publish_message = disabled_publish
if args.disable_gyro_com:
gyro_com.publish_message = disabled_publish
if args.disable_compass_com:
compass_com.publish_message = disabled_publish
if args.disable_depth_com:
depth_com.publish_message = disabled_publish
if args.disable_battery_voltage_com:
battery_voltage_com.publish_message = disabled_publish
if not DEBUG:
ser = serial.Serial()
cmd_thruster.ser = ser
# this may change, depending on what port the OS gives the
# microcontroller
ser.port = args.port
# the baudrate may change in the future
ser.baudrate = args.baudrate
# attempt to open the serial port (there is no guard code, I'm assuming
# this does not fail)
ser.open()
get_lock(ser, com) # get in sync with the stream
prev_gv_timestamp = 0.0
while True:
gv_packet = com.get_last_message("movement/translation")
# handle values from grapevine.py
if gv_packet is not None and gv_packet['timestamp'] > prev_gv_timestamp:
cmd_thruster(THRUSTER_BOW_PORT, gv_packet["Thruster_Values"]["front_left"])
cmd_thruster(THRUSTER_BOW_SB, gv_packet["Thruster_Values"]["front_right"])
cmd_thruster(THRUSTER_DEPTH_PORT, gv_packet["Thruster_Values"]["middle_left"])
cmd_thruster(THRUSTER_DEPTH_SB, gv_packet["Thruster_Values"]["middle_right"])
cmd_thruster(THRUSTER_STERN_PORT, gv_packet["Thruster_Values"]["back_left"])
cmd_thruster(THRUSTER_STERN_SB, gv_packet["Thruster_Values"]["back_right"])
prev_gv_timestamp = gv_packet['timestamp']
# handle values from uC, USB port.
if not DEBUG:
uC_packet = get_packet(ser, com)
respond_to_serial_packet(uC_packet, accel_com, gyro_com, compass_com,
depth_com, battery_voltage_com)
time.sleep(args.epoch)
if not DEBUG:
ser.close()
