def main():
hub = myo.Hub()
hub.set_locking_policy(myo.locking_policy.none)
hub.run(1000, Listener())
# Listen to keyboard interrupts and stop the
# hub in that case.
try:
while hub.running:
print(str(pose_now))
if 'rest' in pose_now:
print(0)
elif 'in' in pose_now:
print(1)
elif 'out' in pose_now:
print(2)
elif 'fist' in pose_now:
print(3)
elif 'double' in pose_now:
print(4)
elif 'fingers_spread' in pose_now:
print(5)
myo.time.sleep(0.2)
# print(pose_now)
# hub.
except KeyboardInterrupt:
print_("Quitting ...")
hub.stop(True)
hub.run(1000, Listener())
def on_disconnect(self, myo, timestamp):
''' (Listener, Myo, str) -> NoneType
Displays info when a connection is terminated
'''
if is_debug:
print_('on_disconnect')
def on_connect(self, myo, timestamp):
print_("Connected to Myo")
myo.vibrate('short')
myo.set_stream_emg(stream_emg.enabled)
myo.request_rssi()
global start
start = time.time()
def on_pose(self, myo, timestamp, pose):
self.nowpose = pose
print_('on_pose', pose)
if pose == "fist":
self.fistcount = self.fistcount + 1
if self.fistcount == 1:
self.state = 0
print("Entering state 0")
def on_pose(self, myo, timestamp, pose):
self.nowpose=pose
print_('on_pose', pose)
if pose == "fist":
self.fistcount=self.fistcount+1
if self.fistcount==1:
self.state=0
print("Entering state 0")
def on_pair(self, myo, timestamp):
''' (Listener, Myo, str) -> NoneType
Displays info when a connection has been established
'''
if is_debug:
print_('Paired')
print_("If you don't see any responses to your movements, try re-running the program or making sure the Myo works with Myo Connect (from Thalmic Labs).")
def on_connect(self, myo, timestamp):
''' (Listener, Myo, str) -> NoneType
Vibrates the Myo band once a connection has been established
'''
if is_debug:
print_("Connected to Myo")
myo.vibrate('short')
myo.request_rssi()
def on_pose(self, myo, timestamp, pose):
print_('on_pose', pose)
self.data["on_pose"] = { "timestamp" : timestamp, "data" : pose }
msgs = []
if pose == pose_t.double_tap:
msgs += [ HMTLprotocol.get_rgb_msg(self.options.hmtladdress,
1,
255,0,255) ]
elif pose == pose_t.rest:
msgs += [
HMTLprotocol.get_rgb_msg(self.options.hmtladdress,
1,
0,0,0),
HMTLprotocol.get_value_msg(69,
0,
0),
HMTLprotocol.get_value_msg(69,
1,
0)
]
elif pose == pose_t.wave_in:
msgs += [ HMTLprotocol.get_rgb_msg(self.options.hmtladdress,
1,
0,255,0) ]
elif pose == pose_t.wave_out:
msgs += [ HMTLprotocol.get_rgb_msg(self.options.hmtladdress,
1,
0,0,255) ]
elif pose == pose_t.fist:
msgs += [
HMTLprotocol.get_rgb_msg(self.options.hmtladdress,
1,
255,0,0),
HMTLprotocol.get_value_msg(69,
1,
255)
]
elif pose == pose_t.fingers_spread:
msgs += [
HMTLprotocol.get_rgb_msg(self.options.hmtladdress,
1,
255,255,255),
HMTLprotocol.get_value_msg(69,
0,
128)
]
if (len(msgs) != 0):
for msg in msgs:
self.client.send_and_ack(msg, False)
print("Sent and acked to client")
def main():
hub = myo.Hub()
hub.run(1000, Listener())
# Listen to keyboard interrupts and stop the
# hub in that case.
try:
while hub.running:
myo.time.sleep(0.2)
except KeyboardInterrupt:
print_("Quitting ...")
hub.stop(True)
def on_pose(self, myo, timestamp, pose):
self.nowpose = pose
print_('on_pose', pose)
## if pose == pose_t.double_tap:
## print_("Enabling EMG")
## myo.set_stream_emg(stream_emg.enabled)
## elif pose == pose_t.fingers_spread:
## myo.set_stream_emg(stream_emg.disabled)
if pose == "fist":
self.fistcount = self.fistcount + 1
if self.fistcount == 1:
self.state = 0
print("Entering state 0")
def main():
hub = myo.Hub()
hub.set_locking_policy(myo.locking_policy.none)
hub.run(1000, Listener())
# Listen to keyboard interrupts and stop the
# hub in that case.
try:
while hub.running:
myo.time.sleep(0.2)
except KeyboardInterrupt:
print_("Quitting ...")
hub.stop(True)
def on_pose(self, myo, timestamp, pose):
self.nowpose=pose
print_('on_pose', pose)
## if pose == pose_t.double_tap:
## print_("Enabling EMG")
## myo.set_stream_emg(stream_emg.enabled)
## elif pose == pose_t.fingers_spread:
## myo.set_stream_emg(stream_emg.disabled)
if pose == "fist":
self.fistcount=self.fistcount+1
if self.fistcount==1:
self.state=0
print("Entering state 0")
def on_pair(self, myo, timestamp):
print_("Paired")
print_(
"If you don't see any responses to your movements, try re-running the program or making sure the Myo works with Myo Connect (from Thalmic Labs)."
)
print_("Double tap enables EMG.")
print_("Spreading fingers disables EMG.\n")
def main():
#address = "192.168.1.110"
address = "localhost"
port = 0
hub = myo.Hub()
hub.run(10000000, Listener(address, port))
# Listen to keyboard interrupts and stop the
# hub in that case.
try:
while hub.running:
myo.time.sleep(0.2)
except KeyboardInterrupt:
print_("Quitting ...")
hub.stop(True)
def on_pair(self, myo, timestamp):
print_('Paired')
print_(
"If you don't see any responses to your movements, try re-running the program or making sure the Myo works with Myo Connect (from Thalmic Labs)."
)
print_("Double tap enables EMG.")
print_("Spreading fingers disables EMG.\n")
def main():
#address = "192.168.1.110"
address = "localhost"
port = 0
hub = myo.Hub()
hub.run(10000000, Listener(address, port))
# Listen to keyboard interrupts and stop the
# hub in that case.
try:
while hub.running:
myo.time.sleep(0.2)
except KeyboardInterrupt:
print_("Quitting ...")
hub.stop(True)
def main():
options = handle_args()
print("%s\nOpening client. Address=%d\n%s" % ("*"*80, options.hmtladdress, "*"*80))
client = HMTLClient(options)
hub = myo.Hub()
hub.set_locking_policy(myo.locking_policy.none)
hub.run(1000, Listener(client, options))
# Listen to keyboard interrupts and stop the
# hub in that case.
try:
while hub.running:
myo.time.sleep(0.2)
except KeyboardInterrupt:
print_("Quitting ...")
hub.stop(True)
def on_pose(self, myo, timestamp, pose):
''' (Listener, Myo, str) -> NoneType
Triggers specific events when specific Myo positions are detected
'''
if is_debug:
print_('on_pose', pose)
global middle
global euler_orientation
if pose == pose_t.double_tap and is_debug:
print_("double_tap")
elif pose == pose_t.fist:
if is_debug:
print_("fist")
self.middle_click()
#When finger_spread is detected with palm facing down
elif (pose == pose_t.fingers_spread) and (euler_orientation[0] <=
arm_boundary):
if middle == True:
self.middle_click()
if is_debug:
print_("Left Click")
self.left_click()
#When finger_spread is detected with palm facing up
elif (pose == pose_t.fingers_spread) and (euler_orientation[0] >
arm_boundary):
if middle == True:
self.middle_click()
if is_debug:
print_("Right Click")
self.right_click()
def on_pose(self, myo, timestamp, pose):
print_('on_pose', pose)
if pose == pose_t.double_tap:
print_("=" * 80)
print_("Reset Orientation")
or1x[0] = orx[0]
or1y[0] = ory[0]
or1z[0] = orz[0]
or1w[0] = orw[0]
#myo.set_stream_emg(stream_emg.enabled)
elif pose == pose_t.fingers_spread:
egg = 0
def opponentChoice(userChoice,myo):
choice=random.randint(1,3)
if choice==1:
print_("Opponent chose Scissors!")
if choice==userChoice:
write_message("Tie!", GAME.me, GAME.opponent)
print_c_scissors(display_width-250, 150)
print_scissors(75, 150)
elif 2==userChoice:
GAME.opponent+=1
write_message("You Lose!", GAME.me, GAME.opponent)
print_c_scissors(display_width-250, 150)
print_paper(75, 150)
myo.vibrate('long')
else:
GAME.me+=1
write_message("You Win!", GAME.me, GAME.opponent)
print_c_scissors(display_width-250, 150)
print_rock(75, 150)
elif choice==2:
print_("Opponent chose Paper!")
if choice==userChoice:
write_message("Tie!", GAME.me, GAME.opponent)
print_c_paper(display_width-250, 150)
print_paper(75, 150)
elif 1==userChoice:
GAME.me+=1
write_message("You Win!", GAME.me, GAME.opponent)
print_c_paper(display_width-250, 150)
print_scissors(75, 150)
else:
GAME.opponent+=1
write_message("You Lose!", GAME.me, GAME.opponent)
print_c_paper(display_width-250, 150)
print_rock(75, 150)
myo.vibrate('long')
else:
print_("Opoonent chose Rock!")
if choice==userChoice:
write_message("Tie!", GAME.me, GAME.opponent)
print_c_rock(display_width-250, 150)
print_rock(75, 150)
elif 2==userChoice:
GAME.me+=1
write_message("You Win!", GAME.me, GAME.opponent)
print_c_rock(display_width-250, 150)
print_paper(75, 150)
else:
GAME.opponent+=1
write_message("You Lose!", GAME.me, GAME.opponent)
print_c_rock(display_width-250, 150)
print_scissors(75, 150)
myo.vibrate('long')
def on_pose(self, myo, timestamp, pose):
if pose == pose_t.wave_in:
#print_('on_pose', pose)
print_("You chose Scissors!")
opponentChoice(1,myo)
myo.set_stream_emg(stream_emg.disabled)
elif pose == pose_t.fingers_spread:
#print_('on_pose', pose)
print_("You chose Paper!")
opponentChoice(2,myo)
myo.set_stream_emg(stream_emg.disabled)
elif pose == pose_t.fist:
#print_('on_pose', pose)
print_("You chose Rock!")
opponentChoice(3,myo)
myo.set_stream_emg(stream_emg.disabled)
def show_output(message, data):
print_(message + ':' + str(data))
def on_pose(self, myo, timestamp, pose):
print_('on_pose', pose)
if pose == pose_t.double_tap:
print_("Enabling EMG")
print_("Spreading fingers disables EMG.")
print_("=" * 80)
myo.set_stream_emg(stream_emg.enabled)
elif pose == pose_t.fingers_spread:
print_("=" * 80)
print_("Disabling EMG")
myo.set_stream_emg(stream_emg.disabled)
def on_unlock(self, myo, timestamp):
print_('unlocked')
def on_disconnect(self, myo, timestamp):
print_('on_disconnect')
def on_rssi(self, myo, timestamp, rssi):
print_("RSSI:", rssi)
def on_sync(self, myo, timestamp):
print_('synced')
def on_rssi(self, myo, timestamp, rssi):
print_("RSSI:", rssi)
def on_lock(self, myo, timestamp):
print_('locked')
def on_sync(self, myo, timestamp):
print_('synced')
def on_connect(self, myo, timestamp):
print_("Connected to Myo")
myo.vibrate('short')
myo.request_rssi()
myo.set_stream_emg(stream_emg.enabled)
print_("Enabling EMG")
def on_orientation_data(self, myo, timestamp, orientation):
self.odr = orientation
if self.state > -1:
if self.counter2 < 100:
output = convert_to_rpy(self.odr)
roll1 = output[0]
pitch1 = output[1]
yaw1 = output[2]
self.rcfull[self.counter2] = math.cos(math.radians(roll1))
self.rsfull[self.counter2] = math.sin(math.radians(roll1))
self.pcfull[self.counter2] = math.cos(math.radians(pitch1))
self.psfull[self.counter2] = math.sin(math.radians(pitch1))
self.ycfull[self.counter2] = math.cos(math.radians(yaw1))
self.ysfull[self.counter2] = math.sin(math.radians(yaw1))
if self.state != 1:
print("orientation")
print_(roll1, pitch1, yaw1)
if (self.state == 2):
rollc = math.cos(math.radians(roll1))
rolls = math.sin(math.radians(roll1))
pitchc = math.cos(math.radians(pitch1))
pitchs = math.sin(math.radians(pitch1))
yawc = math.cos(math.radians(yaw1))
yaws = math.sin(math.radians(yaw1))
rollbc = math.cos(math.radians(self.rollb))
rollbs = math.sin(math.radians(self.rollb))
pitchbc = math.cos(math.radians(self.pitchb))
pitchbs = math.sin(math.radians(self.pitchb))
yawbc = math.cos(math.radians(self.yawb))
yawbs = math.sin(math.radians(self.yawb))
rollf = math.degrees(
math.atan2(rolls - rollbs, rollc - rollbc))
pitchf = math.degrees(
math.atan2(pitchs - pitchbs, pitchc - pitchbc))
yawf = math.degrees(
math.atan2(yaws - yawbs, yawc - yawbc))
self.Odata.append([rollf, pitchf, yawf])
self.counter2 = self.counter2 + 1
if self.counter2 == 100:
rc = numpy.mean(self.rcfull)
pc = numpy.mean(self.pcfull)
yc = numpy.mean(self.ycfull)
rs = numpy.mean(self.rsfull)
ps = numpy.mean(self.psfull)
ys = numpy.mean(self.ysfull)
if self.state == 0: # calibration-
print("Calibration")
self.rollb = math.degrees(math.atan2(rs, rc))
self.pitchb = math.degrees(math.atan2(ps, pc))
self.yawb = math.degrees(math.atan2(ys, yc))
baseline = [self.rollb, self.pitchb, self.yawb]
print("baseline", baseline)
print(
"Calibration Complete! You have 2 seconds to change your gesture and then record a new one for 2"
)
self.counter2 = 0
self.state = 1
self.start_time = time.time()
print("Entering state 1")
elif self.state == 1:
for i in range(1, 2000):
for j in range(1, 10000):
a = i
self.counter2 = 0
self.state = 2
print("Entering state 2")
elif self.state == 2: # data collection
pose_number = assign_pose_number(self.nowpose)
new_number = 5 #input("Please enter the gesture number for the last gesture: ")
print("Accel", self.Adata, len(self.Adata))
print("Orient", self.Odata, len(self.Odata))
print("EMG", self.Edata, len(self.Edata))
if (len(self.Edata) == 400):
fo = open("trainingdata200.csv", "ab")
iOA = 0
run = 0
## for point in self.Odata:
## fo.write('%s, %f,%f, %f, %f\n' % (point[0], point[1], point[2], point[3], new_number))
##
## for point in self.Adata:
## fo.write("%s, %f, %f, %f, %f\n" %(point[0], point[1][0], point[1][1], point[1][2], new_number))
for point in self.Edata:
fo.write(
'%f,%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n'
%
(self.Odata[iOA][0], self.Odata[iOA][1],
self.Odata[iOA][2], self.Adata[iOA][0],
self.Adata[iOA][1], self.Adata[iOA][2], point[0],
point[1], point[2], point[3], point[4], point[5],
point[6], point[7], new_number))
## fo.write("%s, %f, %f, %f, %f\n" %(self.Adata[iOA][0], self.Adata[iOA][1], self.Adata[iOA][2], new_number))
## fo.write("%s, %f, %f, %f, %f, %f, %f, %f, %f, %f\n" %(point[0], point[1], point[2], point[3], point[4],point[5], point[6],point[7], new_number))
run += 1
if (run == 4):
run = 0
iOA += 1
fo.close()
f50 = open("trainingdata50.csv", "ab")
EI = 3
for i in xrange(100):
f50.write(
'%f,%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n'
%
(self.Odata[i][0], self.Odata[i][1],
self.Odata[i][2], self.Adata[i][0],
self.Adata[i][1], self.Adata[i][2],
self.Edata[EI][0], self.Edata[EI][1],
self.Edata[EI][2], self.Edata[EI][3],
self.Edata[EI][4], self.Edata[EI][5],
self.Edata[EI][6], self.Edata[EI][7], new_number))
EI += 4
f50.close()
print("Saved!")
else:
print("Not Saved.")
self.counter2 = 0
cont = raw_input("Press c to record another")
if (cont == "c"):
self.Odata = []
self.Adata = []
self.Edata = []
self.state = 2
else:
self.state = -1
def on_unsync(self, myo, timestamp):
myo.set_stream_emg(stream_emg.enabled)
print_('unsynced')
def on_orientation_data(self, myo, timestamp, orientation):
self.odr=orientation
if self.state==-1:
calib_start=input('Press 1 for calibration: ')
self.state=0
print("Beginning Calibration!!!")
if self.counter2<50:
output=convert_to_rpy(self.odr)
roll1=output[0]
pitch1=output[1]
yaw1=output[2]
#print_(roll1,pitch1,yaw1)
self.rcfull[self.counter2] = math.cos(math.radians(roll1))
self.rsfull[self.counter2] = math.sin(math.radians(roll1))
self.pcfull[self.counter2] = math.cos(math.radians(pitch1))
self.psfull[self.counter2] = math.sin(math.radians(pitch1))
self.ycfull[self.counter2] = math.cos(math.radians(yaw1))
self.ysfull[self.counter2] = math.sin(math.radians(yaw1))
self.counter2=self.counter2+1
if self.counter2 == 50:
rc = numpy.mean(self.rcfull)
pc = numpy.mean(self.pcfull)
yc = numpy.mean(self.ycfull)
rs = numpy.mean(self.rsfull)
ps = numpy.mean(self.psfull)
ys = numpy.mean(self.ysfull)
if self.state==0: # calibration
self.rollb = math.degrees(math.atan2(rs,rc))
self.pitchb = math.degrees(math.atan2(ps,pc))
self.yawb = math.degrees(math.atan2(ys,yc))
baseline=[self.rollb,self.pitchb,self.yawb]
print(baseline)
print("Calibration Complete! You have 2 seconds to change your gesture and then record a new one for 2")
self.counter2=0
self.state=1
self.start_time = time.time()
elif self.state == 1:
#for i in range(1,1000):
# for j in range(1,10000):
# a=i
time.sleep(2)
self.counter2 = 0
self.state = 2
elif self.state == 2: # data collection, identification and transfer
self.roll = math.degrees(math.atan2(rs,rc))-self.rollb
self.pitch = math.degrees(math.atan2(ps,pc))-self.pitchb
self.yaw = math.degrees(math.atan2(ys,yc))-self.yawb
a=numpy.matrix([self.roll,self.pitch,self.yaw])
b=a.transpose()
d=numpy.matlib.repmat(b,1,4) #hardcoded------CHANGE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
pose_number=assign_pose_number(self.nowpose)
self.W=numpy.matrix([[-2.38946624937314,172.907592400722,-12.1921835866013,30.7345074055692],[-1.60615657009811, 70.7802307763561,-10.0331866141660,-71.1455632604777],[5.87052357307342,52.4896992580217,-45.3268722289868,-67.7618396439250]])
diff=self.W-d
squared=numpy.square(diff)
summed=sum(squared)
#rooted=numpy.sqrt(summed)
#rooted=rooted.tolist()
rooted=summed.tolist()
print("rooted", rooted)
P = min(rooted[0])
#print ("P ", P)
Q = rooted[0].index(P)
#print ("Q", Q)
R = Q + 1
#print("R" , R)
#indx=rooted.index(min(rooted))+1#1-front; 2-up; 3-side; 4-down
indx=R
print ("indx",indx)
print ("pose num ", pose_number)
if indx==1:#front
if pose_number==1:
gesture_number=1
gesture_name='FRONT_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 1\n", ("<broadcast>", 10000))
elif pose_number==3:
gesture_number=2
gesture_name='FRONT_fist'
print_(gesture_name)
self._sock.sendto("Gesture: 3\n", ("<broadcast>", 10000))
elif pose_number==4:
gesture_number=3
gesture_name='FRONT_wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 4\n", ("<broadcast>", 10000))
elif pose_number==5:
gesture_number=4
gesture_name='FRONT_wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 5\n", ("<broadcast>", 10000))
elif indx==2:#top
if pose_number==1:
gesture_number=5
gesture_name='UP_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 101\n", ("<broadcast>", 10000))
elif pose_number==3:
gesture_number=6
gesture_name='UP_fist'
print_(gesture_name)
self._sock.sendto("Gesture: 103\n", ("<broadcast>", 10000))
elif pose_number==4:
gesture_number=7
gesture_name='UP_wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 104\n", ("<broadcast>", 10000))
elif pose_number==5:
gesture_number=8
gesture_name='UP_wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 105\n", ("<broadcast>", 10000))
elif indx==3:#side
if pose_number==1:
gesture_number=9
gesture_name='SIDE_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 201\n", ("<broadcast>", 10000))
elif pose_number==3:
gesture_number=10
gesture_name='SIDE__fist'
print_(gesture_name)
self._sock.sendto("Gesture: 203\n", ("<broadcast>", 10000))
elif pose_number==4:
gesture_number=11
gesture_name='SIDE__wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 204\n", ("<broadcast>", 10000))
elif pose_number==5:
gesture_number=12
gesture_name='SIDE__wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 205\n", ("<broadcast>", 10000))
elif indx==4:#down
if pose_number==1:
gesture_number=13
gesture_name='DOWN_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 301\n", ("<broadcast>", 10000))
elif pose_number==3:
gesture_number=14
gesture_name='DOWN__fist'
print_(gesture_name)
self._sock.sendto("Gesture: 303\n", ("<broadcast>", 10000))
elif pose_number==4:
gesture_number=15
gesture_name='DOWN__wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 304\n", ("<broadcast>", 10000))
elif pose_number==5:
gesture_number=16
gesture_name='DOWN__wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 305\n", ("<broadcast>", 10000))
#print(pose_number,self.roll,self.pitch,self.yaw)
self.counter2=0
self.state=2
def on_unsync(self, myo, timestamp):
print_('unsynced')
def on_lock(self, myo, timestamp):
if is_debug:
print_('locked')
def on_orientation_data(self, myo, timestamp, orientation):
self.odr=orientation
if self.state>-1:
if self.counter2<100:
output=convert_to_rpy(self.odr)
roll1=output[0]
pitch1=output[1]
yaw1=output[2]
self.rcfull[self.counter2] = math.cos(math.radians(roll1))
self.rsfull[self.counter2] = math.sin(math.radians(roll1))
self.pcfull[self.counter2] = math.cos(math.radians(pitch1))
self.psfull[self.counter2] = math.sin(math.radians(pitch1))
self.ycfull[self.counter2] = math.cos(math.radians(yaw1))
self.ysfull[self.counter2] = math.sin(math.radians(yaw1))
if self.state!=1:
print ("orientation")
print_(roll1,pitch1,yaw1)
if (self.state == 2):
rollc = math.cos(math.radians(roll1))
rolls = math.sin(math.radians(roll1))
pitchc = math.cos(math.radians(pitch1))
pitchs = math.sin(math.radians(pitch1))
yawc = math.cos(math.radians(yaw1))
yaws = math.sin(math.radians(yaw1))
rollbc = math.cos(math.radians(self.rollb))
rollbs = math.sin(math.radians(self.rollb))
pitchbc = math.cos(math.radians(self.pitchb))
pitchbs = math.sin(math.radians(self.pitchb))
yawbc = math.cos(math.radians(self.yawb))
yawbs = math.sin(math.radians(self.yawb))
rollf = math.degrees(math.atan2(rolls-rollbs,rollc-rollbc))
pitchf = math.degrees(math.atan2(pitchs-pitchbs,pitchc-pitchbc))
yawf = math.degrees(math.atan2(yaws-yawbs,yawc-yawbc))
self.Odata.append([rollf, pitchf, yawf])
self.counter2=self.counter2+1
if self.counter2 == 100:
rc = numpy.mean(self.rcfull)
pc = numpy.mean(self.pcfull)
yc = numpy.mean(self.ycfull)
rs = numpy.mean(self.rsfull)
ps = numpy.mean(self.psfull)
ys = numpy.mean(self.ysfull)
if self.state==0: # calibration-
print ("Calibration")
self.rollb = math.degrees(math.atan2(rs,rc))
self.pitchb = math.degrees(math.atan2(ps,pc))
self.yawb = math.degrees(math.atan2(ys,yc))
baseline=[self.rollb,self.pitchb,self.yawb]
print("baseline", baseline)
print("Calibration Complete! You have 2 seconds to change your gesture and then record a new one for 2")
self.counter2=0
self.state=1
self.start_time = time.time()
print("Entering state 1")
elif self.state == 1:
for i in range(1,2000):
for j in range(1,10000):
a=i
self.counter2=0
self.state = 2
print("Entering state 2")
elif self.state == 2: # data collection
pose_number=assign_pose_number(self.nowpose)
new_number=5#input("Please enter the gesture number for the last gesture: ")
print ("Accel", self.Adata, len(self.Adata))
print ("Orient", self.Odata, len(self.Odata))
print ("EMG", self.Edata, len(self.Edata))
if (len(self.Edata) == 400):
fo = open("trainingdata200.csv","ab")
iOA = 0
run = 0
## for point in self.Odata:
## fo.write('%s, %f,%f, %f, %f\n' % (point[0], point[1], point[2], point[3], new_number))
##
## for point in self.Adata:
## fo.write("%s, %f, %f, %f, %f\n" %(point[0], point[1][0], point[1][1], point[1][2], new_number))
for point in self.Edata:
fo.write('%f,%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n' % (self.Odata[iOA][0], self.Odata[iOA][1], self.Odata[iOA][2], self.Adata[iOA][0], self.Adata[iOA][1],
self.Adata[iOA][2], point[0], point[1], point[2], point[3], point[4],point[5], point[6],point[7], new_number))
## fo.write("%s, %f, %f, %f, %f\n" %(self.Adata[iOA][0], self.Adata[iOA][1], self.Adata[iOA][2], new_number))
## fo.write("%s, %f, %f, %f, %f, %f, %f, %f, %f, %f\n" %(point[0], point[1], point[2], point[3], point[4],point[5], point[6],point[7], new_number))
run+=1
if (run == 4):
run=0
iOA+=1
fo.close()
f50 = open("trainingdata50.csv","ab")
EI = 3
for i in xrange(100):
f50.write('%f,%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n' % (self.Odata[i][0], self.Odata[i][1], self.Odata[i][2], self.Adata[i][0], self.Adata[i][1],
self.Adata[i][2], self.Edata[EI][0], self.Edata[EI][1], self.Edata[EI][2], self.Edata[EI][3],
self.Edata[EI][4],self.Edata[EI][5], self.Edata[EI][6],self.Edata[EI][7], new_number))
EI+=4
f50.close()
print("Saved!")
else:
print ("Not Saved.")
self.counter2=0
cont = raw_input("Press c to record another")
if (cont == "c"):
self.Odata = []
self.Adata = []
self.Edata = []
self.state = 2
else:
self.state=-1
def on_pair(self, myo, timestamp):
print_('on_pair')
def on_sync(self, myo, timestamp, arm, x_direction):
print_('synced', arm, x_direction)
def on_connect(self, myo, timestamp):
print_("Connected to Myo")
myo.vibrate('short')
myo.request_rssi()
def show_output(message, data):
if random.random() < SHOW_OUTPUT_CHANCE:
print_(message + ':' + str(data))
def telemetry(sid, data):
global pose_status
try:
# myo.time.sleep(0.2)
print(str(pose_now))
if 'rest' in pose_now:
pose_status = 0
elif 'in' in pose_now:
print(1)
pose_status = 1
elif 'out' in pose_now:
print(2)
pose_status = 2
elif 'fist' in pose_now:
pose_status = 3
elif 'double' in pose_now:
pose_status = 4
elif 'fingers_spread' in pose_now:
pose_status = 5
# hub.
except KeyboardInterrupt:
print_("Quitting ...")
hub.stop(True)
if AutoMode == True:
try:
if pose_status == 1:
MAX_SPEED += 1
MIN_SPEED += 1
time.sleep(0.1)
# print('Speeding up 1 mph, current speed:',speed_limit)
elif pose_status == 2:
MAX_SPEED -= 1
MIN_SPEED -= 1
time.sleep(0.1)
# print('Reducing down 1 mph, current speed:',speed_limit)
elif pose_status == 3:
# if MAX_SPEED == 30 and MIN_SPEED == 25:
MAX_SPEED = 30
MIN_SPEED = 25
print('Max Speeding On')
time.sleep(0.1)
elif pose_status == 5:
AutoMode = False
print('AutoMode:', AutoMode)
time.sleep(0.1)
except:
pass
global MAX_SPEED
global MIN_SPEED
global AutoMode
speed_limit = MAX_SPEED
if data:
# The current steering angle of the car
steering_angle = float(data["steering_angle"])
# The current throttle of the car
throttle = float(data["throttle"])
# The current speed of the car
speed = float(data["speed"])
# The current image from the center camera of the car
image = Image.open(BytesIO(base64.b64decode(data["image"])))
# save frame
# if args.image_folder != '':
# timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3]
# image_filename = os.path.join(args.image_folder, timestamp)
# image.save('{}.jpg'.format(image_filename))
try:
image = np.asarray(image) # from PIL image to numpy array
# cv.imshow('display', image)
# plt.imshow(image)
# cv.ShowImage(win_name, image)("Cameras", image)
image = utils.preprocess(image) # apply the preprocessing
image = np.array([image]) # the model expects 4D array
# predict the steering angle for the image
steering_angle = float(model.predict(image, batch_size=1))
# PID control here
if speed > speed_limit:
speed_limit = MIN_SPEED # slow down
else:
speed_limit = MAX_SPEED
throttle = 1.0 - steering_angle**2 - (speed / speed_limit)**2
# print('{} {} {}'.format(steering_angle, throttle, speed))
send_control(steering_angle, throttle)
# send_control(2, 2)
except Exception as e:
print(e)
else:
MAX_SPEED = 25
MIN_SPEED = 10
try:
if pose_status == 0:
steering_angle = 0
time.sleep(0.1)
if pose_status == 1:
steering_angle = -0.3
time.sleep(0.1)
elif pose_status == 2:
steering_angle = 0.3
time.sleep(0.1)
elif pose_status == 3:
print('Max Speeding On')
time.sleep(0.1)
elif pose_status == 5:
AutoMode = True
print('AutoMode:', AutoMode)
time.sleep(0.1)
except:
pass
if data:
speed = float(data["speed"])
try:
global steering_angle
# PID control here
if speed > speed_limit:
speed_limit = MIN_SPEED # slow down
else:
speed_limit = MAX_SPEED
throttle = 1.0 - steering_angle**2 - (speed /
speed_limit)**2
# print('{} {} {}'.format(steering_angle, throttle, speed))
send_control(steering_angle, throttle)
# send_control(2, 2)
except Exception as e:
print(e)
def on_connect(self, myo, timestamp):
print_("Connected to Myo")
myo.vibrate('short')
myo.request_rssi()
def on_pose(self, myo, timestamp, pose):
print_('on_pose', pose)
if pose == pose_t.double_tap:
print_("Enabling EMG")
print_("Spreading fingers disables EMG.")
print_("=" * 80)
myo.set_stream_emg(stream_emg.enabled)
elif pose == pose_t.fingers_spread:
print_("=" * 80)
print_("Disabling EMG")
myo.set_stream_emg(stream_emg.disabled)
def on_pose(self, myo, timestamp, pose):
print_('on_pose', pose)
def on_lock(self, myo, timestamp):
print_('locked')
def on_pair(self, myo, timestamp):
print_('Paired')
print_("If you don't see any responses to your movements, try re-running the program or making sure the Myo works with Myo Connect (from Thalmic Labs).")
def on_unsync(self, myo, timestamp):
print_('unsynced')
def on_orientation_data(self, myo, timestamp, orientation):
self.odr = orientation
if self.state == -1:
calib_start = input('Press 1 for calibration: ')
self.state = 0
print("Beginning Calibration!!!")
if self.counter2 < 50:
output = convert_to_rpy(self.odr)
roll1 = output[0]
pitch1 = output[1]
yaw1 = output[2]
#print_(roll1,pitch1,yaw1)
self.rcfull[self.counter2] = math.cos(math.radians(roll1))
self.rsfull[self.counter2] = math.sin(math.radians(roll1))
self.pcfull[self.counter2] = math.cos(math.radians(pitch1))
self.psfull[self.counter2] = math.sin(math.radians(pitch1))
self.ycfull[self.counter2] = math.cos(math.radians(yaw1))
self.ysfull[self.counter2] = math.sin(math.radians(yaw1))
self.counter2 = self.counter2 + 1
if self.counter2 == 50:
rc = numpy.mean(self.rcfull)
pc = numpy.mean(self.pcfull)
yc = numpy.mean(self.ycfull)
rs = numpy.mean(self.rsfull)
ps = numpy.mean(self.psfull)
ys = numpy.mean(self.ysfull)
if self.state == 0: # calibration
self.rollb = math.degrees(math.atan2(rs, rc))
self.pitchb = math.degrees(math.atan2(ps, pc))
self.yawb = math.degrees(math.atan2(ys, yc))
baseline = [self.rollb, self.pitchb, self.yawb]
print(baseline)
print(
"Calibration Complete! You have 2 seconds to change your gesture and then record a new one for 2"
)
self.counter2 = 0
self.state = 1
self.start_time = time.time()
elif self.state == 1:
#for i in range(1,1000):
# for j in range(1,10000):
# a=i
time.sleep(2)
self.counter2 = 0
self.state = 2
elif self.state == 2: # data collection, identification and transfer
self.roll = math.degrees(math.atan2(rs, rc)) - self.rollb
self.pitch = math.degrees(math.atan2(ps, pc)) - self.pitchb
self.yaw = math.degrees(math.atan2(ys, yc)) - self.yawb
a = numpy.matrix([self.roll, self.pitch, self.yaw])
b = a.transpose()
d = numpy.matlib.repmat(
b, 1, 4
) #hardcoded------CHANGE LATER!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
pose_number = assign_pose_number(self.nowpose)
self.W = numpy.matrix([[
-2.38946624937314, 172.907592400722, -12.1921835866013,
30.7345074055692
],
[
-1.60615657009811, 70.7802307763561,
-10.0331866141660, -71.1455632604777
],
[
5.87052357307342, 52.4896992580217,
-45.3268722289868, -67.7618396439250
]])
diff = self.W - d
squared = numpy.square(diff)
summed = sum(squared)
#rooted=numpy.sqrt(summed)
#rooted=rooted.tolist()
rooted = summed.tolist()
print("rooted", rooted)
P = min(rooted[0])
#print ("P ", P)
Q = rooted[0].index(P)
#print ("Q", Q)
R = Q + 1
#print("R" , R)
#indx=rooted.index(min(rooted))+1#1-front; 2-up; 3-side; 4-down
indx = R
print("indx", indx)
print("pose num ", pose_number)
if indx == 1: #front
if pose_number == 1:
gesture_number = 1
gesture_name = 'FRONT_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 1\n",
("<broadcast>", 10000))
elif pose_number == 3:
gesture_number = 2
gesture_name = 'FRONT_fist'
print_(gesture_name)
self._sock.sendto("Gesture: 3\n",
("<broadcast>", 10000))
elif pose_number == 4:
gesture_number = 3
gesture_name = 'FRONT_wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 4\n",
("<broadcast>", 10000))
elif pose_number == 5:
gesture_number = 4
gesture_name = 'FRONT_wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 5\n",
("<broadcast>", 10000))
elif indx == 2: #top
if pose_number == 1:
gesture_number = 5
gesture_name = 'UP_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 101\n",
("<broadcast>", 10000))
elif pose_number == 3:
gesture_number = 6
gesture_name = 'UP_fist'
print_(gesture_name)
self._sock.sendto("Gesture: 103\n",
("<broadcast>", 10000))
elif pose_number == 4:
gesture_number = 7
gesture_name = 'UP_wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 104\n",
("<broadcast>", 10000))
elif pose_number == 5:
gesture_number = 8
gesture_name = 'UP_wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 105\n",
("<broadcast>", 10000))
elif indx == 3: #side
if pose_number == 1:
gesture_number = 9
gesture_name = 'SIDE_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 201\n",
("<broadcast>", 10000))
elif pose_number == 3:
gesture_number = 10
gesture_name = 'SIDE__fist'
print_(gesture_name)
self._sock.sendto("Gesture: 203\n",
("<broadcast>", 10000))
elif pose_number == 4:
gesture_number = 11
gesture_name = 'SIDE__wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 204\n",
("<broadcast>", 10000))
elif pose_number == 5:
gesture_number = 12
gesture_name = 'SIDE__wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 205\n",
("<broadcast>", 10000))
elif indx == 4: #down
if pose_number == 1:
gesture_number = 13
gesture_name = 'DOWN_finger_spread'
print_(gesture_name)
self._sock.sendto("Gesture: 301\n",
("<broadcast>", 10000))
elif pose_number == 3:
gesture_number = 14
gesture_name = 'DOWN__fist'
print_(gesture_name)
self._sock.sendto("Gesture: 303\n",
("<broadcast>", 10000))
elif pose_number == 4:
gesture_number = 15
gesture_name = 'DOWN__wave_in'
print_(gesture_name)
self._sock.sendto("Gesture: 304\n",
("<broadcast>", 10000))
elif pose_number == 5:
gesture_number = 16
gesture_name = 'DOWN__wave_out'
print_(gesture_name)
self._sock.sendto("Gesture: 305\n",
("<broadcast>", 10000))
#print(pose_number,self.roll,self.pitch,self.yaw)
self.counter2 = 0
self.state = 2
def on_connect(self, myo, timestamp):
print_("Connected to Myo")
myo.vibrate('short')
myo.request_rssi()
myo.set_stream_emg(stream_emg.enabled)
print_("Enabling EMG")
def on_pair(self, myo, timestamp):
print_('on_pair')
def on_disconnect(self, myo, timestamp):
print_('on_disconnect')
def on_pose(self, myo, timestamp, pose):
if pose == pose_t.fist:
print_('on_pose', pose)
fire()
def on_unlock(self, myo, timestamp):
print_('unlocked')
def on_rssi(self, myo, timestamp, rssi):
''' (Listener, Myo, str, rssi) -> NoneType
Displays RSSI state
'''
if is_debug:
print_("RSSI:", rssi)
def on_sync(self, myo, timestamp, arm, x_direction):
print_('synced', arm, x_direction)
def on_lock(self, myo, timestamp):
print_("locked")
def show_output(message, data):
if random.random() < SHOW_OUTPUT_CHANCE:
print_(message + ':' + str(data))
def on_sync(self, myo, timestamp):
print_("synced")
