NXTBrick.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # -*- Python -*-
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   # Testing motors
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   # Testing sensors
00127   for i in range(100):
00128     sensors = ["Touch", "Sound", "Light", "USonic"]
00129     sval = nxt.getSensors()
00130     print sval
00131     time.sleep(0.1)


openrtm_aist_python
Author(s): Shinji Kurihara
autogenerated on Thu Aug 27 2015 14:17:28