pyHand_api.py
Go to the documentation of this file.
00001 #  pyHand_api.py
00002 #  
00003 #  ~~~~~~~~~~~~
00004 #  
00005 #
00006 #  pyHand_api based on pyHand_api.py from Barrett Technology Hand Communications
00007 #  
00008 #  ~~~~~~~~~~~~
00009 #  
00010 # Software License Agreement (BSD License)
00011 #
00012 # Copyright (c) 2014, Robotnik Automation SLL
00013 # All rights reserved.
00014 #
00015 # Redistribution and use in source and binary forms, with or without
00016 # modification, are permitted provided that the following conditions
00017 # are met:
00018 #
00019 #  * Redistributions of source code must retain the above copyright
00020 #    notice, this list of conditions and the following disclaimer.
00021 #  * Redistributions in binary form must reproduce the above
00022 #    copyright notice, this list of conditions and the following
00023 #    disclaimer in the documentation and/or other materials provided
00024 #    with the distribution.
00025 #  * Neither the name of Robotnik Automation SSL nor the names of its
00026 #    contributors may be used to endorse or promote products derived
00027 #    from this software without specific prior written permission.
00028 #
00029 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00030 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00031 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00032 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00033 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00034 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00035 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00036 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00037 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00038 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00039 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00040 # POSSIBILITY OF SUCH DAMAGE.
00041 
00042 
00043 from pcan_python.pcan_library import *
00044 
00045 
00046 from puck_properties_consts import*
00047 from ctypes import *
00048 import time
00049 
00050 BASE_TYPE = 0
00051 TIP_TYPE = 1
00052 SPREAD_TYPE = 2
00053 BASE_LIMIT = 140.0
00054 TIP_LIMIT = 48.0
00055 SPREAD_LIMIT = 180.0
00056 
00057 HAND_GROUP = 0x405
00058 
00059 # CAN MSG IDs
00060 F1_POSITION = 0x563
00061 F2_POSITION = 0x583
00062 F3_POSITION = 0x5A3
00063 SPREAD_POSITION = 0x5C3
00064 F1_STRAIN       = 0x566
00065 F2_STRAIN       = 0x586
00066 F3_STRAIN       = 0x5A6
00067 STRAIN_ID   = 0x99
00068 F1_MOTOR_TEMP   = 0x566
00069 F2_MOTOR_TEMP   = 0x586
00070 F3_MOTOR_TEMP   = 0x5A6
00071 SPREAD_MOTOR_TEMP       = 0x5C6
00072 MOTOR_TEMP_ID   = 0x89
00073 F1_MOTOR_THERM  = 0x566
00074 F2_MOTOR_THERM  = 0x586
00075 F3_MOTOR_THERM  = 0x5A6
00076 SPREAD_MOTOR_THERM      = 0x5C6
00077 MOTOR_THERM_ID   = 0x94
00078 F1_TACT = 0x569
00079 F2_TACT = 0x589
00080 F3_TACT = 0x5A9
00081 PALM_TACT = 0x5C9
00082 TACT_ID = 0x40
00083 
00084 
00085 #==========================CAN_STUFF=======================================
00086 
00087 # Class pyHand based on library pyHand_api.py
00088 class pyHand:
00089         
00090         def __init__(self, port = '/dev/pcan32'):
00091                 '''
00092                 Creates a PCANBasic object
00093 
00094                         @param port: CAN port used to communicate with the hand
00095                         @type connection: string
00096                 '''
00097                 
00098                 self.PCAN = PCANBasic(port)
00099                 
00100                 self.motor_positions = {FINGER1: {'encoder': [0, 0], 'position': [0.0, 0.0]}, FINGER2: {'encoder': [0, 0], 'position': [0.0, 0.0]}, FINGER3: {'encoder': [0, 0], 'position': [0.0, 0.0]},
00101                 SPREAD: {'encoder': [0, 0], 'position': [0.0, 0.0]}}
00102                 self.strain = {FINGER1: 0, FINGER2: 0, FINGER3: 0}
00103                 self.temp = {FINGER1:{ 'temp': 0.0, 'therm': 0.0}, FINGER2: { 'temp': 0.0, 'therm': 0.0}, FINGER3: { 'temp': 0.0, 'therm': 0.0},
00104                  SPREAD: { 'temp': 0.0, 'therm': 0.0}}
00105                 self.tactile_sensor = {FINGER1: {'values': range(0,24), 'data': range(0,5)}, FINGER2: {'values': range(0,24), 'data': range(0,5)}, 
00106                 FINGER3: {'values': range(0,24), 'data': range(0,5)}, SPREAD: {'values': range(0,24), 'data': range(0,5)}}
00107                 
00108         def check_error(self, connection,result,location_of_error):
00109                 '''
00110                 Checks error on the CAN Bus.
00111 
00112                         @param connection: The connection on which the CAN is talking. For example, PCANBasic()
00113                         @type connection: PCANBasic
00114                         @param result: The number corresponding to the error returned.
00115                         @type resulr: int
00116                         @param location_of_error: A description of where the error occurred.
00117                         @type location_of_error: str
00118                 '''
00119                 if result == PCAN_ERROR_OK:
00120                         pass
00121                 else:
00122                         raise Exception("Error Number: " + hex(result) + " while attempting to " + str(location_of_error) + "\n" + connection.GetErrorText(result)[1])
00123 
00124         def enum(self):
00125                 '''
00126                 Finds and returns all of the pucks that are attached to the bus.
00127 
00128                         @rtype: Array[]
00129                         @return: An array containing the pucks attached to the bus.
00130                 '''
00131                 pucks = []
00132                 for i in range(32):
00133                         try:
00134                                 self.get_property(i,1)
00135                                 pucks.append(i)
00136                         except:
00137                                 pass
00138                 return pucks
00139 
00140         def can_reset(self):
00141                 '''
00142                 Resets the CAN connection.
00143                 Note that this may cause a loss of data, but may also clear unwanted data. Utilize as needed.
00144                 '''
00145                 reset_result=self.PCAN.Reset(PCAN_USBBUS1)
00146                 try:
00147                         self.check_error(self.PCAN,reset_result,"reset")
00148                 except:
00149                         return False
00150                 time.sleep(0.025)
00151                 return True
00152 
00153         def can_status(self):
00154                 '''
00155                 Returns the status of the CAN connection as specified in PCANBasic's GetStatus method.
00156                 '''
00157                 status_result=self.PCAN.GetStatus(PCAN_USBBUS1)
00158                 time.sleep(0.025)
00159                 return status_result
00160 
00161         def can_init(self):
00162                 '''
00163                 Initializes the CAN connection. Note that this does not initialize the hand itself.
00164                 '''
00165                 # initialize self.PCAN bus
00166                 init_result=self.PCAN.Initialize(PCAN_USBBUS1, PCAN_BAUD_1M)
00167                 self.check_error(self.PCAN,init_result,"initialize")
00168                 time.sleep(0.025)
00169 
00170         def can_uninit(self):
00171                 '''
00172                 Uninitializes the CAN connection. Rarely used.
00173                 '''
00174                 uninit_result=self.PCAN.Uninitialize(PCAN_USBBUS1)
00175                 self.check_error(self.PCAN,uninit_result,"uninitialize")
00176                 
00177         #============================INIT_STUFF==================================
00178                 
00179         def initialize(self):
00180                 '''
00181                 Wakes up pucks and initializes the CAN.
00182                 This function must be implemented at the beginning of a program for this library to properly work.
00183 
00184                         @rtype: ROLE
00185                         @return: First finger's ROLE property if initialization is successful. Otherwise it returns False.
00186                 '''
00187                 try:
00188                         self.can_init()
00189                         #Reset Can bus
00190                         self.can_reset()
00191                         time.sleep(0.025)
00192                         # wake up pucks by setting the STAT(5) property to READY(2)
00193                         self.set_property(0x400, 5, 2)
00194                         time.sleep(1)
00195                         self.can_reset()
00196                         
00197                         return True
00198                 except:
00199                         return False
00200 
00201         def init_hand(self):
00202                 '''
00203                 Initialize all pucks in the hand.
00204 
00205                         @rtype: Boolean
00206                         @return: Succesfully Initialized Hand?
00207                 '''
00208                 try:
00209                         #write 13 to the command property (CMD-29) of each of the pucks.
00210                         self.init_finger(FINGER1)
00211                         self.init_finger(FINGER2)
00212                         self.init_finger(FINGER3)
00213                         time.sleep(3)
00214                         self.init_finger(SPREAD)
00215                         time.sleep(2)
00216                         
00217                         self.can_reset()
00218                         self.get_property(FINGER1, ROLE)
00219                         return True
00220                 except:
00221                         self.can_reset()
00222                         return False
00223 
00224         def init_finger(self, msgID):
00225                 '''
00226                 Sends a command to the puck to wake up the motor attatched to the puck.
00227 
00228                         @param msgID: The id of the finger to initialize.
00229                         @type msgID: int
00230                 '''
00231                 self.set_property(msgID, CMD, CMD_HI)
00232 
00233         #============================SET_AND_GET_STUFF=========================
00234 
00235         def read_msg(self):
00236                 '''
00237                 Read a general message from PCAN_USBBUS1
00238                 Typically, msg[1] (where msg is the thing returned), contains the pertinent information.
00239 
00240                         @rtype: (TPCANStatus, TPCANMsg, TPCANTimestamp)
00241                         @return: A tuple containing the status, message, and timestamp.
00242                 '''
00243                 return self.PCAN.Read(PCAN_USBBUS1)
00244 
00245         #def write_msg(self, msgID, data, delay=.015):
00246         
00247         def write_msg(self, msgID, data, delay=.002):
00248                 '''
00249                 Send a general message to PCAN_USBBUS1. This can be a get, set, or even garbage.
00250 
00251                         @param msgID: The puck or group to which the message will be sent.
00252                         @type msgID: int
00253                         @param data: The array containing the data for the TPCANMsg.
00254                         @type data: Array[]
00255                         @param delay: The time delay to wait for the message to be written.
00256                         @type delay: float
00257                         @rtype: TPCANStatus
00258                         @return: Status of the self.PCAN bus.
00259                 '''
00260                 msg = TPCANMsg()
00261                 msg.ID = msgID
00262                 msg.LEN = len(data)
00263                 msg.MSGTYPE = PCAN_MESSAGE_STANDARD
00264                 for j in range(0, len(data)):
00265                         msg.DATA[j] = data[j]
00266                 stat = self.PCAN.Write(PCAN_USBBUS1, msg)
00267                 self.check_error(self.PCAN,stat,"write")
00268                 time.sleep(delay)
00269                 return stat
00270         
00271         
00272         def send_msg(self, msgID, data):
00273                 '''
00274                         Send a general message to PCAN_USBBUS1. This can be a get, set, or even garbage.
00275                         It does not apply any sleep
00276                         
00277                         @param msgID: The puck or group to which the message will be sent.
00278                         @type msgID: int
00279                         @param data: The array containing the data for the TPCANMsg.
00280                         @type data: Array[]
00281                         
00282                         @return the return status after writing in the bus
00283                 '''
00284                 msg = TPCANMsg()
00285                 msg.ID = msgID
00286                 msg.LEN = len(data)
00287                 msg.MSGTYPE = PCAN_MESSAGE_STANDARD
00288                 for j in range(0, len(data)):
00289                         msg.DATA[j] = data[j]
00290                 stat = self.PCAN.Write(PCAN_USBBUS1, msg)
00291                 self.check_error(self.PCAN,stat,"write")
00292                 
00293                 return stat
00294 
00295         def set_property(self, msgID, propID, value):
00296                 '''
00297                 Set property to a given value.
00298 
00299                         @param msgID: The puck or group whose property will be set.
00300                         @type msgID: int
00301                         @param propID: The number corresponding to the property to be set.
00302                         @type propID: int
00303                         @param value: The value to which the property will be set.
00304                         @type value: int
00305                 '''
00306                 is32bits = [48, 50, 52, 54, 56, 58, 66, 68, 74, 88, 96, 98]
00307                 if propID in is32bits:
00308                         self.set_32(msgID, propID, value)
00309                 else:
00310                         self.set_16(msgID, propID, value)
00311                         
00312         def set_32(self, msgID, propID, value):
00313                 '''
00314                 Set property to a given value for a 32 bit property.
00315                 Avoid usage of this method. Use self.set_property instead.
00316 
00317                         @param msgID: The puck or group whose property will be set.
00318                         @type msgID: int
00319                         @param propID: The number corresponding to the property to be set.
00320                         @type propID: int
00321                         @param value: The value to which the property will be set.
00322                         @type value: int
00323                 '''
00324                 data = [0x80+propID, 0, value%0x100, int(value/0x100)%0x100, int(value/0x10000)%0x100, int(value/0x1000000)]
00325                 #self.write_msg(msgID, data)
00326                 return self.send_msg(msgID, data)
00327 
00328         def set_16(self, msgID, propID, value):
00329                 '''
00330                 Set property to a given value for a 16 bit property.
00331                 Avoid usage of this method. Use self.set_property instead.
00332                 
00333                         @param msgID: The puck or group whose property will be set.
00334                         @type msgID: int
00335                         @param propID: The number corresponding to the property to be set.
00336                         @type propID: int
00337                         @param value: The value to which the property will be set.
00338                         @type value: int
00339                 '''
00340                 data = [0x80+propID, 0, value%256, int(value/256)]
00341                 #self.write_msg(msgID, data)
00342                 return self.send_msg(msgID, data)
00343 
00344 
00345         # TODO: Allow a "GET" from a group.
00346         def get_property(self, msgID, propID):
00347                 '''
00348                 Get property from pucks in msgID.
00349 
00350                         @param msgID: The puck whose property will be read from.
00351                         @type msgID: int
00352                         @param propID: The property be read from.
00353                         @type propID: int
00354                         @rtype: int
00355                         @return: The value held in the property.
00356                 '''
00357                 is32bits = [48, 50, 52, 54, 56, 58, 66, 68, 74, 88, 96, 98]
00358                 if propID in is32bits:
00359                         return self.get_32(msgID, propID)
00360                 else:
00361                         if propID == TACT:
00362                                 return self.get_tact(msgID)
00363                         return self.get_16(msgID, propID)
00364 
00365 
00366         def get_32(self, msgID, propID):
00367                 '''
00368                 Gets a 32 bit property. Please use get_property instead of this method where applicable.
00369 
00370                         @param msgID: The puck whose property will be read from.
00371                         @type msgID: int
00372                         @param propID: The property be read from.
00373                         @type propID: int
00374                         @rtype: int
00375                         @return: The value held in the property.
00376                 '''
00377                 self.write_msg(msgID, [propID])
00378                 #time.sleep(0.005)
00379                 read_result = self.PCAN.Read(PCAN_USBBUS1)
00380                 self.check_error(self.PCAN, read_result[0], "read")
00381                 data = read_result[1].DATA
00382                 value = (0x1000000 * data[5]) + (0x0010000 * data[4]) + (0x0000100 * data[3]) + (0x0000001 * data[2])
00383                 return value
00384 
00385 
00386         def get_16(self, msgID, propID):
00387                 '''
00388                 Gets a 16 bit property. Please use get_property instead of this method where applicable.
00389                 
00390                         @param msgID: The puck whose property will be read from.
00391                         @type msgID: int
00392                         @param propID: The property be read from.
00393                         @type propID: int
00394                         @rtype: int
00395                         @return: The value held in the property.
00396                 '''
00397                 self.write_msg(msgID, [propID])
00398                 #time.sleep(0.05)
00399                 #time.sleep(0.005)
00400                 read_result = self.PCAN.Read(PCAN_USBBUS1)
00401                 self.check_error(self.PCAN, read_result[0], "read")
00402                 data = read_result[1].DATA
00403                 value =(0x0000100 * data[3]) + (0x0000001 * data[2])
00404                 return value
00405 
00406                 
00407         def save_property(self, msgID, propID):
00408                 '''
00409                 Save a property.
00410 
00411                         @param msgID: The puck or group to have its property saved.
00412                         @type msgID: int
00413                         @param propID: The property to be saved.
00414                         @type propID: int
00415                 '''
00416                 self.set_property(msgID, SAVE, propID)
00417 
00418 
00419         def load_property(self, msgID, propID):
00420                 '''
00421                 Load a property's value for puck's flash memory.
00422 
00423                         @param msgID: The puck or group to have its property loaded.
00424                         @type msgID: int
00425                         @param propID: The property to be loaded.
00426                         @type propID: int
00427                 '''
00428                 self.set_property(msgID, LOAD, propID)
00429                 
00430         def get_prop_quick(self, msgID,propID,speed):
00431                 '''
00432                 Gets a property timed at a certain rate.
00433 
00434                         @param msgID: The puck or group to have its property gotten.
00435                         @type msgID: int
00436                         @param propID: The property to be saved.
00437                         @type propID: int
00438                         @param speed: The time delay for the get.
00439                         @type speed: float
00440                 '''
00441                 self.write_msg(msgID, [propID],speed)
00442                 read_result=self.read_msg_resilient(msgID,propID)
00443                 self.check_error(self.PCAN, read_result[0], "read")
00444                 data = read_result[1].DATA
00445                 value = (0x1000000 * data[5]) + (0x0010000 * data[4]) + (0x0000100 * data[3]) + (0x0000001 * data[2])
00446                 return value
00447 
00448         def read_msg_resilient(self, expect_puck,expect_prop,max_recurse=10,counter=0):
00449                 '''
00450                 Reads message given the puckID and the propertyID. 
00451                 It will read as normal, until it gets some expected output from the puck.
00452 
00453                         @param expect_puck: The puck to read from.
00454                         @type expect_puck: int
00455                         @param expect_prop: The property read from.
00456                         @type expect_prop: int
00457                         @param max_recurse: The most number of times to repeat the get.
00458                         @type max_recurse: int
00459                         @param counter: Used internally. Do not set.
00460                         @type counter: int
00461                         @rtype: int
00462                         @return: The value held in the property of the given puck.
00463                 '''
00464                 counter+=1
00465                 response=self.PCAN.Read(PCAN_USBBUS1)
00466                 received_prop=response[1].DATA[0]-128
00467                 received_puck=(response[1].ID-1024)>>5
00468                 
00469                 if (received_prop==expect_prop) and (received_puck==expect_puck):
00470                         return response
00471                 else:
00472                         print "a"
00473                         if counter<max_recurse:
00474                                 return self.read_msg_resilient(expect_puck,expect_prop,counter=counter)
00475                         else:
00476                                 raise Exception("Missed message")
00477 
00478         def get_role(self, msgID):
00479                 '''
00480                 Read from ROLE property and return something that makes sense.
00481                 Returns an array of holding the following data:
00482 
00483                 [4-bit Product Identifier,
00484                 Internal Thermistor,
00485                 20 MHz (vs 32 MHz),
00486                 Hall Motor Encoder,
00487                 Enc Motor Encoder,
00488                 Strain Gauge,
00489                 IMU for Force-Torque Sensor,
00490                 Optical Motor Encoder]
00491 
00492                         @param msgID: The puck to get the ROLE from.
00493                         @type msgID: int
00494                         @rtype: Array[int,bool,bool,bool,bool,bool,bool,bool]
00495                         @return: An array holding the above values.
00496                 '''
00497                 role = self.get_property(msgID, 1)
00498                 data= [0,0,0,0,0,0,0,0,0]
00499                 data[0] = role%16               # 4-bit Product Identifier
00500                 data[1] = int(role/2**6)%2==1   # Internal Thermistor Bit
00501                 data[2] = int(role/2**7)%2==1   # 20 MHz (vs 32 MHz)
00502                 data[3] = int(role/2**8)%2==1   # Is there a Dig+Ser Motor Encoder?
00503                 data[4] = int(role/2**9)%2==1   # Is there a Hall Motor Encoder?
00504                 data[5] = int(role/2**10)%2==1  # Is there an Enc Motor Encoder?
00505                 data[6] = int(role/2**11)%2==1  # Is there a Strain Gauge?
00506                 data[7] = int(role/2**12)%2==1  # IMU for Force-Torque Sensor
00507                 data[8] = int(role/2**13)%2==1  # Optical Motor Encoder
00508                 return data
00509 
00510         def get_mode(self, msgID):
00511                 '''
00512                 Read from MODE property, and return a tuple with the number corresponding to the mode, along with
00513                 the string name of the mode.
00514 
00515                         @param msgID: The puck from which to get the mode.
00516                         @type msgID: int
00517                         @rtype: Tuple(int, str)
00518                         @return: A tuple with the number and name of the mode.
00519                 '''
00520                 m = self.get_property(msgID, MODE)
00521                 if m==MODE_IDLE:
00522                         return (MODE_IDLE, "IDLE")
00523                 elif m==MODE_TORQUE:
00524                         return (MODE_TORQUE, "TORQUE")
00525                 elif m==MODE_PID:
00526                         return (MODE_PID, "PID")
00527                 elif m==MODE_VEL:
00528                         return (MODE_VEL, "VEL")
00529                 elif m==MODE_TRAP:
00530                         return (MODE_TRAP, "TRAP")
00531                 else:
00532                         print "Invalid get_mode() operation: "+str(m)
00533                         return (m, "???")
00534 
00535         def set_mode(self, msgID, value):
00536                 '''
00537                 Set the mode property using either strings or numbers.
00538 
00539                         @param msgID: The puck or group to set the mode.
00540                         @type msgID: int
00541                         @param value: The value to which the mode should be set.
00542                         @type value: int
00543                 '''
00544                 modes = {"IDLE":MODE_IDLE, "TORQUE":MODE_TORQUE, "PID":MODE_PID, "VEL":MODE_VEL, "TRAP":MODE_TRAP}
00545                 m = value
00546                 if m in modes:
00547                         m = modes[m]
00548                 self.set_property(msgID, MODE, m)
00549                 
00550         # I'm sorry that this method is so unbearably long, but it won't compile otherwise because of circular imports TT_TT
00551         def set_puck_like(self, puckID, virtID):
00552                 '''
00553                 Set the puck to have all the default properties of the indicated puck ID.
00554 
00555                         @param puckID: The original puck to change.
00556                         @type puckID: int
00557                         @param virtID: The ID of the puck to load defaults from.
00558                         @type virtID: int
00559                 '''
00560                 self.set_property(puckID, MODE, MODE_IDLE)
00561                 # Set universal puck properties. (taterDefs[])
00562                 # if virtID in range(1,8)
00563                 #     self.set_property(puckID, TIE, 0)
00564                 #     self.set_property(puckID, ACCEL, 100)
00565                 #     self.set_property(puckID, AP, 0)
00566                 #     self.set_property(puckID, CT, 4096)
00567                 #     self.set_property(puckID, OT, 0)
00568                 #     self.set_property(puckID, CTS, 4096)
00569                 #     self.set_property(puckID, DP, 0)
00570                 #     self.set_property(puckID, MV, 100)
00571                 #     self.set_property(puckID, MCV, 100)
00572                 #     self.set_property(puckID, MOV, 100)
00573                 #     self.set_property(puckID, OT, 0)
00574                 #     self.set_property(puckID, HOLD, 0)
00575                 #     self.set_property(puckID, TSTOP, 0)
00576                 #     self.set_property(puckID, OTEMP, 60)
00577                 #     self.set_property(puckID, PTEMP, 0)
00578                 #     self.set_property(puckID, DS, -256)
00579                 #     self.set_property(puckID, KP, 2000)
00580                 #     self.set_property(puckID, KD, 8000)
00581                 #     self.set_property(puckID, KI, 0)
00582                         #wamDefaultMT has yet to be implemented correctly. The interns coding this don't really care about the WAM, so this will be put off until someone does.
00583                 
00584                 #Set Barrett Hand Defaults
00585                 is_280 = self.get_property(puckID, HALLH)==7 #Identifier for 280 version
00586                 if virtID in [FINGER1, FINGER2, FINGER3, SPREAD]:
00587                         self.set_property(puckID, JIDX, virtID-3)
00588                         self.save_property(puckID, JIDX)
00589                         self.set_property(puckID, PIDX, virtID-10)
00590                         self.save_property(puckID, PIDX)
00591                         self.set_property(puckID, TIE, 0)
00592                         self.save_property(puckID, TIE)
00593                         self.set_property(puckID, ACCEL, 200)
00594                         self.save_property(puckID, ACCEL)
00595                         #self.set_property(puckID, AP, 0)
00596                         #self.save_property(puckID, AP)
00597                         self.set_property(puckID, OT, 0)
00598                         self.save_property(puckID, OT)
00599                         self.set_property(puckID, CTS, 4096)
00600                         self.save_property(puckID, CTS)
00601                         self.set_property(puckID, MT, 2200)
00602                         self.save_property(puckID, MT)
00603                         self.set_property(puckID, MCV, 200)
00604                         self.save_property(puckID, MCV)
00605                         self.set_property(puckID, MOV, 200)
00606                         self.save_property(puckID, MOV)
00607                         self.set_property(puckID, OTEMP, 60)
00608                         self.save_property(puckID, OTEMP)
00609                         self.set_property(puckID, PTEMP, 0)
00610                         self.save_property(puckID, PTEMP)
00611                         self.set_property(puckID, POLES, 6)
00612                         self.save_property(puckID, POLES)
00613                         self.set_property(puckID, IKCOR, 102)
00614                         self.save_property(puckID, IKCOR)
00615                         self.set_property(puckID, IOFF, 0)
00616                         self.save_property(puckID, IOFF)
00617                         self.set_property(puckID, IVEL, -75)
00618                         self.save_property(puckID, IVEL)
00619                         self.set_property(puckID, DS, 25600)
00620                         self.save_property(puckID, DS)
00621                         self.set_property(puckID, KI, 0)
00622                         self.save_property(puckID, KI)
00623                         self.set_property(puckID, IPNM, 20000)
00624                         self.save_property(puckID, IPNM)
00625                         self.set_property(puckID, GRPA, 0)
00626                         self.save_property(puckID, GRPA)
00627                         self.set_property(puckID, GRPB, 7)
00628                         self.save_property(puckID, GRPB)
00629                         self.set_property(puckID, GRPC, 5)
00630                         self.save_property(puckID, GRPC)
00631                         self.set_property(puckID, IKI, 204)
00632                         self.save_property(puckID, IKI)
00633                         self.set_property(puckID, IKP, 500)
00634                         self.save_property(puckID, IKP)
00635                         if virtID == SPREAD:
00636                                 self.set_property(puckID, CT, 35950)
00637                                 self.save_property(puckID, CT)
00638                                 self.set_property(puckID, DP, 17975)
00639                                 self.save_property(puckID, DP)
00640                                 self.set_property(puckID, MV, 50)
00641                                 self.save_property(puckID, MV)
00642                                 self.set_property(puckID, HSG, 0)
00643                                 self.save_property(puckID, HSG)
00644                                 self.set_property(puckID, LSG, 0)
00645                                 self.save_property(puckID, LSG)
00646                                 self.set_property(puckID, HOLD, 1)
00647                                 self.save_property(puckID, HOLD)
00648                                 self.set_property(puckID, TSTOP, 150)
00649                                 self.save_property(puckID, TSTOP)
00650                                 self.set_property(puckID, KP, 1000)
00651                                 self.save_property(puckID, KP)
00652                                 self.set_property(puckID, KD, 10000)
00653                                 self.save_property(puckID, KD)
00654                         else:
00655                                 self.set_property(puckID, CT, 195000)
00656                                 self.save_property(puckID, CT)
00657                                 self.set_property(puckID, DP, 45000)
00658                                 self.save_property(puckID, DP)
00659                                 self.set_property(puckID, MV, 200)
00660                                 self.save_property(puckID, MV)
00661                                 self.set_property(puckID, HSG, 0)
00662                                 self.save_property(puckID, HSG)
00663                                 self.set_property(puckID, LSG, 0)
00664                                 self.save_property(puckID, LSG)
00665                                 self.set_property(puckID, HOLD, 0)
00666                                 self.save_property(puckID, HOLD)
00667                                 self.set_property(puckID, TSTOP, 50)
00668                                 self.save_property(puckID, TSTOP)
00669                                 self.set_property(puckID, KP, 500)
00670                                 self.save_property(puckID, KP)
00671                                 self.set_property(puckID, KD, 2500)
00672                                 self.save_property(puckID, KD)
00673                 else:
00674                         print "Invalid Puck Id for Hand"
00675                 
00676         #============================MOVING_STUFF================================
00677 
00678         def set_hand_targets(self, f1_target, f2_target, f3_target, sp_target):
00679                 '''
00680                 Given fingers and spread target values, move the hand to that position.
00681                 Will mainly be used to load user-defined hand positions.
00682                 Takes Barrett Units as inputs.
00683 
00684                         @param f1_target: The position (in encoder ticks) for finger 1 to move to.
00685                         @type f1_target: int
00686                         @param f2_target: The position for finger 2 to move to.
00687                         @type f2_target: int
00688                         @param f3_target: The position for finger 3 to move to.
00689                         @type f3_target: int
00690                         @param sp_target: The position for spread to move to.
00691                         @type sp_target: int
00692                 '''
00693                 self.set_property(FINGER1, DP, f1_target)
00694                 self.set_property(FINGER2, DP, f2_target)
00695                 self.set_property(FINGER3, DP, f3_target)
00696                 tar = self.get_position(SPREAD)
00697                 self.set_property(SPREAD, DP, tar)
00698                 self.set_property(0x405, CMD, CMD_MOVE)
00699                 time.sleep(2)
00700                 self.set_property(SPREAD, DP, sp_target)
00701                 self.set_property(SPREAD, CMD, CMD_MOVE)
00702                 time.sleep(1)
00703 
00704         def move_to(self, puckID, target, autowait=True):
00705                 '''
00706                 Move the motor to a specific position.
00707 
00708                         @param puckID: The puck to move.
00709                         @type puckID: int
00710                         @param target: The end position to move to.
00711                         @type target: int
00712                         @param autowait: Does the program wait until the motor is done moving?
00713                         @type autowait: bool
00714                 '''
00715                 self.set_property(puckID, M, target)
00716                 if autowait:
00717                         self.wait_done_moving([puckID])
00718 
00719         def done_moving(self, motors_to_check=ALL_FINGERS):
00720                 '''
00721                 Checks a given list of motors once to see if they have stopped moving, and if so, it returns true
00722 
00723                         @param motors_to_check: A list of motors to check.
00724                         @type motors_to_check: Array[*int]
00725                         @rtype: bool
00726                         @return: Whether or not the motors are done moving.
00727                 '''
00728                 for FINGER in motors_to_check:
00729                         if (self.get_mode(FINGER)[1]!="IDLE" and (self.get_mode(FINGER)[1]!="PID" or self.get_property(FINGER,77)==0)):
00730                                 return False
00731                 return True
00732 
00733         # TODO: Ensure that TSTOP > 0. Otherwise, this may be an infinite loop.
00734         def wait_done_moving(self, motors_to_check=ALL_FINGERS):
00735                 '''
00736                 Waits until the given list of motors have all stopped moving.
00737 
00738                         @param motors_to_check: A list of motors to wait for.
00739                         @type motors_to_check: Array[*int]
00740                 '''
00741                 while(not self.done_moving(motors_to_check)):
00742                         time.sleep(0.025)
00743 
00744         def detect_breakaway(self, finger):
00745                 '''
00746 
00747                         @return: True if the finger has broken away, False if it hasn't
00748                 '''
00749                 tup = self.get_packed_position(finger)
00750                 ratio = tup[0]/tup[1]
00751                 return (ratio>3)
00752 
00753         def open_grasp(self):
00754                 '''
00755                 Opens all fingers to the position encoded by Open Target (OT)
00756                 '''
00757                 self.set_property(0x405, TSTOP, 50)
00758                 self.set_property(SPREAD, TSTOP, 150)
00759 
00760                 open_target=self.get_property(FINGER1,OT)
00761                 self.set_property(FINGER1,DP,open_target)
00762                 
00763                 open_target=self.get_property(FINGER2,OT)
00764                 self.set_property(FINGER2,DP,open_target)
00765                 
00766                 open_target=self.get_property(FINGER3,OT)
00767                 self.set_property(FINGER3,DP,open_target)
00768 
00769                 spread_stay=self.get_position(SPREAD)
00770                 self.set_property(SPREAD,DP,spread_stay)
00771                 
00772                 self.set_property(0x405,CMD,CMD_MOVE)
00773                 self.wait_done_moving(GRASP)
00774 
00775         def close_grasp(self):
00776                 '''
00777                 Closes all fingers to the position encoded by Close Target (CT).
00778                 '''
00779                 self.set_property(0x405, TSTOP, 50)
00780                 self.set_property(SPREAD, TSTOP, 150)
00781 
00782                 close_target=self.get_property(FINGER1,CT)
00783                 self.set_property(FINGER1,DP,close_target)
00784 
00785                 close_target=self.get_property(FINGER2,CT)
00786                 self.set_property(FINGER2,DP,close_target)
00787 
00788                 close_target=self.get_property(FINGER3,CT)
00789                 self.set_property(FINGER3,DP,close_target)
00790 
00791                 spread_stay=self.get_position(SPREAD)
00792                 self.set_property(SPREAD,DP,spread_stay)
00793 
00794                 self.set_property(0x405,CMD,CMD_MOVE)
00795                 self.wait_done_moving(GRASP)
00796                 
00797         def open_spread(self):
00798                 '''
00799                 Open spread to position determined by Open Target (OT).
00800                 '''
00801                 self.set_property(SPREAD, TSTOP, 150)
00802                 self.set_property(SPREAD, CMD, CMD_OPEN)
00803                 self.wait_done_moving([SPREAD])
00804 
00805         def close_spread(self):
00806                 '''
00807                 Close spread to position determined by Close Target (CT).
00808                 '''
00809                 self.set_property(SPREAD, CMD, CMD_CLOSE)
00810                 self.wait_done_moving([SPREAD])
00811 
00812         def open_finger(self, puckID, autowait=True):
00813                 '''
00814                 Open finger and wait for completion.
00815 
00816                         @param puckID: Finger to be opened.
00817                         @type puckID: int
00818                         @param autowait: calls wait_done_moving if True. Defaults to True.
00819                         @type autowait: bool
00820                 '''
00821                 if puckID in [FINGER1, FINGER2, FINGER3]:
00822                         self.set_property(puckID, TSTOP, 50)
00823                 if puckID == SPREAD:
00824                         self.set_property(puckID, TSTOP, 150)
00825                 self.set_property(puckID, CMD, CMD_OPEN)
00826                 if autowait:
00827                         self.wait_done_moving([puckID])
00828 
00829         def close_finger(self, puckID, autowait=True):
00830                 '''
00831                 Close finger and wait for completion.
00832 
00833                 @param puckID: Finger to be closed.
00834                 @type puckID: int
00835                 @param autowait: calls wait_done_moving if True. Defaults to True.
00836                 @type autowait: bool
00837                 '''
00838                 if puckID in [FINGER1, FINGER2, FINGER3]:
00839                         self.set_property(puckID, TSTOP, 50)
00840                 if puckID == SPREAD:
00841                         self.set_property(puckID, TSTOP, 150)
00842                 self.set_property(puckID, CMD, CMD_CLOSE)
00843                 if autowait:
00844                         self.wait_done_moving([puckID])
00845 
00846         def move_grasp(self, position = -1):
00847                 '''
00848                 Moves all fingers to input argument or default position (50).
00849 
00850                         @param position: position of fingers. Defaults to -1. Valid position range is from 0-195,000 encoder counts.
00851                         @type position: int
00852                 '''
00853                 default = self.get_position(SPREAD)
00854                 self.set_property(SPREAD, DP, default)
00855                 if(position != -1):
00856                         self.set_property(FINGER1, DP, position)
00857                         self.set_property(FINGER2, DP, position)
00858                         self.set_property(FINGER3, DP, position)        
00859                 self.move()
00860 
00861         def move(self):
00862                 '''
00863                 Moves all fingers/spread to their default.
00864                 '''
00865                 self.set_property(0x405, CMD, CMD_MOVE)
00866                 self.wait_done_moving(GRASP)
00867 
00868         def open_all(self):
00869                 '''
00870                 Opens every fingers at once. Mainly used in DEMO. WARNING: can be dangerous because it may cause fingers to collide if the hand is in an unknown position.
00871                 '''
00872                 open_target = self.get_property(FINGER1, OT)
00873                 self.set_property(FINGER1, DP, open_target)
00874                 open_target = self.get_property(FINGER2, OT)
00875                 self.set_property(FINGER2, DP, open_target)
00876                 open_target = self.get_property(FINGER3, OT)
00877                 self.set_property(FINGER3, DP, open_target)
00878                 open_target = self.get_property(SPREAD, OT)
00879                 self.set_property(SPREAD, DP, open_target)
00880                 self.move()
00881 
00882         def close_all(self):
00883                 '''
00884                 Closes every finger at once. Mainly for use in DEMO. WARNING: can be dangerous because it may cause fingers to collide if the hand is in an unknown position.
00885                 '''
00886                 close_target = self.get_property(FINGER1, CT)
00887                 self.set_property(FINGER1, DP, close_target)
00888                 close_target = self.get_property(FINGER2, CT)
00889                 self.set_property(FINGER2, DP, close_target)
00890                 close_target = self.get_property(FINGER3, CT)
00891                 self.set_property(FINGER3, DP, close_target)
00892                 close_target = self.get_property(SPREAD, CT)
00893                 self.set_property(SPREAD, DP, close_target)
00894                 self.move()
00895 
00896         #============================INCREMENTALLY_MOVE_STUFF========================
00897                 
00898         def open_grasp_step(self, step=0):
00899                 '''
00900                 Open grasp by input increment.
00901 
00902                         @param step: size of increment in encoder counts. Defaults to 0.
00903                         @type step: int
00904                 '''
00905                 self.open_finger_step(FINGER1, step, False)
00906                 self.open_finger_step(FINGER2, step, False)
00907                 self.open_finger_step(FINGER3, step, False)
00908                 self.wait_done_moving(GRASP)
00909 
00910         def close_grasp_step(self, step=0):
00911                 '''
00912                 Close grasp by input decrement.
00913 
00914                         @param step: size of decrement in encoder counts. Defaults to 0.
00915                         @type step: int
00916                 '''
00917                 self.close_finger_step(FINGER1, step, False)
00918                 self.close_finger_step(FINGER2, step, False)
00919                 self.close_finger_step(FINGER3, step, False)
00920                 self.wait_done_moving(GRASP)
00921 
00922         def open_spread_step(self, step=-1):
00923                 '''
00924                 Open spread by input increment.
00925 
00926                         @param step: size of increment in encoder counts. Defaults to -1.
00927                         @type step: int
00928                 '''
00929                 if step == -1:
00930                         step = self.get_property(SPREAD, 60)
00931                 self.open_finger_step(SPREAD, step)
00932                 
00933         def close_spread_step(self, step=-1):
00934                 '''
00935                 Close spread by input decrement.
00936 
00937                         @param step: size of decrement in encoder counts. Defaults to -1.
00938                         @type step: int 
00939                 '''
00940                 if step == -1:
00941                         step = self.get_property(SPREAD, 60)
00942                 self.close_finger_step(SPREAD, step)
00943 
00944         def open_finger_step(self, puckID, step=-1, autowait=True):
00945                 '''
00946                 Open finger by input increment.
00947 
00948                         @param puckID: Finger to be opened.
00949                         @type puckID: int
00950                         @param step: size of increment in encoder counts. Defaults to -1.
00951                         @type step: int
00952                         @param autowait: calls wait_done_moving if True. Defaults to True.
00953                         @type autowait: bool
00954                 '''
00955                 if step == -1:
00956                         step = self.get_property(puckID, 60)
00957                 self.set_property(puckID, DS, step)
00958                 self.set_property(puckID, CMD, CMD_IO)
00959                 if autowait:
00960                         self.wait_done_moving([puckID])
00961 
00962         def close_finger_step(self, puckID, step=-1, autowait=True):
00963                 '''
00964                 Close finger by input decrement.
00965 
00966                         @param puckID: Finger to be closed.
00967                         @type puckID: int
00968                         @param step: size of decrement in encoder counts. Defaults to -1.
00969                         @type step: int
00970                         @param autowait: calls wait_done_moving if True. Defaults to True.
00971                         @type autowait: bool
00972                 '''
00973                 if step == -1:
00974                         step = self.get_property(puckID, 60)
00975                 self.set_property(puckID, DS, step)
00976                 self.set_property(puckID, CMD, CMD_IC)
00977                 if autowait:
00978                         self.wait_done_moving([puckID])
00979 
00980         #===========================NON_TRIVIAL FUNCTIONS=============================
00981         def get_full_pos_packet(msgID):
00982                         self.write_msg(msgID, [P])
00983                         read_result=self.PCAN.Read(PCAN_USBBUS1)
00984                         return read_result
00985                         
00986         def get_velocity(self, msgID):
00987                 '''
00988                 Returns velocity values of finger when in motion. Mostly returns garbage. It's used to help tell when finger is stopped or near to it.
00989 
00990                         @param msgID: The puck or group to get velocity.
00991                         @type msgID: int
00992                         @rtype: float
00993                         @return: A (garbage) value representing the approximate velocity of the finger.
00994                 '''     
00995                 packet1=self.get_full_pos_packet(msgID)
00996                 packet2=self.get_full_pos_packet(msgID)
00997                 
00998                 error1=packet1[0]
00999                 error2=packet2[0]
01000 
01001                 msg1=packet1[1]
01002                 msg2=packet2[1]
01003 
01004                 data1=msg1.DATA 
01005                 data2=msg2.DATA
01006 
01007                 time1=packet1[2]
01008                 time2=packet2[2]
01009                 
01010                 stamp1=time1.micros + 1000 * time1.millis + 0xFFFFFFFF * 1000 * time1.millis_overflow
01011                 stamp2=time2.micros + 1000 * time2.millis + 0xFFFFFFFF * 1000 * time2.millis_overflow
01012 
01013                 delta=(stamp2-stamp1)/1000000.0
01014                 
01015                 self.check_error(self.PCAN,error1,"reading position for fake get_velocity")
01016                 self.check_error(self.PCAN,error2,"reading position for fake get_velocity")
01017 
01018                 val1=(0x0000100 * data1[3]) + (0x0000001 * data1[2])
01019                 val2=(0x0000100 * data2[3]) + (0x0000001 * data2[2])
01020                 
01021                 return (val2-val1)/delta
01022 
01023         def get_temp(self, msgID):
01024                 '''
01025                 Gets temperature value for all pucks in msgID.
01026 
01027                         @param msgID: The puck or group to get temp.
01028                         @type msgID: int
01029                         @rtype: int
01030                         @return: The value of the TEMP property.
01031                 '''
01032                 return self.temp[msgID]['temp']
01033                 
01034                 
01035         def read_temp(self, msgID):
01036                 '''
01037                 Sends a message to get the temperature value for all pucks in msgID.
01038 
01039                         @param msgID: The puck or group to get temp.
01040                         @type msgID: int
01041                         @rtype: int
01042                         @return: The value of the TEMP property.
01043                 '''
01044                 return self.send_msg(msgID, [TEMP])
01045 
01046         def get_therm(self, msgID):
01047                 '''
01048                 Gets motor temperature value for all pucks in msgID. 
01049 
01050                         @param msgID: The puck or group to get motor temperature.
01051                         @text msgID: int
01052                         @rtype: int
01053                         @return: The value of the THERM property.
01054                 '''
01055                 return self.temp[msgID]['therm']
01056         
01057         def read_therm(self, msgID):
01058                 '''
01059                 Gets motor temperature value for all pucks in msgID. 
01060 
01061                         @param msgID: The puck or group to get motor temperature.
01062                         @text msgID: int
01063                         @rtype: int
01064                         @return: The value of the THERM property.
01065                 '''
01066                 return self.send_msg(msgID, [THERM])
01067 
01068         def get_top_tact(self, msgID):
01069                 '''
01070                 Unpack the top10 values from TACT.
01071                 Returns a dictionary with 10 items like (sensor number):(tact value).
01072 
01073                         @param msgID: The puck or group to get top 10 tactile data.
01074                         @type msgID: int
01075                         @rtype: Dictionary{sensorID:value}
01076                         @return topVals: Dictionary of the top 10 tactile array sensor values. 
01077                 '''
01078                 # Set TACT(106) to top10 mode (1)
01079                 self.set_property(msgID, TACT, TACT_10) 
01080                 self.write_msg(msgID, [TACT]) #GET TACT
01081                 read_result=self.PCAN.Read(PCAN_USBBUS1)
01082                 self.check_error(self.PCAN,read_result[0],"reading top ten tactile values")
01083                 #output is mapped to here.
01084                 output = read_result[1].DATA 
01085                 #parsing this output
01086                 top10 = output[0] * 0x10000 + output[1] * 0x100 + output[2] * 0x1
01087                 topVals = {}
01088                 data = [output[3]/(0x10), output[3]%(0x10), output[4]/(0x10), output[4]%(0x10), output[5]/(0x10), output[5]%(0x10)]
01089                 count=0
01090                 for sensor in range(0, 24):
01091                         if top10%2 == 1:
01092                                 #print 'sensor = %d, count = %d'%(sensor, count)
01093                                 topVals[sensor] = data[count]
01094                                 count+=1
01095                         top10 = top10/2
01096                         # Each bit represents one of the top 10 pressures for purposes of efficiency. top10 has the last bit sliced
01097                 return topVals
01098 
01099         def read_full_tact(self, msgID):
01100                 '''
01101                         Read all tactile sensors
01102                 '''
01103                 return self.set_property(msgID, TACT, TACT_FULL) 
01104 
01105         def get_full_tact(self, msgID):
01106                 '''
01107                 Unpack all tactile sensor values in an array.
01108 
01109                         @param msgID: The puck or group to get full tactile array sensor data.
01110                         @type msgID: int
01111                         @rtype: Array[*data]
01112                         @return: An array containing the tactile data from a given puck.
01113                 '''
01114                 # Set TACT(106) to full mode (2)
01115                 '''self.set_property(msgID, TACT, TACT_FULL) 
01116                 #self.write_msg(msgID, [TACT])
01117                 output = [0,0,0,0,0]
01118                 read_result = self.PCAN.Read(PCAN_USBBUS1)
01119                 self.check_error(self.PCAN,read_result[0],"reading full tactile data")
01120                 read_result2 = self.PCAN.Read(PCAN_USBBUS1)
01121                 self.check_error(self.PCAN,read_result2[0],"reading full tactile data")
01122                 read_result3 = self.PCAN.Read(PCAN_USBBUS1)
01123                 self.check_error(self.PCAN,read_result3[0],"reading full tactile data")
01124                 read_result4 = self.PCAN.Read(PCAN_USBBUS1)
01125                 self.check_error(self.PCAN,read_result4[0],"reading full tactile data")
01126                 read_result5 = self.PCAN.Read(PCAN_USBBUS1)
01127                 self.check_error(self.PCAN,read_result5[0],"reading full tactile data")
01128                 
01129                 output[0] = read_result[1].DATA
01130                 output[1] = read_result2[1].DATA
01131                 output[2] = read_result3[1].DATA
01132                 output[3] = read_result4[1].DATA
01133                 output[4] = read_result5[1].DATA
01134                 
01135                 #print 'Init: ID1 = %x,  ID2 = %x,  ID3 = %x,  ID4 = %x,  ID5 = %x'%(read_result[1].ID, read_result2[1].ID, read_result3[1].ID, read_result4[1].ID, read_result5[1].ID)
01136                 tactileVals = range(0,24)
01137                 index_ = 0
01138                 for data in output:
01139                         index_ = int(data[0]/16) * 5
01140                         #print 'index = %d, data[0] = %x'%(index_, data[0])
01141                         # Get the bits and then unpack them.
01142                         tactileVals[index_ + 0] = round(((data[0]%0x10)*0x100 + data[1])/256.0,2)
01143                         tactileVals[index_ + 1] = round((data[2]*0x10 + int(data[3]/0x10))/256.0,2)
01144                         tactileVals[index_ + 2] = round(((data[3]%0x10)*0x100 + data[4])/256.0,2)
01145                         tactileVals[index_ + 3] = round((data[5]*0x10 + int(data[6]/0x10))/256.0,2)
01146                         if index_ != 20:
01147                                 tactileVals[index_ + 4] = round(((data[6]%0x10)*0x100 + data[7])/256.0,2)
01148                 #print 'Return OK : %s'%(tactileVals)
01149                 return tactileVals'''
01150                 
01151                 return self.tactile_sensor[msgID]['values']
01152 
01153 
01154         def get_tact(self, msgID, topOrFull="TOP10"):
01155                 '''
01156                 Obtain and interpret tactile sensor data.
01157 
01158                         @param msgID: The puck or group to get full or top 10 tactile array sensor data.
01159                         @type msgID: int
01160                         @param topOrFull: To get full data, enter "FULL". To get the top 10 values, enter "TOP10". Or anything else, really.
01161                         @type topOrFull: str
01162                         @return 
01163                 '''
01164                 if topOrFull == "FULL":
01165                         return self.get_full_tact(msgID)
01166                 else:
01167                         return self.get_top_tact(msgID)
01168 
01169         def set_velocity(self, puckID, velocity):
01170                 '''
01171                 Set the velocity and make the motor move.
01172 
01173                         @param puckID: The ID of the puck to set the velocity of.
01174                         @type puckID: int
01175                         @param velocity: The velocity (in cts/ms) of the motor.
01176                         @type velocity: int
01177                 '''
01178                 #First set TSTOP to 0.
01179                 self.set_property(puckID, TSTOP, 0)
01180                 #Set Velocity
01181                 self.set_property(puckID, V, velocity)
01182                 #Set mode to allow the puck to move.
01183                 self.set_property(puckID, MODE, MODE_VEL)
01184 
01185 
01186         def get_strain(self, msgID):
01187                 '''
01188                 Gets the fingertip torque sensor value. 
01189 
01190                         @param msgID: The puck or group to get fingertip torque sensor data. 
01191                         @type msgID: int
01192                         @rtype: int
01193                         @return: Strain Gauge Reading
01194                 '''
01195                 return self.strain[msgID]
01196         
01197         
01198         def read_strain(self, msgID):
01199                 '''
01200                 Sends the message to get the fingertip torque sensor value. 
01201 
01202                         @param msgID: The puck or group to get fingertip torque sensor data. 
01203                         @type msgID: int
01204                         @rtype: int
01205                         @return: CAN status
01206                 '''
01207                 return self.send_msg(msgID, [SG])
01208                 
01209         
01210 
01211         def onescomp(self, binstr):
01212                 return ''.join('1' if b=='0' else '0' for b in binstr)
01213 
01214         def twoscomp(self, number):
01215                 binstr= bin(number)[2:]
01216                 a= bin(int(self.onescomp(binstr),2)+1)[2:]
01217                 return -1*int(a,2)
01218 
01219         def get_position(self, msgID, depth=0):
01220                 '''
01221                 Get packed position data and return it.
01222 
01223                         @param msgID: The puck or group to get position data.
01224                         @type msgID: int
01225                         @rtype: int
01226 
01227                         @param depth: number of times get message was retried.
01228                         
01229                         @return: The position of the finger in encoder counts.
01230                 '''
01231                 if depth!=0:
01232                         self.write_msg(msgID, [P],.009)
01233                 read_result=self.PCAN.Read(PCAN_USBBUS1)
01234                 try:
01235                         self.check_error(self.PCAN,read_result[0],"getting position data")
01236                         received_puck=(read_result[1].ID-1024)>>5
01237                         if received_puck!=msgID:
01238                                 raise Exception("Did not read expected MSGID")
01239                 except:
01240                         if depth>10:
01241                                 raise Exception("Failure to get position data.")
01242                         else:
01243                                 return self.get_position(msgID, depth+1)
01244 
01245                 output = read_result[1].DATA
01246                 temp=(output[0]-0x80)*0x10000 + output[1] * 0x100 + output[2]
01247 
01248                 if (temp & 0b1000000000000000000000): 
01249                         return self.twoscomp(temp)
01250                 else:
01251                         return temp
01252 
01253         def get_packed_position(self, msgID):
01254                 '''
01255                 Get packed position data and return both P and JP.
01256 
01257                         @param msgID: The puck or group to get position data.
01258                         @type msgID: int
01259                         @rtype: (int, int)
01260 
01261                         @return: The position and joint position of the finger in encoder counts.
01262                                          Position in radians
01263                 '''
01264                 
01265                 return  self.motor_positions[msgID]['position']
01266         
01267         
01268         def read_packed_position(self, msgID):
01269                 '''
01270                 Get packed position data and return both P and JP.
01271 
01272                         @param msgID: The puck or group to get position data.
01273                         @type msgID: int
01274                         @rtype: (int, int)
01275 
01276                         @return: sends a msg to read the position.
01277                 '''
01278                 
01279                 return self.send_msg(msgID, [P])
01280                 
01281         
01282         def process_can_messages(self):
01283                 ''' 
01284                         Reads and process all the msgs in the bus
01285                         Depending on the CAN id, it'll use different methods
01286                 '''
01287                 ret = 0
01288                 # Reads a can msg
01289                 msg = self.read_msg()
01290                 
01291                 # No read messages
01292                 if msg[0] != PCAN_ERROR_OK:
01293                         ret = -1
01294                 
01295                 while msg[0] == PCAN_ERROR_OK:
01296                         
01297                         can_id = msg[1].ID
01298                         #print 'process_can_messages: CAN ID = %x'%can_id
01299                         
01300                         if can_id in [F1_POSITION, F2_POSITION, F3_POSITION, SPREAD_POSITION]:
01301                                 self.process_packed_position(msg[1])
01302                         elif can_id in [F1_STRAIN, F2_STRAIN, F3_STRAIN] and msg[1].DATA[0] == STRAIN_ID:
01303                                 self.process_strain(msg[1])
01304                         elif can_id in [F1_MOTOR_TEMP, F2_MOTOR_TEMP, F3_MOTOR_TEMP, SPREAD_MOTOR_TEMP] and msg[1].DATA[0] == MOTOR_TEMP_ID:
01305                                 self.process_motor_temp(msg[1])
01306                         elif can_id in [F1_MOTOR_THERM, F2_MOTOR_THERM, F3_MOTOR_THERM, SPREAD_MOTOR_THERM] and msg[1].DATA[0] == MOTOR_THERM_ID:
01307                                 self.process_motor_therm(msg[1])
01308                         elif can_id in [F1_TACT, F2_TACT, F3_TACT, PALM_TACT]:
01309                                 self.process_full_tact(msg[1])
01310                                 
01311                         msg = self.read_msg()
01312                         
01313                 return ret
01314         
01315         def process_packed_position(self, msg):
01316                 '''
01317                         Process the CAN msgs and saves the position depending on the MSG ID
01318                 '''
01319                 data = msg.DATA
01320                 pos = (data[0]-0x80)*0x10000 + data[1]*0x100 + data[2]
01321                 jpos= (data[3]-0x80)*0x10000 + data[4]*0x100 + data[5]
01322                 pos = self.twoscomp(pos) if pos & 0b1000000000000000000000 else pos
01323                 jpos= self.twoscomp(jpos) if jpos & 0b1000000000000000000000 else jpos
01324                 
01325                 if msg.ID == F1_POSITION:
01326                         self.motor_positions[FINGER1]['encoder'][0] = pos
01327                         self.motor_positions[FINGER1]['encoder'][1] = jpos
01328                         self.motor_positions[FINGER1]['position'][0] = self.enc_to_rad(pos, BASE_TYPE)
01329                         self.motor_positions[FINGER1]['position'][1] = self.enc_to_rad(jpos, BASE_TYPE)
01330                         #print 'F1'
01331                         
01332                 elif msg.ID == F2_POSITION:
01333                         self.motor_positions[FINGER2]['encoder'][0] = pos
01334                         self.motor_positions[FINGER2]['encoder'][1] = jpos
01335                         self.motor_positions[FINGER2]['position'][0] = self.enc_to_rad(pos, BASE_TYPE)
01336                         self.motor_positions[FINGER2]['position'][1] = self.enc_to_rad(jpos, BASE_TYPE)
01337                         #print 'F2'
01338                         
01339                 elif msg.ID == F3_POSITION:
01340                         self.motor_positions[FINGER3]['encoder'][0] = pos
01341                         self.motor_positions[FINGER3]['encoder'][1] = jpos
01342                         self.motor_positions[FINGER3]['position'][0] = self.enc_to_rad(pos, BASE_TYPE)
01343                         self.motor_positions[FINGER3]['position'][1] = self.enc_to_rad(jpos, BASE_TYPE)
01344                         #print 'F3 = %f, %f'%(self.motor_positions[FINGER3]['position'][0], self.motor_positions[FINGER3]['position'][1])
01345                         #print 'F3'
01346                         
01347                 elif msg.ID == SPREAD_POSITION:
01348                         self.motor_positions[SPREAD]['encoder'][0] = pos
01349                         self.motor_positions[SPREAD]['encoder'][1] = jpos
01350                         self.motor_positions[SPREAD]['position'][0] = self.enc_to_rad(pos, SPREAD_TYPE)
01351                         self.motor_positions[SPREAD]['position'][1] = self.enc_to_rad(jpos, SPREAD_TYPE)
01352                         #print 'SPREAD = %f, %f'%(self.motor_positions[SPREAD]['position'][0], self.motor_positions[SPREAD]['position'][1])
01353                         
01354         
01355         def process_strain(self, msg):
01356                 '''
01357                         Process the msg and extract the strain value depending on the CAN ID
01358                 '''
01359                 data = msg.DATA
01360                 value =(0x0000100 * data[3]) + (0x0000001 * data[2])
01361                 
01362                 if msg.ID == F1_STRAIN:
01363                         self.strain[FINGER1] = value
01364                 if msg.ID == F2_STRAIN:
01365                         self.strain[FINGER2] = value
01366                 if msg.ID == F3_STRAIN:
01367                         self.strain[FINGER3] = value
01368         
01369         def process_motor_temp(self, msg):
01370                 '''
01371                         Process the msg and extract the temperature of the motor puck on the CAN ID
01372                 '''
01373                 data = msg.DATA
01374                 value =(0x0000100 * data[3]) + (0x0000001 * data[2])
01375                 
01376                 if msg.ID == F1_MOTOR_TEMP:
01377                         self.temp[FINGER1]['temp'] = value
01378                         
01379                 if msg.ID == F2_MOTOR_TEMP:
01380                         self.temp[FINGER2]['temp'] = value
01381                         
01382                 if msg.ID == F3_MOTOR_TEMP:
01383                         self.temp[FINGER3]['temp'] = value
01384                         
01385                 if msg.ID == SPREAD_MOTOR_TEMP:
01386                         self.temp[SPREAD]['temp'] = value
01387                         #print 'Motor Temp S'
01388                         
01389         def process_motor_therm(self, msg):
01390                 '''
01391                         Process the msg and extract the temperature of the motor on the CAN ID
01392                 '''
01393                 data = msg.DATA
01394                 value =(0x0000100 * data[3]) + (0x0000001 * data[2])
01395                 
01396                 if msg.ID == F1_MOTOR_THERM:
01397                         self.temp[FINGER1]['therm'] = value
01398                         
01399                 if msg.ID == F2_MOTOR_THERM:
01400                         self.temp[FINGER2]['therm'] = value
01401                         
01402                 if msg.ID == F3_MOTOR_THERM:
01403                         self.temp[FINGER3]['therm'] = value
01404                         
01405                 if msg.ID == SPREAD_MOTOR_THERM:
01406                         self.temp[SPREAD]['therm'] = value
01407                         #print 'Motor Temp S'
01408         
01409         def process_full_tact(self, msg):
01410                 '''
01411                         Process and saves all the messages containing the tactile information
01412                 '''
01413                 
01414                 data = msg.DATA
01415                 
01416                 if msg.ID == F1_TACT:
01417                         if data[0] >= 0x00 and data[0] < 0x10:
01418                                 self.tactile_sensor[FINGER1]['data'][0] = data                  
01419                         if data[0] >= 0x10 and data[0] < 0x20:
01420                                 self.tactile_sensor[FINGER1]['data'][1] = data
01421                         if data[0] >= 0x20 and data[0] < 0x30:
01422                                 self.tactile_sensor[FINGER1]['data'][2] = data
01423                         if data[0] >= 0x30 and data[0] < 0x40:
01424                                 self.tactile_sensor[FINGER1]['data'][3] = data
01425                         if data[0] >= 0x40:
01426                                 self.tactile_sensor[FINGER1]['data'][4] = data
01427                                 self.tactile_sensor[FINGER1]['values'] = self.process_tactile_data(self.tactile_sensor[FINGER1]['data'])
01428                 if msg.ID == F2_TACT:
01429                         if data[0] >= 0x00 and data[0] < 0x10:
01430                                 self.tactile_sensor[FINGER2]['data'][0] = data                  
01431                         if data[0] >= 0x10 and data[0] < 0x20:
01432                                 self.tactile_sensor[FINGER2]['data'][1] = data
01433                         if data[0] >= 0x20 and data[0] < 0x30:
01434                                 self.tactile_sensor[FINGER2]['data'][2] = data
01435                         if data[0] >= 0x30 and data[0] < 0x40:
01436                                 self.tactile_sensor[FINGER2]['data'][3] = data
01437                         if data[0] >= 0x40:
01438                                 self.tactile_sensor[FINGER2]['data'][4] = data
01439                                 self.tactile_sensor[FINGER2]['values'] = self.process_tactile_data(self.tactile_sensor[FINGER2]['data'])
01440                 if msg.ID == F3_TACT:
01441                         if data[0] >= 0x00 and data[0] < 0x10:
01442                                 self.tactile_sensor[FINGER3]['data'][0] = data                  
01443                         if data[0] >= 0x10 and data[0] < 0x20:
01444                                 self.tactile_sensor[FINGER3]['data'][1] = data
01445                         if data[0] >= 0x20 and data[0] < 0x30:
01446                                 self.tactile_sensor[FINGER3]['data'][2] = data
01447                         if data[0] >= 0x30 and data[0] < 0x40:
01448                                 self.tactile_sensor[FINGER3]['data'][3] = data
01449                         if data[0] >= 0x40:
01450                                 self.tactile_sensor[FINGER3]['data'][4] = data
01451                                 self.tactile_sensor[FINGER3]['values'] = self.process_tactile_data(self.tactile_sensor[FINGER3]['data'])
01452                 if msg.ID == PALM_TACT:
01453                         if data[0] >= 0x00 and data[0] < 0x10:
01454                                 self.tactile_sensor[SPREAD]['data'][0] = data                   
01455                         if data[0] >= 0x10 and data[0] < 0x20:
01456                                 self.tactile_sensor[SPREAD]['data'][1] = data
01457                         if data[0] >= 0x20 and data[0] < 0x30:
01458                                 self.tactile_sensor[SPREAD]['data'][2] = data
01459                         if data[0] >= 0x30 and data[0] < 0x40:
01460                                 self.tactile_sensor[SPREAD]['data'][3] = data
01461                         if data[0] >= 0x40:
01462                                 self.tactile_sensor[SPREAD]['data'][4] = data
01463                                 self.tactile_sensor[SPREAD]['values'] = self.process_tactile_data(self.tactile_sensor[SPREAD]['data'])
01464                                 
01465         def process_tactile_data(self, data_array):
01466                 '''
01467                         Process the array of data and returns a tactile array
01468                 '''
01469                 tactileVals = range(0,24)
01470                 index_ = 0
01471                 for data in data_array:
01472                         index_ = int(data[0]/16) * 5
01473                         #print 'index = %d, data[0] = %x'%(index_, data[0])
01474                         # Get the bits and then unpack them.
01475                         tactileVals[index_ + 0] = round(((data[0]%0x10)*0x100 + data[1])/256.0,2)
01476                         tactileVals[index_ + 1] = round((data[2]*0x10 + int(data[3]/0x10))/256.0,2)
01477                         tactileVals[index_ + 2] = round(((data[3]%0x10)*0x100 + data[4])/256.0,2)
01478                         tactileVals[index_ + 3] = round((data[5]*0x10 + int(data[6]/0x10))/256.0,2)
01479                         if index_ != 20:
01480                                 tactileVals[index_ + 4] = round(((data[6]%0x10)*0x100 + data[7])/256.0,2)
01481                 #print 'Return OK : %s'%(tactileVals)
01482                 return tactileVals
01483         
01484                 
01485         def new_temp_mail(self, fingers_to_change):
01486                 former_mailbox_c={}
01487                 for finger in fingers_to_change:
01488                         former_mailbox_c[finger]=self.get_property(finger,GRPC)
01489                         self.set_property(finger,GRPC,12)
01490                 return former_mailbox_c
01491 
01492         def revert_temp_mail(self, fingers_to_change,former):
01493                 for finger in fingers_to_change:
01494                         former_mailbox_value=former[finger]
01495                         self.set_property(finger,GRPC,former_mailbox_value)
01496         #==========================ANGLE_CONVERSIONS=========================
01497 
01498         def enc_to_per(self, enc):
01499                 '''
01500                 Given an angle in encoder counts, return the percentage of the angle that represents.
01501 
01502                         @param enc: Encoder counts.
01503                         @type enc: int
01504                         @return: Percentage
01505                         @rtype: float
01506                 '''
01507                 per = enc/1950.0
01508                 return round(per, 2)
01509 
01510         def enc_to_rad(self, enc, type = BASE_TYPE):
01511                 '''
01512                 Given an angle in encoder counts, return the radian measure of the angle that represents.
01513 
01514                         @param enc: Encoder counts.
01515                         @type enc: int
01516                         @return: Radians
01517                         @rtype: float
01518                 '''
01519                 motion_limit = BASE_LIMIT
01520                 tics = MAX_ENCODER_TICKS
01521                 
01522                 if type == TIP_TYPE:
01523                         motion_limit = TIP_LIMIT
01524                         tics = MAX_FINGERTIP_TICKS
01525                 elif type == SPREAD_TYPE:
01526                         motion_limit = SPREAD_LIMIT
01527                         tics = MAX_SPREAD_TICKS
01528                                 
01529                 PI = 3.141592653589
01530                 rad = enc * (motion_limit*PI/180)/tics
01531                 return round(rad,2)
01532 
01533         def enc_to_deg(self, enc):
01534                 '''
01535                 Given an angle in encoder counts, return the degree measure of the angle that represents.
01536 
01537                         @param enc: Encoder counts.
01538                         @type enc: int
01539                         @return: Degrees
01540                         @rtype: float
01541                 '''
01542                 deg = enc * 140/MAX_ENCODER_TICKS
01543                 return round(deg,2)
01544 
01545         def per_to_enc(self, per):
01546                 '''
01547                 Given a percentage of an angle, return it in encoder counts.
01548 
01549                         @param per: Percentage
01550                         @type per: float
01551                         @return: Encoder counts
01552                         @rtype: int
01553                 '''
01554                 enc = per * 1950.0
01555                 return int(enc)
01556 
01557         def rad_to_enc(self, rad, type = BASE_TYPE):
01558                 '''
01559                 Given the readian measure of an angle, return it in encoder counts.
01560 
01561                         @param rad: Radians
01562                         @type rad: float
01563                         @return: Encoder counts
01564                         @rtype: int
01565                 '''
01566                 motion_limit = BASE_LIMIT
01567                 tics = MAX_ENCODER_TICKS
01568                 
01569                 if type == TIP_TYPE:
01570                         motion_limit = TIP_LIMIT
01571                         tics = MAX_FINGERTIP_TICKS
01572                 elif type == SPREAD_TYPE:
01573                         motion_limit = SPREAD_LIMIT
01574                         tics = MAX_SPREAD_TICKS
01575                         
01576                 PI = 3.141592653589
01577                 enc = rad / ((motion_limit*PI/180)/tics)
01578                 return int(enc)
01579 
01580         def deg_to_enc(self, deg):
01581 
01582                 '''
01583                 Given a degree measure of an angle, return it in encoder counts.
01584 
01585                         @param deg: Degrees
01586                         @type deg: float
01587                         @return: Encoder counts
01588                         @rtype: int
01589                 '''
01590                 enc = deg * 195000.0/140
01591                 return int(enc)
01592         
01593         def clean_read_buffer(self):
01594                 
01595                 result_ = self.read_msg()
01596                         
01597                 while result_[0] == 0:
01598                         result_ = self.read_msg()
01599                 


bhand_controller
Author(s): Román Navarro , Jorge Ariño
autogenerated on Wed Aug 26 2015 10:45:49