NXTBrick20.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 # -*- Python -*-
4 
5 import nxt.locator
6 from nxt.sensor import *
7 from nxt.motor import *
8 import time
9 
10 class NXTBrick:
11  def __init__(self, bsock=None):
12  """
13  Ctor
14  Connecting to NXT brick and creating motor object, sensor object
15  and so on. Motor encoders will be reset.
16  """
17  if bsock:
18  self.sock = bsock
19  else:
20  self.sock = nxt.locator.find_one_brick()
21 
22  self.motors = [Motor(self.sock, PORT_A),
23  Motor(self.sock, PORT_B),
24  Motor(self.sock, PORT_C)]
25 
26  self.sensors = [Touch(self.sock, PORT_1),
27  Sound(self.sock, PORT_2),
28  Light(self.sock, PORT_3),
29  Ultrasonic(self.sock, PORT_4)]
30  self.resetPosition()
31 
32  def close(self):
33  """
34  Finalizing connection with NXT brick.
35  """
36  self.sock.close()
37 
38  def resetPosition(self, relative = 0):
39  """
40  Resetting encoders of NXT motors
41  """
42  for m in self.motors:
43  m.reset_position(relative)
44 
45  def setMotors(self, vels):
46  """
47  This operation receives array and set them as motor power. If the
48  number of vels items does not match with the number of motors,
49  smaller number of them will be taken and set respectively.
50  """
51  for i, v in enumerate(vels[:min(len(vels),len(self.motors))]):
52  self.motors[i].sync = 1
53  self.motors[i].run(max(min(v,127),-127))
54 
55  def getMotors(self):
56  """
57  Getting motors' angle (degrees)
58  """
59  state = []
60  for m in self.motors:
61  stat = None
62  for i in range(3):
63  try:
64  stat = m.get_tacho().tacho_count
65  break
66  except:
67  time.sleep(0.01)
68  continue
69 
70  if stat == None:
71  import sys
72  print "Unknown motor encoder error"
73  print sys.exc_info()[1]
74  state.append(stat)
75 
76  return state
77 
78 
79  def getSensors(self):
80  """
81  Getting sensors' values. Data will be returned as array.
82  """
83  state = []
84  for s in self.sensors:
85  stat = None
86  for i in range(3):
87  try:
88  stat = s.get_sample()
89  break
90  except:
91  time.sleep(0.01)
92  continue
93  if stat == None:
94  import sys
95  print "Unknown sensor error"
96  print sys.exc_info()[1]
97  state.append(stat)
98 
99  return state
100 
101 
102 """
103 Test program
104 It gives appropriate values to motors, and angles of motors are
105 obtained and shown. Sensor data are also obtained and shown.
106 """
107 if __name__ == "__main__":
108  import time
109  nxt = NXTBrick()
110  print "connected"
111 
112  # Testing motors
113  for i in range(0):
114  nxt.setMotors([80,-80,80])
115  print "Motor: "
116  mstat = nxt.getMotors()
117  for i, m in enumerate(mstat):
118  print "(" , i, "): ", m
119  time.sleep(0.1)
120  nxt.setMotors([0,0,0])
121 
122  # Testing sensors
123  for i in range(100):
124  sensors = ["Touch", "Sound", "Light", "USonic"]
125  sval = nxt.getSensors()
126  print sval
127  time.sleep(0.1)


openrtm_aist_python
Author(s): Shinji Kurihara
autogenerated on Mon Feb 28 2022 23:01:06