Go to the documentation of this file.00001
00002
00003
00004
00005 import nxt.locator
00006 from nxt.sensor import *
00007 from nxt.motor import *
00008 import time
00009
00010 class NXTBrick:
00011 def __init__(self, bsock=None):
00012 """
00013 Ctor
00014 Connecting to NXT brick and creating motor object, sensor object
00015 and so on. Motor encoders will be reset.
00016 """
00017 if bsock:
00018 self.sock = bsock
00019 else:
00020 self.sock = nxt.locator.find_one_brick().connect()
00021
00022 self.motors = [Motor(self.sock, PORT_A),
00023 Motor(self.sock, PORT_B),
00024 Motor(self.sock, PORT_C)]
00025
00026 self.sensors = [TouchSensor(self.sock, PORT_1),
00027 SoundSensor(self.sock, PORT_2),
00028 LightSensor(self.sock, PORT_3),
00029 UltrasonicSensor(self.sock, PORT_4)]
00030 self.resetPosition()
00031
00032 def close(self):
00033 """
00034 Finalizing connection with NXT brick.
00035 """
00036 self.sock.close()
00037
00038 def resetPosition(self, relative = 0):
00039 """
00040 Resetting encoders of NXT motors
00041 """
00042 for m in self.motors:
00043 m.reset_position(relative)
00044
00045 def setMotors(self, vels):
00046 """
00047 This operation receives array and set them as motor power. If the
00048 number of vels items does not match with the number of motors,
00049 smaller number of them will be taken and set respectively.
00050 """
00051 for i, v in enumerate(vels[:min(len(vels),len(self.motors))]):
00052 self.motors[i].power = max(min(v,127),-127)
00053 self.motors[i].mode = MODE_MOTOR_ON | MODE_REGULATED
00054 self.motors[i].regulation_mode = REGULATION_MOTOR_SYNC
00055 self.motors[i].run_state = RUN_STATE_RUNNING
00056 self.motors[i].tacho_limit = 0
00057 self.motors[i].set_output_state()
00058
00059 def getMotors(self):
00060 """
00061 Getting motors' angle (degrees)
00062 """
00063 state = []
00064 for m in self.motors:
00065 stat = None
00066 for i in range(3):
00067 try:
00068 stat = m.get_output_state()
00069 break
00070 except:
00071 time.sleep(0.01)
00072 continue
00073
00074 if stat == None:
00075 import sys
00076 print "Unknown motor encoder error"
00077 print sys.exc_info()[1]
00078 state.append(stat)
00079
00080 return state
00081
00082
00083 def getSensors(self):
00084 """
00085 Getting sensors' values. Data will be returned as array.
00086 """
00087 state = []
00088 for s in self.sensors:
00089 stat = None
00090 for i in range(3):
00091 try:
00092 stat = s.get_sample()
00093 break
00094 except:
00095 time.sleep(0.01)
00096 continue
00097 if stat == None:
00098 import sys
00099 print "Unknown sensor error"
00100 print sys.exc_info()[1]
00101 state.append(stat)
00102
00103 return state
00104
00105
00106 """
00107 Test program
00108 It gives appropriate values to motors, and angles of motors are
00109 obtained and shown. Sensor data are also obtained and shown.
00110 """
00111 if __name__ == "__main__":
00112 import time
00113 nxt = NXTBrick()
00114 print "connected"
00115
00116
00117 for i in range(0):
00118 nxt.setMotors([80,-80,80])
00119 print "Motor: "
00120 mstat = nxt.getMotors()
00121 for i, m in enumerate(mstat):
00122 print "(" , i, "): ", m
00123 time.sleep(0.1)
00124 nxt.setMotors([0,0,0])
00125
00126
00127 for i in range(100):
00128 sensors = ["Touch", "Sound", "Light", "USonic"]
00129 sval = nxt.getSensors()
00130 print sval
00131 time.sleep(0.1)