|
Posted by Highway on October 7, 2006, 9:26 am
Please log in for more thread options
Hi,
I am a newbie, and found something interesting by using Bluetooth
direct command to control the NXT.
A program Anders made in
http://www.norgesgade14.dk/legoSider/mindstorm_en.html really inspired
me.
So I wrote a motor tester in python first. Finally the script became a
NXT BT controller.
The usage is written in the script.
Have fun!
Regards,
Haiwei
BTW, I am confused by a property named turn_ratio. This property didn't
behaviour as the spec. Maybe I don't understand the meaning this
property.
-----------
btc1.py
-----------
__author__ = 'Haiwei'
'''
Require:
1. a PC with blue tooth and a NXT of course
2. python 2.4 with tk
3. python for windows extension in
http://sourceforge.net/projects/pywin32/
4. pytone serial in http://sourceforge.net/projects/pyserial/
Prepare:
1. connect right motor in port B, and left motor in port C
2. turn on the NXT bluetooth and PC bluetooth, and make a BT connection
between NXT and PC
Usage:
1. run this script
2. press connect button to make the serial port work, the default is
COM9
(make sure the right port, or you can get nothing)
3. press direction key like up down left right to control NXT
or
4. get status by pressing the status button
Note:
This script only tested in i386 PC(litte endian). If you want to run
this script in other platform, pay attention on big-endian issue.
Have fun!
hack this as you like, please send your version or comments to
highfly22 at gmail dot com
'''
import sys
import os
import serial
import struct
import time
from Tkinter import *
import Tix
from Tkconstants import *
class Cmddlg:
run_state_vec = [0x0, 0x10, 0x20, 0x40]
def __init__(self, parent=NONE):
self.power = 0
self.turn = 0
self.right = 1
self.left = 2
self.output = open('./out', "wb")
frame = Frame(parent)
self.frame = frame
parent.bind("<Key>", self.controller)
frame.pack()
Label(frame, text="Port").grid(row=0, column=0)
self.port = Listbox(frame, height=3, selectmode=SINGLE,
exportselection=0)
for item in ["A", "B", "C"]:
self.port.insert(END, item)
self.port.select_set(1)
self.port.grid(row=1, column=0, sticky=N)
Label(frame, text="Speed").grid(row=0, column=1)
self.speed = Entry(frame)
self.speed.insert(END, "50")
self.speed.grid(row=1, column=1, sticky=N)
Label(frame, text="mode").grid(row=0, column=2)
self.mode = Listbox(frame, height=3, selectmode=MULTIPLE,
exportselection=0)
for item in ["motoron", "brake", "regulated"]:
self.mode.insert(END, item)
self.mode.select_set(0)
self.mode.grid(row=1, column=2, sticky=N)
Label(frame, text="reg_mode").grid(row=0, column=3)
self.reg_mode = Listbox(frame, height=3, selectmode=SINGLE,
exportselection=0)
for item in ["regulate_mode_idle", "regulate_mode_speed",
"regulate_mode_sync"]:
self.reg_mode.insert(END, item)
self.reg_mode.select_set(0)
self.reg_mode.grid(row=1, column=3, sticky=N)
Label(frame, text="turn_ratio").grid(row=0, column=4)
self.turn_ratio = Entry(frame)
self.turn_ratio.insert(END, "0")
self.turn_ratio.grid(row=1, column=4, sticky=N)
Label(frame, text="run_state").grid(row=0, column=5)
self.run_state = Listbox(frame, height=4, selectmode=SINGLE,
exportselection=0)
for item in ["run_state_idle", "run_state_rampup" ,
"run_state_running", "run_state_rampdown"]:
self.run_state.insert(END, item)
self.run_state.select_set(2)
self.run_state.grid(row=1, column=5, sticky=N)
Label(frame, text="tach_limt").grid(row=0, column=6)
self.tach_limt = Entry(frame)
self.tach_limt.insert(END, "0")
self.tach_limt.grid(row=1, column=6, sticky=N)
Button(frame, text="Msg", command=self.msg).grid(row=2,
column=2)
Button(frame, text="Status",
command=self.getstatus).grid(row=2, column=3)
Button(frame, text="Connect", command=self.connect).grid(row=2,
column=4)
Button(frame, text="Close", command=self.close).grid(row=2,
column=5)
Button(frame, text="Stop", command=self.stop).grid(row=2,
column=6)
# Label(frame, text="istest").grid(row=2, column=0)
self.istest = IntVar()
self.cb = Checkbutton(frame, variable=self.istest,
command=self.cbcmd)
self.cb.grid(row=2, column=1)
def cbcmd(self):
print self.istest.get()
def connect(self, comport=9):
try:
if not self.istest.get():
self.ser = serial.Serial('COM%d' % comport, timeout=10)
print "ser connected"
except:
print "error open serial port %d" %(comport)
print "Unexpected error:", sys.exc_info()[0]
sys.exit()
def add(self):
print self.mode.curselection()
print self.reg_mode.curselection()
print self.run_state.curselection()
def msg(self):
if not self.istest.get():
self.ser.write(struct.pack("8l", 0, 0, 0, 0, 0, 0, 0, 0))
port = int(self.port.curselection()[0])
speed = int(self.speed.get())
mode = 0
for item in self.mode.curselection():
mode = mode | (1 << (int(item)))
reg_mode = int(self.reg_mode.curselection()[0])
turn_ratio = int(self.turn_ratio.get())
run_state = self.run_state_vec[
int(self.run_state.curselection()[0]) ]
tach_limt = int(self.tach_limt.get())
self.output.write( struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
port, speed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
self.output.write("AAAA")
if not self.istest.get():
self.ser.write( struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
port, speed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
red = self.ser.read(5)
print struct.unpack("=5B", red)
def getstatus(self):
if not self.istest.get():
self.ser.write(struct.pack("4l", 0, 0, 0, 0))
self.ser.write( struct.pack("5B", 3, 0, 0x00, 0x06,
self.left) )
red1 = self.ser.read(27)
self.ser.write( struct.pack("5B", 3, 0, 0x00, 0x06,
self.right) )
red2 = self.ser.read(27)
print "len1 %d len2 %d" %(len(red1), len(red2))
print "len0 len1 b0 b1 status port power mode reg turn run
talt tact btact roct"
print struct.unpack("=6Bb2BbBi3I", red1)
print struct.unpack("=6Bb2BbBi3I", red2)
def stop(self):
if not self.istest.get():
speed = 0
mode = 0
reg_mode = 0
turn_ratio = 0
run_state = 0
tach_limt = 0
self.ser.write(struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
self.left, speed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
red = self.ser.read(5)
print struct.unpack("=5B", red)
self.ser.write(struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
self.right, speed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
red = self.ser.read(5)
print struct.unpack("=5B", red)
def close(self):
self.output.close()
if not self.istest.get():
self.ser.close()
def controller(self, event):
if event.keysym_num == 65362:
if self.power < 50 and self.power > -50:
self.power = 45
else:
self.power += 5
if self.power > 100:
self.power = 100
print "KP_Up %d %d" %( self.power, self.turn)
elif event.keysym_num == 65364:
if self.power > -50 and self.power < 50:
self.power = -45
else:
self.power -= 5
if self.power < -100:
self.power = -100
print "KP_Down %d %d" %( self.power, self.turn)
elif event.keysym_num == 65361:
self.turn -= 3
if self.turn < -100:
self.turn = -100
if self.turn > 0:
self.turn = 0
print "KP_Left %d %d" %( self.power, self.turn)
elif event.keysym_num == 65363:
self.turn += 3
if self.turn > 100:
self.turn = 100
if self.turn < 0:
self.turn = 0
print "KP_Right %d %d" %( self.power, self.turn)
else:
return
if not self.istest.get():
self.ser.write(struct.pack("8l", 0, 0, 0, 0, 0, 0, 0, 0))
lspeed = self.power + self.turn
if lspeed > 100:
lspeed = 100
if lspeed < -100:
lspeed = -100
rspeed = self.power - self.turn
if rspeed > 100:
rspeed = 100
if rspeed < -100:
rspeed = -100
mode = 0
for item in self.mode.curselection():
mode = mode | (1 << (int(item)))
reg_mode = int(self.reg_mode.curselection()[0])
turn_ratio = int(self.turn_ratio.get())
run_state = self.run_state_vec[
int(self.run_state.curselection()[0]) ]
tach_limt = int(self.tach_limt.get())
self.output.write( struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
self.left, lspeed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
self.output.write("AAAA")
self.output.write( struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
self.right, rspeed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
self.output.write("AAAA")
if not self.istest.get():
self.ser.write( struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
self.left, lspeed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
red = self.ser.read(5)
print struct.unpack("=5B", red)
self.ser.write( struct.pack("=5BbBBbBI", 12, 0, 0x00, 0x04,
self.right, rspeed,
mode, reg_mode, turn_ratio,
run_state, tach_limt))
red = self.ser.read(5)
print struct.unpack("=5B", red)
if __name__ == '__main__':
root = Tix.Tk()
dlg = Cmddlg(root)
root.title("NXT direct command")
root.mainloop()
dlg.close()
|
XML site map