This will assume the wiimote has a motionplus attached - if it doesn't, no
cwiid.MESG_MOTIONPLUS events will be recorded, and it will just plot the three graphs for the accelerometers rather than the five for accelerometers and motionplus.
#!/usr/bin/python
import cwiid
from time import time, asctime, sleep
from numpy import *
from pylab import *
def plotter(plot_title, timevector, data, position, n_graphs):
subplot(n_graphs, 1, position)
plot(timevector, data[0], "r",
timevector, data[1], "g",
timevector, data[2], "b")
xlabel("time (s)")
ylabel(plot_title)
print "Press 1+2 on the Wiimote now"
wiimote = cwiid.Wiimote()
# Rumble to indicate a connection
wiimote.rumble = 1
print "Connection established - release buttons"
sleep(0.2)
wiimote.rumble = 0
sleep(1.0)
wiimote.enable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS)
wiimote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC | cwiid.RPT_MOTIONPLUS
print "Press plus to start recording, minus to end recording"
loop = True
record = False
accel_data = []
angle_data = []
while (loop):
sleep(0.01)
messages = wiimote.get_mesg()
for mesg in messages:
# Motion plus:
if mesg[0] == cwiid.MESG_MOTIONPLUS:
if record:
angle_data.append({"Time" : time(), \
"Rate" : mesg[1]['angle_rate']})
# Accelerometer:
elif mesg[0] == cwiid.MESG_ACC:
if record:
accel_data.append({"Time" : time(), "Acc" : mesg[1]})
# Button:
elif mesg[0] == cwiid.MESG_BTN:
if mesg[1] & cwiid.BTN_PLUS and not record:
print "Recording - press minus button to stop"
record = True
start_time = time()
if mesg[1] & cwiid.BTN_MINUS and record:
if len(accel_data) == 0:
print "No data recorded"
else:
print "End recording"
print "{0} data points in {1} seconds".format(
len(accel_data), time() - accel_data[0]["Time"])
record = False
loop = False
else:
pass
wiimote.disable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS)
if len(accel_data) == 0:
sys.exit()
timevector = []
a = [[],[],[]]
v = [[],[],[]]
p = [[],[],[]]
last_time = 0
velocity = [0,0,0]
position = [0,0,0]
for n, x in enumerate(accel_data):
if (n == 0):
origin = x
else:
elapsed = x["Time"] - origin["Time"]
delta_t = x["Time"] - last_time
timevector.append(elapsed)
for i in range(3):
acceleration = x["Acc"][i] - origin["Acc"][i]
velocity[i] = velocity[i] + delta_t * acceleration
position[i] = position[i] + delta_t * velocity[i]
a[i].append(acceleration)
v[i].append(velocity[i])
p[i].append(position[i])
last_time = x["Time"]
n_graphs = 3
if len(angle_data) == len(accel_data):
n_graphs = 5
ar = [[],[],[]] # Angle rates
aa = [[],[],[]] # Angles
angle = [0,0,0]
for n, x in enumerate(angle_data):
if (n == 0):
origin = x
else:
delta_t = x["Time"] - last_time
for i in range(3):
rate = x["Rate"][i] - origin["Rate"][i]
angle[i] = angle[i] + delta_t * rate
ar[i].append(rate)
aa[i].append(angle[i])
last_time = x["Time"]
plotter("Acceleration", timevector, a, 1, n_graphs)
plotter("Velocity", timevector, v, 2, n_graphs)
plotter("Position", timevector, p, 3, n_graphs)
if n_graphs == 5:
plotter("Angle Rate", timevector, ar, 4, n_graphs)
plotter("Angle", timevector, aa, 5, n_graphs)
show()
