def _send_code(code):
pi = pigpio.pi()
tx = Transmitter(pi, PIN)
tx.send(code)
time.sleep(1)
tx.cancel()
from linear_code import LC
from random import randint
transmitter = Transmitter()
receiver = Receiver()
try:
if (len(argv) > 1):
filename = argv[1]
else:
filename = raw_input("Insert file name: ")
try:
P = list(
input(
"\n(Press ENTER to use a default [12, 3] linear code option)\nInsert P matrix as list of lists(rows): "
))
except SyntaxError:
P = [[0, 1, 1, 1, 0, 1, 0, 1, 0], [0, 0, 0, 1, 1, 1, 0, 1, 0],
[1, 1, 0, 0, 0, 0, 0, 1, 1]]
print(
"Warning: Current code can correct up to {} errors per word!\n".format(
LC.getErrorCorrectingCapability(P)))
maxNoise = int(raw_input("Insert max noise per word: "))
transmitter.readTextFromFile(filename)
transmitter.send(receiver, P, maxNoise)
except IOError:
print("File '{}' not found!".format(filename))
from transmitter import Transmitter
from receiver import Receiver
import atexit
SEND_ADDRESS = "c"
RECEIVE_ADDRESS = "c"
MINIMUM_BLINK_TIME = 1000
if __name__ == '__main__':
transmitter = Transmitter(minimum_blink_time = MINIMUM_BLINK_TIME, send_address = SEND_ADDRESS)
receiver = Receiver(transmitter, minimum_blink_time = MINIMUM_BLINK_TIME, address = RECEIVE_ADDRESS)
def cleanup():
transmitter.cleanup()
receiver.cleanup()
GPIO.cleanup()
atexit.register(cleanup)
while True:
message = input("message: ")
transmitter.send(message)
class D3MDevice:
def __init__(self, midi_in, midi_out, loopback, modes):
self.midi_in = midi_in
self.led_control = Transmitter(midi_out)
self.transmitter = Transmitter(loopback)
self.active_mode = None
self.active_bank = 0
self.button_state = [False] * 12
self.detune = 0
self.octave = [0, 0]
self.split_zone = 0
self.black_key_indices = [
1, 3, 6, 8, 10,
]
self.modes = modes
for mode in self.modes:
mode.register(self)
# set initial mode on start
self.set_mode(0)
print('Ready!')
def update(self):
message = self.midi_in.get_message()
if message is None:
return
command = message[0][0]
note = message[0][1]
velocity = message[0][2]
# undefined command, send to both channels
if command != NOTE_ON:
self.transmitter.send(command, note, velocity, SPLIT_LEFT)
self.transmitter.send(command, note, velocity, SPLIT_RIGHT)
return
# check if keybed is pressed
if note >= 36 and note <= 96:
# adjust velocity
if (note % 12) in self.black_key_indices and velocity > 0:
velocity -= (int)(math.log(velocity) * 2)
velocity = max(velocity, 0)
# select output channel
zone = ((note - 36) / 12) % NUM_SPLIT_ZONES
# detune note
note += self.detune
# select zone
if zone >= self.split_zone:
note += self.octave[SPLIT_RIGHT] * 12
self.transmitter.send(NOTE_ON, note, velocity, SPLIT_RIGHT)
else:
note += self.octave[SPLIT_LEFT] * 12
self.transmitter.send(NOTE_ON, note, velocity, SPLIT_LEFT)
# check preset buttons
elif note >= 0 and note < 12:
if not self.button_state[note] and velocity == 127:
self.active_mode.on_preset_pressed(note)
elif self.button_state[note] and velocity == 0:
self.active_mode.on_preset_released(note)
# check preset banks
elif note >= 14 and note < 24 and velocity == 127:
bank_index = note - 14
if bank_index != self.active_bank:
self.set_mode(bank_index)
self.active_bank = bank_index
def cc_send_on(self, note):
self.transmitter.send(NOTE_ON, note, 127, CONTROL_CHANNEL)
def cc_send_off(self, note):
self.transmitter.send(NOTE_OFF, note, 127, CONTROL_CHANNEL)
def toggle_light(self, number, state = True):
self.led_control.send(NOTE_ON, number, 127 if state else 0)
self.button_state[number] = state
def clear_lights(self):
for i in range(12):
self.toggle_light(i, False)
def set_mode(self, bank_index):
# set bank lights
for i in range(10):
self.led_control.send(NOTE_ON, i + 14, 0)
self.led_control.send(NOTE_ON, bank_index + 14, 127)
if self.modes[bank_index] is not self.active_mode:
self.clear_lights()
self.active_mode = self.modes[bank_index]
self.active_mode.on_enter()
def close(self):
self.clear_lights()
for i in range(10):
self.led_control.send(NOTE_ON, i + 14, 0)
class Server:
def __init__(self):
self.message = bytes(0)
self.transmitter = Transmitter(SERIAL)
self.location = 'imgs/foto_nova.png'
def set_location(self):
# name = input('SPECIFY NAME TO SAVE FILE WITHOUT EXTENSION: ')
name = 'foto_new2'
while len(name) < 1:
print('INVALID FILE NAME')
name = input('SPECIFY NAME TO SAVE FILE WITHOUT EXTENSION: ')
self.location = 'imgs/{}.png'.format(name)
def send_confirmation(self):
packet = Datagram()
packet.set_head(
message_type=3,
message_id=1,
num_payloads=0,
payload_index=0,
payload_size=0,
error_type=0,
restart_index=0
)
packet.set_EOP()
self.transmitter.send(packet.get_datagram())
def confirm_handshake(self):
handshake_received = self.transmitter.receive(expected_type='handshake')
print(handshake_received)
while not handshake_received:
print('HANDSHAKE NOT SENT BY CLIENT - LISTENING ON SERIAL PORT {}'.format(SERIAL))
handshake_received = self.transmitter.receive(expected_type='handshake')
print('HANDSHAKE RECEIVED')
self.send_confirmation()
print('HANDSHAKE CONFIRMED')
def send_error(self):
packet = Datagram()
packet.set_head(
message_type=0,
message_id=1,
num_payloads=0,
payload_index=0,
payload_size=0,
error_type=0,
restart_index=0
)
packet.set_EOP()
self.transmitter.send(packet.get_datagram())
def receive_packet(self):
response = self.transmitter.receive(expected_type='data')
if not response:
return 0
return response # response => tuple(payload, payload_index, num_packets)
def receive_message(self):
buffer = bytes()
last_payload = 0
while True:
response = self.receive_packet()
if not response:
print('ENCOUNTERED ERROR RECEIVING DATA')
self.send_error()
break
(payload, payload_index, num_packets) = response
if payload_index != last_payload + 1:
print('PAYLOAD OUT OF ORDER')
self.send_error()
break
last_payload = payload_index
buffer += payload
self.send_confirmation()
print('\n\n\n')
print(buffer)
print(payload_index, num_packets)
print('\n\n\n')
if payload_index == num_packets:
break
with open(self.location, 'wb') as f:
f.write(buffer)
def disable(self):
self.transmitter.disable()
def _send_code(code):
pi = pigpio.pi()
tx = Transmitter(pi, PIN)
tx.send(code)
time.sleep(1)
tx.cancel()
class Client:
def __init__(self):
self.message = bytes(0)
self.transmitter = Transmitter(SERIAL)
def set_message(self):
# file = input('What file do you want to send? ')
file = 'imgs/fofinha.png'
while not path.exists(file):
print("INVALID PATH")
file = input('WHAT FILE DO YOU WANT TO SEND? ')
with open(file, 'rb') as f:
byte_file = f.read()
self.message = byte_file
def send_packet(self, payload, num_payloads, payload_index):
packet = Datagram()
packet.set_head(message_type=1,
message_id=1,
num_payloads=num_payloads,
payload_index=payload_index,
payload_size=len(payload),
error_type=0,
restart_index=0)
packet.set_payload(payload=payload)
packet.set_EOP(0)
self.transmitter.send(packet.get_datagram())
def receive_confirmation(self):
return self.transmitter.receive(expected_type='confirmation')
def send_handshake(self):
packet = Datagram()
packet.set_head(message_type=2,
message_id=1,
num_payloads=0,
payload_index=0,
payload_size=0,
error_type=0,
restart_index=0)
packet.set_EOP()
self.transmitter.send(packet.get_datagram())
def assert_server_status(self):
self.send_handshake()
print('ASSERTING SERVER CONNECTION')
confirmed = self.receive_confirmation()
while not confirmed:
print('SERVER DISCONNECTED')
retry = input('RETRY CONNECTION? (y/n) ')
if retry == 'y':
self.send_handshake()
print('ASSERTING SERVER CONNECTION')
confirmed = self.receive_confirmation()
else:
self.disable()
sys.exit()
print('SERVER AWAKE')
def send_message(self):
num_bytes = len(self.message)
num_packets = num_bytes // 114 + 1 if num_bytes % 114 != 0 else num_bytes // 114
print('BEGINNING TRANSMISSION')
print('BYTES TO SEND: {}'.format(num_bytes))
print('NUMBER OF PAYLOADS: {}'.format(num_packets))
for i in range(1, num_packets + 1):
payload = self.message[(i - 1) * 114:i * 114]
self.send_packet(payload=payload,
payload_index=i,
num_payloads=num_packets)
if not self.receive_confirmation():
break
def disable(self):
self.transmitter.disable()
class Communication:
def __init__(self, ser):
self._last_angles = np.ndarray
self._last_imu = np.ndarray
# https://www.pieter-jan.com/node/11
self.pitch_acc = 0
self.roll_acc = 0
self.pitch = 0
self.roll = 0
self._tx_thread = Transmitter(name="tx_th", ser=ser)
self._rx_thread = Receiver(name="rx_th", ser=ser)
self._rx_thread.set_timeout(0.010)
self._rx_thread.bind(self.receive_callback)
self._pub_imu = rp.Publisher('imu_raw', Imu, queue_size=1)
self._pub_joint_states = rp.Publisher('joint_states',
JointState,
queue_size=1)
self._motor_map = rp.get_param("~motor_mapping")
self._imu_calibration = rp.get_param("~imu_calibration")
for motor in self._motor_map:
self._motor_map[motor]["subscriber"] = rp.Subscriber(
motor + "/command", Float64, self.trajectory_callback, motor)
self._motor_map[motor]["publisher"] = rp.Publisher(
motor + "/state", JointControllerState, queue_size=1)
self._motor_map[motor]["value"] = 0.0
self._publish_timer = rp.Timer(rp.Duration(nsecs=10000000),
self.send_angles)
def run(self):
self._rx_thread.start()
self._tx_thread.start()
tx_cycle = WaitForMs(10)
tx_cycle.set_e_gain(1.5)
# Never need to wait longer than the target time, but allow calls to
# time.sleep for down to 3 ms less than the desired time
tx_cycle.set_e_lim(0, -3.0)
rp.spin()
def trajectory_callback(self, robot_goal, motor):
self._motor_map[motor]["value"] = robot_goal.data
def send_angles(self, event):
motor_angles = [0] * len(self._motor_map)
for motor in self._motor_map:
motor_angles[int(self._motor_map[motor]["id"])] = (
np.rad2deg(self._motor_map[motor]["value"] *
float(self._motor_map[motor]["direction"])) +
float(self._motor_map[motor]["offset"]))
self._tx_thread.send(motor_angles)
def receive_callback(self, received_angles, received_imu):
self._last_angles = received_angles
self._last_imu = received_imu
self.publish_sensor_data(received_angles, received_imu)
def publish_sensor_data(self, received_angles, received_imu):
# IMU FEEDBACK
imu = Imu()
imu.header.stamp = rp.rostime.get_rostime()
imu.header.frame_id = "imu_link"
# TODO autocalibrate
imu.angular_velocity = Vector3(
(-received_imu[2][0] - self._imu_calibration["gyro_offset"][0]) *
self._imu_calibration["gryo_scale"][0],
(received_imu[1][0] - self._imu_calibration["gyro_offset"][1]) *
self._imu_calibration["gryo_scale"][1],
(received_imu[0][0] - self._imu_calibration["gyro_offset"][2]) *
self._imu_calibration["gryo_scale"][2])
imu.linear_acceleration = Vector3(
(received_imu[5][0] - self._imu_calibration["acc_offset"][0]) *
self._imu_calibration["acc_scale"][0],
(received_imu[4][0] - self._imu_calibration["acc_offset"][1]) *
self._imu_calibration["acc_scale"][1],
(received_imu[3][0] - self._imu_calibration["acc_offset"][2]) *
self._imu_calibration["acc_scale"][2])
imu.orientation_covariance[0] = -1
self._pub_imu.publish(imu)
# MOTOR FEEDBACK
joint_state = JointState()
joint_state.header.stamp = rp.rostime.get_rostime()
for motor in self._motor_map:
if int(self._motor_map[motor]["id"]) < 12:
assert (int(self._motor_map[motor]["id"]) <
len(received_angles))
angle = received_angles[int(self._motor_map[motor]["id"])]
if math.isnan(angle): # TODO fix this
continue
angle = (angle - float(self._motor_map[motor]["offset"])
) * float(self._motor_map[motor]["direction"])
angle = np.deg2rad(angle)
else:
angle = self._motor_map[motor]["value"]
# Joint controller state
state = JointControllerState()
state.process_value = angle
state.command = self._motor_map[motor]["value"]
state.error = angle - self._motor_map[motor]["value"]
state.process_value_dot = 0 # TODO PID settings and process value dot
state.header.stamp = rp.rostime.get_rostime()
self._motor_map[motor]["publisher"].publish(state)
# Joint State
joint_state.name.append(motor)
joint_state.position.append(angle)
self._pub_joint_states.publish(joint_state)
