NXTBrick.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().connect()
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 = [TouchSensor(self.sock, PORT_1),
27  SoundSensor(self.sock, PORT_2),
28  LightSensor(self.sock, PORT_3),
29  UltrasonicSensor(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].power = max(min(v,127),-127)
53  self.motors[i].mode = MODE_MOTOR_ON | MODE_REGULATED
54  self.motors[i].regulation_mode = REGULATION_MOTOR_SYNC
55  self.motors[i].run_state = RUN_STATE_RUNNING
56  self.motors[i].tacho_limit = 0
57  self.motors[i].set_output_state()
58 
59  def getMotors(self):
60  """
61  Getting motors' angle (degrees)
62  """
63  state = []
64  for m in self.motors:
65  stat = None
66  for i in range(3):
67  try:
68  stat = m.get_output_state()
69  break
70  except:
71  time.sleep(0.01)
72  continue
73 
74  if stat == None:
75  import sys
76  print "Unknown motor encoder error"
77  print sys.exc_info()[1]
78  state.append(stat)
79 
80  return state
81 
82 
83  def getSensors(self):
84  """
85  Getting sensors' values. Data will be returned as array.
86  """
87  state = []
88  for s in self.sensors:
89  stat = None
90  for i in range(3):
91  try:
92  stat = s.get_sample()
93  break
94  except:
95  time.sleep(0.01)
96  continue
97  if stat == None:
98  import sys
99  print "Unknown sensor error"
100  print sys.exc_info()[1]
101  state.append(stat)
102 
103  return state
104 
105 
106 """
107 Test program
108 It gives appropriate values to motors, and angles of motors are
109 obtained and shown. Sensor data are also obtained and shown.
110 """
111 if __name__ == "__main__":
112  import time
113  nxt = NXTBrick()
114  print "connected"
115 
116  # Testing motors
117  for i in range(0):
118  nxt.setMotors([80,-80,80])
119  print "Motor: "
120  mstat = nxt.getMotors()
121  for i, m in enumerate(mstat):
122  print "(" , i, "): ", m
123  time.sleep(0.1)
124  nxt.setMotors([0,0,0])
125 
126  # Testing sensors
127  for i in range(100):
128  sensors = ["Touch", "Sound", "Light", "USonic"]
129  sval = nxt.getSensors()
130  print sval
131  time.sleep(0.1)


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