00001 ################################################################################ 00002 # 00003 # File: wiimoteConstants.py 00004 # RCS: $Header: $ 00005 # Description: Constants for Wii Arm Control 00006 # Author: Andreas Paepcke 00007 # Created: Thu Aug 13 11:44:04 2009 (Andreas Paepcke) paepcke@anw.willowgarage.com 00008 # Modified: Thu Jan 13 13:44:31 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com 00009 # Language: Python 00010 # Package: N/A 00011 # Status: Experimental (Do Not Distribute) 00012 # 00013 # # 00014 ################################################################################ 00015 # 00016 # Revisions: 00017 # 00018 # Thu Mar 18 10:56:09 2010 (David Lu) davidlu@wustl.edu 00019 # Added nunchuk options 00020 # Fri Oct 29 08:58:21 2010 (Miguel Angel Julian Aguilar, QBO Project) miguel.angel@thecorpora.com 00021 # Added classic controller options 00022 ################################################################################ 00023 00024 _DEBUGLEVEL = 1 00025 _MONITOR_LEVEL = 1 00026 00027 import numpy as np 00028 00029 # Control over calibration: 00030 NUM_ZEROING_READINGS = 100 # Number of readings to take for zeroing acc and gyro 00031 NUM_WARMUP_READINGS = 100 # Number of readings to throw away initially so that 00032 # the gyro starts providing data for calibration 00033 00034 # Number of standard deviations that accelerator or 00035 # gyro measurements need to be beyond the mean (axis 00036 # by axis) to be considered an outlier: 00037 00038 OUTLIER_STDEV_MULTIPLE = 3 00039 00040 # Whether to calibrate the Wiimote even when 00041 # the calibration process was less than perfect: 00042 00043 CALIBRATE_WITH_FAILED_CALIBRATION_DATA = False 00044 00045 # TimedSwitch message field value constants: 00046 SWITCH_ON = 1 00047 SWITCH_OFF = 0 00048 SWITCH_PULSE_PATTERN = -1 00049 00050 # Output indicators on the Wiimote: 00051 00052 RUMBLE = 1 00053 LED = 2 00054 00055 # Buttons in cwiid structure: 00056 00057 BTN_1 = 0x0002 # cwiid.CWIID_BTN_1 00058 BTN_2 = 0x0001 # cwiid.CWIID_BTN_2 00059 BTN_B = 0x0004 # cwiid.CWIID_BTN_B 00060 BTN_A = 0x0008 # cwiid.CWIID_BTN_A 00061 BTN_MINUS = 0x0010 # cwiid.CWIID_BTN_MINUS 00062 BTN_PLUS = 0x1000 # cwiid.CWIID_BTN_PLUS 00063 BTN_LEFT = 0x0100 # cwiid.CWIID_BTN_LEFT 00064 BTN_RIGHT = 0x0200 # cwiid.CWIID_BTN_RIGHT 00065 BTN_DOWN = 0x0400 # cwiid.CWIID_BTN_DOWN 00066 BTN_UP = 0x0800 # cwiid.CWIID_BTN_UP 00067 BTN_HOME = 0x0080 # cwiid.CWIID_BTN_HOME 00068 00069 # nunchuk Buttons 00070 BTN_C = 0x0002 00071 BTN_Z = 0x0001 00072 00073 # Classic Controller Buttons 00074 CLASSIC_BTN_UP = 0x0001 00075 CLASSIC_BTN_LEFT = 0x0002 00076 CLASSIC_BTN_ZR = 0x0004 00077 CLASSIC_BTN_X = 0x0008 00078 CLASSIC_BTN_A = 0x0010 00079 CLASSIC_BTN_Y = 0x0020 00080 CLASSIC_BTN_B = 0x0040 00081 CLASSIC_BTN_ZL = 0x0080 00082 CLASSIC_BTN_R = 0x0200 00083 CLASSIC_BTN_PLUS = 0x0400 00084 CLASSIC_BTN_HOME = 0x0800 00085 CLASSIC_BTN_MINUS = 0x1000 00086 CLASSIC_BTN_L = 0x2000 00087 CLASSIC_BTN_DOWN = 0x4000 00088 CLASSIC_BTN_RIGHT = 0x8000 00089 00090 X = 0 00091 Y = 1 00092 Z = 2 00093 00094 PHI = 0 00095 THETA = 1 00096 PSI = 2 00097 00098 00099 # Buttons order in Wiimote ROS message array: 00100 # These buttons are now defined in Wiimote.msg, 00101 # so that they are available to other languages. 00102 00103 #MSG_BTN_1 = 0 00104 #MSG_BTN_2 = 1 00105 #MSG_BTN_A = 2 00106 #MSG_BTN_B = 3 00107 #MSG_BTN_PLUS = 4 00108 #MSG_BTN_MINUS = 5 00109 #MSG_BTN_LEFT = 6 00110 #MSG_BTN_RIGHT = 7 00111 #MSG_BTN_UP = 8 00112 #MSG_BTN_DOWN = 9 00113 #MSG_BTN_HOME = 10 00114 00115 00116 X_COORD = 'x' 00117 Y_COORD = 'y' 00118 Z_COORD = 'z' 00119 NORM_X = 'normX' 00120 NORM_Y = 'normY' 00121 NORM_Z = 'normZ' 00122 00123 NUM_IR_SENSORS = 4 00124 IR1 = 'ir1' 00125 IR2 = 'ir2' 00126 IR3 = 'ir3' 00127 IR4 = 'ir4' 00128 00129 NUM_LEDS = 4 00130 LED1_ON = 0x01 00131 LED2_ON = 0x02 00132 LED3_ON = 0x04 00133 LED4_ON = 0x08 00134 00135 # Indices into a two-tuple of info about battery state 00136 # in wiimote messages: 00137 # 0: Percentage of battery left. 00138 # 1: The raw battery reading 00139 00140 BATTERY_PERCENTAGE = 0 00141 BATTERY_RAW = 1 00142 00143 # Turning wiimote accelerator readings from g's to m/sec^2: 00144 EARTH_GRAVITY = 9.80665 # m/sec^2 00145 00146 # Turning wiimote gyro readings to radians/sec. 00147 # This scale factor is highly approximate. Procedure: 00148 # - Tape Wiimote to center of an office chair seat 00149 # - Rotate the chair at approximately constant speed 00150 # for 10 seconds. This resulted in 6 chair revolutions 00151 # - On average, the Wiimote gyro read 3570 during this 00152 # experiment. 00153 # - Speed of chair revolving: 00154 # * One full circle is: 2#pi radians 00155 # * Six revolutions = 12pi radians. ==> 12pi rad in 10 sec ==> 1.2pi rad/sec 00156 # * => 3570 == 1.2pi 00157 # * => x*3570 = 1.2pi 00158 # * => x = 1.2pi/3570 (1.2pi = 3.769908) 00159 # * => scale factor = 0.001055997 00160 # So multiplying the gyro readings by this factor 00161 # calibrates the readings to show angular velocity 00162 # in radians/sec. 00163 GYRO_SCALE_FACTOR = 0.001055997 00164 00165 # Status type of message from Wii to us: 00166 WII_MSG_TYPE_STATUS = 0 00167 WII_MSG_TYPE_BTN = 1 00168 WII_MSG_TYPE_ACC = 2 00169 WII_MSG_TYPE_IR = 3 00170 WII_MSG_TYPE_NUNCHUK = 4 00171 WII_MSG_TYPE_CLASSIC = 5 00172 # WII_MSG_TYPE_BALANCE = 6 00173 WII_MSG_TYPE_MOTIONPLUS = 7 # Gyro 00174 WII_MSG_TYPE_ERROR = 8 00175 WII_MSG_TYPE_UNKNOWN = 9 00176 00177 00178 #define CWIID_IR_X_MAX 1024 00179 #define CWIID_IR_Y_MAX 768 00180 00181 ACC_X_STDEV_THRESHOLD = 1.0 00182 ACC_Y_STDEV_THRESHOLD = 1.0 00183 ACC_Z_STDEV_THRESHOLD = 1.0 00184 00185 GYRO_X_STDEV_THRESHOLD = 50.0 00186 GYRO_Y_STDEV_THRESHOLD = 50.0 00187 GYRO_Z_STDEV_THRESHOLD = 50.0 00188 00189 THRESHOLDS_ARRAY = np.array([ACC_X_STDEV_THRESHOLD, 00190 ACC_Y_STDEV_THRESHOLD, 00191 ACC_Z_STDEV_THRESHOLD, 00192 GYRO_X_STDEV_THRESHOLD, 00193 GYRO_Y_STDEV_THRESHOLD, 00194 GYRO_Z_STDEV_THRESHOLD]) 00195 00196