animSkull_bluetooth-i2c.py (8420B)
1 # Bluetooth and i2c control for Sylvie 2021 (Animatronic Skull) 2 3 from adafruit_servokit import ServoKit 4 from time import sleep 5 6 import time 7 import board 8 import busio 9 import bluetooth 10 # import time 11 12 # import keyboard 13 import os 14 15 # Bluetooth socket settings 16 17 server_socket=bluetooth.BluetoothSocket( bluetooth.RFCOMM ) 18 port = 1 19 20 # Nvidia Jetson Nano i2c Bus 0 and 1 21 22 i2c_bus0 = (busio.I2C(board.SCL_1, board.SDA_1)) 23 i2c_bus1 = (busio.I2C(board.SCL, board.SDA)) 24 25 kit = ServoKit(channels=16, i2c=i2c_bus1) 26 27 # These are the adresses we setup in the Arduino Program 28 nema17z_address = 0x20 29 nema17x_address = 0x10 30 31 # Slots on the PCA9685 PWM Driver, and the corresponding servos 32 33 servo_0_eyeR_y = 0 34 servo_1_eyeL_y = 1 35 servo_2_eyeR_x = 2 36 servo_3_eyeL_x = 3 37 38 servo_4_lidR_top = 4 39 servo_5_lidL_top = 5 40 servo_6_lidR_bot = 6 41 servo_7_lidL_bot = 7 42 43 servo_8_browR = 8 44 servo_9_browL = 9 45 46 servo_10_lip = 10 47 servo_11_cornerR = 11 48 servo_12_cornerL = 12 49 50 servo_13_jawR = 13 51 servo_14_jawL = 14 52 53 # Offsets of articulation points... for fine tuning 54 55 offset_lid_top_right = 10 56 offset_lid_top_left = -5 57 58 offset_lid_bottom_right = -20 59 offset_lid_bottom_left = 30 60 61 offset_right_y = -10 62 offset_right_x = -10 63 64 offset_left_y = 15 65 offset_left_x = -10 66 67 offset_brow1 = -15 68 offset_brow2 = 5 69 70 offset_lip = -34 71 offset_corner1 = 0 72 offset_corner2 = -8 73 74 offset_jaw = 45 75 offset_jaw2 = 0 76 77 # previous servo positions for smooth motion 78 79 previous_pos_browR = 90 80 previous_pos_browL = 90 81 82 previous_pos_lip = 90 83 84 previous_pos_cornerR = 90 85 previous_pos_cornerL = 90 86 87 previous_pos_jawR = 90 88 previous_pos_jawL = 90 89 90 91 def initialize_servos(): 92 93 kit.servo[servo_0_eyeR_y].angle = 90 + offset_right_y 94 kit.servo[servo_1_eyeL_y].angle = 90 + offset_left_y 95 96 kit.servo[servo_2_eyeR_x].angle = 90 + offset_right_x 97 kit.servo[servo_3_eyeL_x].angle = 90 + offset_left_x 98 99 kit.servo[servo_4_lidR_top].angle = 90 + offset_lid_top_right 100 kit.servo[servo_5_lidL_top].angle = 90 + offset_lid_top_left 101 kit.servo[servo_6_lidR_bot].angle = 90 + offset_lid_bottom_right 102 kit.servo[servo_7_lidL_bot].angle = 90 + offset_lid_bottom_left 103 104 kit.servo[servo_8_browR].angle = 90 + offset_brow1 105 kit.servo[servo_9_browL].angle = 90 + offset_brow2 106 107 kit.servo[servo_10_lip].angle = 90 + offset_lip 108 kit.servo[servo_11_cornerR].angle = 90 + offset_corner1 109 kit.servo[servo_12_cornerL].angle = 90 + offset_corner2 110 111 kit.servo[servo_13_jawR].angle = 90 + offset_jaw 112 kit.servo[servo_14_jawL].angle = 90 + offset_jaw2 113 114 def move_eye_servos(posR_y=None, posL_y=None, posR_x=None, posL_x=None): 115 116 if posR_y is not None: 117 kit.servo[servo_0_eyeR_y].angle = posR_y + offset_right_y 118 if posL_y is not None: 119 kit.servo[servo_1_eyeL_y].angle = posL_y + offset_left_y 120 if posR_x is not None: 121 kit.servo[servo_2_eyeR_x].angle = posR_x + offset_right_x 122 if posL_x is not None: 123 kit.servo[servo_3_eyeL_x].angle = posL_x + offset_left_x 124 125 def move_eyelid_servos(posR_top=None, posL_top=None, posR_bot=None, posL_bot=None): 126 127 if posR_top is not None: 128 kit.servo[servo_4_lidR_top].angle = posR_top + offset_lid_top_right 129 if posL_top is not None: 130 kit.servo[servo_5_lidL_top].angle = posL_top + offset_lid_top_left 131 if posR_bot is not None: 132 kit.servo[servo_6_lidR_bot].angle = posR_bot + offset_lid_bottom_right 133 if posL_bot is not None: 134 kit.servo[servo_7_lidL_bot].angle = posL_bot + offset_lid_bottom_left 135 136 def move_face_servos(pos_browR=None, pos_browL=None, pos_lip=None, pos_cornerR=None, pos_cornerL=None, pos_jawR=None, pos_jawL=None): 137 138 global previous_pos_browR 139 global previous_pos_browL 140 141 global previous_pos_lip 142 143 global previous_pos_cornerR 144 global previous_pos_cornerL 145 146 global previous_pos_jawR 147 global previous_pos_jawL 148 149 if pos_browR is not None: 150 kit.servo[servo_8_browR].angle = pos_browR + offset_brow1 151 previous_pos_browR = pos_browR 152 if pos_browL is not None: 153 kit.servo[servo_9_browL].angle = pos_browL + offset_brow2 154 previous_pos_browL = pos_browL 155 if pos_lip is not None: 156 kit.servo[servo_10_lip].angle = pos_lip + offset_lip 157 previous_pos_lip = pos_lip 158 if pos_cornerR is not None: 159 kit.servo[servo_11_cornerR].angle = pos_cornerR + offset_corner1 160 previous_pos_cornerR = pos_cornerR 161 if pos_cornerL is not None: 162 kit.servo[servo_12_cornerL].angle = pos_cornerL + offset_corner2 163 previous_pos_cornerL = pos_cornerL 164 if pos_jawR is not None: 165 kit.servo[servo_13_jawR].angle = pos_jawR + offset_jaw 166 previous_pos_jawR = pos_jawR 167 if pos_jawL is not None: 168 kit.servo[servo_14_jawL].angle = pos_jawL + offset_jaw2 169 previous_pos_jawL = pos_jawL 170 171 initialize_servos() 172 173 print("Waiting for Bluetooth connection...") 174 server_socket.bind(("",port)) 175 server_socket.listen(1) 176 client_socket,address = server_socket.accept() 177 print("Accepted connection from ",address) 178 179 while True: 180 res = client_socket.recv(1024) 181 client_socket.send(res) 182 if str(res)[2] == 'q': 183 print("KEY [Q] RECEIVED!") 184 185 move_eyelid_servos(15, 155, 120, 50) 186 187 sleep(0.25) 188 189 elif str(res)[2] == 'w': 190 print("KEY [W] RECEIVED!") 191 192 move_eyelid_servos(90, 90, 90, 90) 193 194 sleep(0.25) 195 196 elif str(res)[2] == 'e': 197 print("KEY [E] RECEIVED!") 198 199 move_eyelid_servos(60, 120, 120, 60) 200 201 sleep(0.25) 202 203 elif str(res)[2] == 'r': 204 print("KEY [R] RECEIVED!") 205 206 move_eyelid_servos(90, 90, 90, 90) 207 208 sleep(0.25) 209 210 elif str(res)[2] == 'a': 211 print("KEY [A] RECEIVED!") 212 213 move_eye_servos(105, 70) 214 move_eyelid_servos(120, 60, 95, 85) 215 216 sleep(0.25) 217 218 elif str(res)[2] == 's': 219 print("KEY [S] RECEIVED!") 220 221 move_eye_servos(75, 100) 222 move_eyelid_servos(80, 100, 75, 115) 223 224 sleep(0.25) 225 226 elif str(res)[2] == 'd': 227 print("KEY [D] RECEIVED!") 228 229 move_eye_servos(None, None, 65, 120) 230 231 sleep(0.25) 232 233 elif str(res)[2] == 'f': 234 print("KEY [F] RECEIVED!") 235 236 move_eye_servos(None, None, 130, 60) 237 238 sleep(0.25) 239 240 elif str(res)[2] == 't': 241 print("KEY [T] RECEIVED") 242 243 i2c_bus0.writeto(nema17x_address, "6", stop=False) 244 245 sleep(0.25) 246 247 elif str(res)[2] == 'y': 248 print("KEY [Y] RECEIVED!") 249 250 i2c_bus0.writeto(nema17x_address, "7", stop=False) 251 252 sleep(0.25) 253 254 elif str(res)[2] == 'g': 255 print("KEY [G] RECEIVED!") 256 257 i2c_bus0.writeto(nema17z_address, "2", stop=False) 258 259 sleep(0.25) 260 261 elif str(res)[2] == 'h': 262 print("KEY [H] RECEIVED!") 263 264 i2c_bus0.writeto(nema17z_address, "3", stop=False) 265 266 sleep(0.25) 267 268 elif str(res)[2] == 'b': 269 print("KEY [B] RECEIVED!") 270 271 move_face_servos(80, 100) 272 273 sleep(0.25) 274 275 elif str(res)[2] == 'n': 276 print("KEY [N] RECEIVED!") 277 278 move_face_servos(100, 80) 279 280 sleep(0.25) 281 282 elif str(res)[2] == 'u': 283 print("KEY [U] RECEIVED!") 284 285 move_face_servos(None, None, 110) 286 287 sleep(0.25) 288 289 elif str(res)[2] == 'i': 290 print("KEY [I] RECEIVED!") 291 292 move_face_servos(None, None, 90) 293 294 sleep(0.25) 295 296 elif str(res)[2] == 'j': 297 print("KEY [J] RECEIVED!") 298 299 move_face_servos(90, 90, 90, 90, 90, 90, 90) 300 time.sleep(0.05) 301 move_face_servos(80, 100, 90, 80, 100, 90, 90) 302 time.sleep(0.05) 303 move_eyelid_servos(90, 90, 90, 90) 304 305 sleep(0.25) 306 307 elif str(res)[2] == 'k': 308 print("KEY [K] RECEIVED!") 309 310 move_face_servos(90, 90, 90, 90, 90, 90, 90) 311 time.sleep(0.05) 312 move_face_servos(95, 85, 93, 97, 83, 115, 65) 313 time.sleep(0.05) 314 move_eyelid_servos(70, 110, 105, 70) 315 316 sleep(0.25) 317 318 elif str(res)[2] == 'z': 319 print("KEY [Z] RECEIVED!") 320 321 move_eyelid_servos(15, 90, 120, 90) 322 323 sleep(0.25) 324 325 elif str(res)[2] == 'x': 326 print("KEY [X] RECEIVED!") 327 328 move_eyelid_servos(90, 155, 90, 50) 329 330 sleep(0.25) 331 332 elif str(res)[2] == '1': 333 print("KEY [1] RECEIVED!") 334 335 move_face_servos(105, 80, 100, 80, 100, 110, 70) 336 time.sleep(0.05) 337 move_face_servos(105, 80, 100, 80, 100, 120, 60) 338 time.sleep(0.05) 339 move_face_servos(105, 80, 100, 80, 100, 135, 45) 340 341 sleep(0.25) 342 343 elif str(res)[2] == '2': 344 print("KEY [2] RECEIVED!") 345 346 move_face_servos(90, 90, 90, 90, 90, 125, 55) 347 time.sleep(0.05) 348 move_face_servos(90, 90, 90, 90, 90, 115, 65) 349 time.sleep(0.05) 350 move_face_servos(90, 90, 90, 90, 90, 105, 75) 351 352 sleep(0.25) 353 354 elif str(res)[2] == 'o': 355 print("KEY [O] RECEIVED - POSITIONS RESET!") 356 357 move_eye_servos(90, 90, 90, 90) 358 move_eyelid_servos(90, 90, 90, 90) 359 move_face_servos(90, 90, 90, 90, 90, 90, 90) 360 361 sleep(0.25) 362 363 elif str(res)[2] == 'p': 364 print("Quit") 365 break 366 else: 367 print("RECEIVED UNUSUAL COMMAND: ",res) 368 369 client_socket.close() 370 server_socket.close()