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