GPIO.setmode(GPIO.BCM)
GPIO.setup(LED, GPIO.OUT) # status LED
GPIO.output(LED, GPIO.HIGH) # switch on to indicate software startup
GPIO.setup(RELEASE, GPIO.OUT) # release pin
GPIO.output(RELEASE, GPIO.LOW) # disable release
GPIO.setup(RELEASE_FB, GPIO.IN) # release feedback pin
GPIO.setup(HEADSHOT, GPIO.OUT) # set shutoff to output
GPIO.output(HEADSHOT, GPIO.LOW) # disable shutoff
GPIO.setup(CAMERA, GPIO.OUT) # set cam power to output
GPIO.output(CAMERA, GPIO.HIGH) # enable camera power
# setup the transmitter thread
txthread = Transmitter()
txthread.setDaemon(True)
txthread.start()
# setup the GPS-listener thread
gpsthread = GPSListener()
gpsthread.setDaemon(True)
gpsthread.start()
# wait for at least a 2D-Fix from the GPS
while GPSListener.fix < 2:
pass
logging.info("MC GPS fix OK")
# indicate GPS fix on LED
for i in range(1,15):
GPIO.output(LED, GPIO.HIGH)
time.sleep(0.5)
from collector import Ping
from transmitter import Transmitter
output_queue = None
def enqueue_output(data):
""" Accepts a dictionary containg send_time and replies[] """
output_queue.put(data)
print("enqueued output")
def dequeue_output():
""" Pops a dictionary from the output queue """
return output_queue.get()
def collector_run():
hosts = ('192.168.5.5', '192.168.5.18', '8.8.8.8', 'ucla.edu')
pinger = Ping(hosts, enqueue_output)
pinger.run()
if __name__ == '__main__':
output_queue = queue.Queue()
collector_thread = threading.Thread(target=collector_run, name="collector")
recorder_url = 'ws://localhost:8765/'
transmitter = Transmitter(dequeue_output, recorder_url)
collector_thread.start()
transmitter.start()
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)
