bipedalGame_keyb-serial.py (26184B)
1 # PC Keyboard and Serial control for Sylvie 2021 (Bipedal Robot Lower Body) 2 3 import sys 4 # insert at 1, 0 is the script path (or '' in REPL) 5 sys.path.insert(1, '../inverse_kinematics') 6 sys.path.insert(1, '../registry') 7 8 from time import sleep 9 import serial 10 import keyboard 11 import os 12 13 import nanoik_v2 as nanoik 14 import bipedalGame_data as bipedalGame 15 import sylvieos_data as sylvieOS 16 17 ser = serial.Serial('/dev/ttyUSB0', 115200) 18 19 # Our global variables 20 previous_menu = 0 21 menu = 0 22 encoded_command = "" 23 24 link_1 = 33.4 # Default thigh length 25 link_2 = 30.5 # Default leg length 26 27 hypotenuse = 62.7 # Default hypotenuse after alignment 28 foot_dist = 14.3 # Default distance between foot gearboxes 29 30 ee_zL = 0 31 ee_xL = 0.001 32 33 ee_zR = 0 34 ee_xR = 0.001 35 36 ee_zL_sr = 0 37 ee_zR_sr = 0 38 39 speed = { 40 "ver": [0.0, 0.25, 0.5, 1.5, 2.0, 2.5, 3.0], 41 "hor": [0.0, 0.5, 1.0, 2.0, 3.0, 4.0, 5.0], 42 "sr": [0.0, 0.05, 0.1, 0.2, 0.3, 0.4, 0.5], 43 "angle": [0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 5.0] 44 } 45 46 default_speed = [0, 50, 45, 40, 35, 30, 25, 20, 10] 47 balance_pid = [24.0, 8.0, 0.01] 48 49 gbx = { 50 "name": ["0", "1", "2", "3", "4", "5", "6", "7", "8", "9", "a"], 51 "angle": [17.0, 0.0, 0.0, 102.48, 102.48, 10.07, 10.07, 47.55, 47.55, 20.0, 20.0], 52 "old_angle": [17.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 53 "offset": [0.0, 0.0, 0.0, -70.0, -70.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 54 "old_offset": [0.0, 0.0, 0.0, -70.0, -70.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 55 "dir": [True, True, True, False, False, True, True, True, True, False, True], 56 "balancing": [False, False, False, False, False, False, False, False, False, False, False], 57 "solvedik": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 58 } 59 60 on_startup = True 61 speed_setting = 1 62 d_speed_setting = 3 63 64 # Quick rounder and stringifyer 65 66 def quickRnd(val, check_plus=False): 67 val_pt1 = str(float(round(val, 2))) 68 69 newVal = val_pt1 70 71 if check_plus == True: 72 if val > 0: 73 newVal = "+" + val_pt1 74 75 return newVal 76 77 # Reset menu 78 79 def reset_menu(): 80 global on_startup 81 global previous_menu 82 83 on_startup = False 84 previous_menu = -1 85 86 # Show key pressed 87 88 def show_key(keyboard_key): 89 os.system('clear') 90 print("KEYBOARD KEY [" + keyboard_key + "] PRESSED") 91 92 sleep(0.25) 93 reset_menu() 94 95 # Apply forward kinematics, and show key pressed 96 97 def fwd_key(name, keyboard_key): 98 os.system('clear') 99 print("KEYBOARD KEY [" + keyboard_key + "] PRESSED") 100 full_angle = gbx["angle"][name] + gbx["offset"][name] 101 102 if gbx["balancing"][name] == False: 103 gbx_n = gbx["name"][name] 104 gbx_s = str(default_speed[d_speed_setting]) 105 gbx_a = quickRnd(full_angle) 106 107 quick_command = gbx_n + "s" + gbx_s + ";" + gbx_n + "m" + gbx_a + "\n" 108 ser.write(quick_command.encode("utf-8")) 109 110 gbx["old_offset"][name] = gbx["offset"][name] 111 112 # print("ENCODED COMMAND: ", quick_command) # Debug only 113 # print("NEW OFFSET: ", str(gbx["offset"][name])) # Debug only 114 115 sleep(0.25) 116 # sleep(1) # Debug only 117 reset_menu() 118 119 # Toggle joint movement angles per key press 120 121 def switch_speed(dial, n_type=1): 122 os.system('clear') 123 n_max = [0, 6, 8] 124 global speed_setting 125 global d_speed_setting 126 c_val = [0, speed_setting, d_speed_setting] 127 c_string = ["", "ANGLE SPEED", "MOVEMENT SPEED"] 128 129 if dial == "up": 130 if c_val[n_type] < n_max[n_type]: 131 c_val[n_type] += 1 132 elif dial == "down": 133 if c_val[n_type] > 1: 134 c_val[n_type] -= 1 135 136 speed_setting = c_val[n_type] if n_type == 1 else speed_setting 137 d_speed_setting = c_val[n_type] if n_type == 2 else d_speed_setting 138 print(c_string[n_type] + " SET TO [" + str(c_val[n_type]) + "]") 139 140 sleep(0.25) 141 reset_menu() 142 143 def switch_pid(dial, n_type=0): 144 os.system('clear') 145 d_value = [0.0, 0.0, 0.0] 146 147 if dial == "up": 148 d_value = [0.1, 0.1, 0.01] 149 elif dial == "down": 150 d_value = [-0.1, -0.1, -0.01] 151 152 balance_pid[n_type] += d_value[n_type] 153 154 d_letter = ["p", "i", "d"] 155 print("PID k" + d_letter[n_type] + " SET TO [" + quickRnd(balance_pid[n_type]) + "]") 156 157 quick_command = "3" + d_letter[n_type] + quickRnd(balance_pid[n_type]) 158 quick_command += ";4" + d_letter[n_type] + quickRnd(balance_pid[n_type]) 159 quick_command += ";0" + d_letter[n_type] + quickRnd(balance_pid[n_type]) + "\n" 160 161 # print("ENCODED COMMAND: ", quick_command) 162 163 ser.write(quick_command.encode("utf-8")) 164 165 sleep(0.25) 166 # sleep(3) # debug only 167 reset_menu() 168 169 # Main loop 170 171 while True: 172 173 if menu == 0: 174 os.system('clear') 175 176 ee_z = 0 177 ee_x = 0.01 178 179 sylvieOS.splash() 180 181 homing_required = str(input("Enter servo homing menu? [y/n]: ")) 182 183 if homing_required == "y": 184 os.system('clear') 185 print("Warning: Please ensure that both legs are as aligned as possible!") 186 187 homing_q1 = str(input("Home the thighs? [y/n]: ")) 188 if homing_q1 == "y": 189 ser.write("3h;4h\n".encode("utf-8")) 190 print("Waiting for thighs homing...") 191 sleep(10) 192 ser.write("3m32.48;4m32.48\n".encode("utf-8")) 193 print("Waiting for thighs start position...") 194 sleep(5) 195 196 homing_q2 = str(input("Home the knees? [y/n]: ")) 197 if homing_q2 == "y": 198 ser.write("5h;6h\n".encode("utf-8")) 199 print("Waiting for knees homing...") 200 sleep(10) 201 ser.write("5m10.07;6m10.07\n".encode("utf-8")) 202 print("Waiting for knees start position...") 203 sleep(5) 204 205 homing_q3 = str(input("Home the legs? [y/n]: ")) 206 if homing_q3 == "y": 207 ser.write("7h;8h\n".encode("utf-8")) 208 print("Waiting for legs homing...") 209 sleep(15) 210 ser.write("7m47.55;8m47.55\n".encode("utf-8")) 211 print("Waiting for legs start position...") 212 sleep(5) 213 214 homing_q4 = str(input("Home the right foot? [y/n]: ")) 215 if homing_q4 == "y": 216 ser.write("9h\n".encode("utf-8")) 217 print("Waiting for right foot homing...") 218 sleep(10) 219 ser.write("9m20\n".encode("utf-8")) 220 print("Waiting for right foot start position...") 221 sleep(5) 222 223 homing_q5 = str(input("Home the left foot? [y/n]: ")) 224 if homing_q5 == "y": 225 ser.write("ah\n".encode("utf-8")) 226 print("Waiting for left foot homing...") 227 sleep(10) 228 ser.write("am20\n".encode("utf-8")) 229 print("Waiting for left foot start position...") 230 sleep(5) 231 232 homing_q6 = str(input("Home the waist? [y/n]: ")) 233 if homing_q6 == "y": 234 ser.write("0h\n".encode("utf-8")) 235 print("Waiting for waist homing...") 236 sleep(10) 237 ser.write("0m17\n".encode("utf-8")) 238 print("Waiting for waist start position...") 239 sleep(5) 240 241 os.system('clear') 242 print("Use Keys 4 and 5 to straighten up or lower the android torso.") 243 sleep(5) 244 else: 245 os.system('clear') 246 print("Warning: It is not recommended to use Inverse Kinematics without homing.") 247 menu_q1 = str(input("Stick to Forward Kinematics. Press 'y' to continue or 'n' to exit [y/n]: ")) 248 if menu_q1 == "y": 249 for i in range(0, 11): 250 gbx["angle"][i] = 0.0 251 gbx["old_angle"][i] = 0.0 252 gbx["offset"][i] = 0.0 253 gbx["old_offset"][i] = 0.0 254 else: 255 exit() 256 257 ee_zL = hypotenuse 258 ee_zR = hypotenuse 259 ee_zL_sr = hypotenuse 260 ee_zR_sr = hypotenuse 261 menu = 1 262 263 elif menu == 1: 264 solvedik_left = nanoik.solveKinematicsSide(ee_zL, ee_xL, link_1, link_2) 265 solvedik_right = nanoik.solveKinematicsSide(ee_zR, ee_xR, link_1, link_2) 266 solvedik_front = nanoik.solveKinematicsFront(ee_zL_sr, ee_zR_sr, foot_dist) 267 268 gbx["solvedik"][3] = solvedik_right[0] 269 gbx["solvedik"][4] = solvedik_left[0] 270 gbx["solvedik"][5] = solvedik_right[1] 271 gbx["solvedik"][6] = solvedik_left[1] 272 gbx["solvedik"][7] = solvedik_right[2] 273 gbx["solvedik"][8] = solvedik_left[2] 274 gbx["solvedik"][9] = solvedik_front 275 gbx["solvedik"][10] = solvedik_front 276 277 # Handle startup and post-startup calculations 278 279 if menu != previous_menu: 280 281 if on_startup == True: 282 for i in range(3, 11): 283 gbx["old_angle"][i] = gbx["solvedik"][i] 284 else: 285 encoded_command = "" 286 new_speed = default_speed[d_speed_setting] 287 i_diff = -1 288 i_char = "" 289 290 for i in range(3, 11): 291 gbx_diff = 0.0 292 if gbx["dir"][i] == True: 293 gbx_diff = gbx["solvedik"][i] - gbx["old_angle"][i] 294 else: 295 gbx_diff = gbx["old_angle"][i] - gbx["solvedik"][i] 296 297 gbx["angle"][i] += gbx_diff 298 full_angle = gbx["angle"][i] + gbx["offset"][i] 299 300 if gbx_diff != 0.0 or gbx["offset"][i] != gbx["old_offset"][i]: 301 if i_diff == -1: 302 i_diff = abs(gbx_diff) 303 else: 304 if abs(gbx_diff) > 0: 305 new_speed = (i_diff / abs(gbx_diff)) * default_speed[d_speed_setting] 306 if new_speed < 10: 307 new_speed = 10 308 elif new_speed > 70: 309 new_speed = 70 310 311 if gbx["balancing"][i] == False: 312 gbx_n = gbx["name"][i] 313 gbx_s = str(int(new_speed)) 314 gbx_a = quickRnd(full_angle) 315 316 encoded_command += i_char + gbx_n + "s" + gbx_s + ";" + gbx_n + "m" + gbx_a 317 318 i_char = ";" 319 320 gbx["old_angle"][i] = gbx["solvedik"][i] 321 gbx["old_offset"][i] = gbx["offset"][i] 322 323 encoded_command += "\n" # Add \n terminator after the loop 324 ser.write(encoded_command.encode("utf-8")) 325 326 sleep(0.25) 327 328 os.system('clear') 329 330 # Print joint angles on screen 331 332 r_leg_angles = [quickRnd(gbx["angle"][3]), quickRnd(gbx["angle"][5]), quickRnd(gbx["angle"][7])] 333 l_leg_angles = [quickRnd(gbx["angle"][4]), quickRnd(gbx["angle"][6]), quickRnd(gbx["angle"][8])] 334 335 r_leg_offsets = [quickRnd(gbx["offset"][3], True), quickRnd(gbx["offset"][5], True), quickRnd(gbx["offset"][7], True)] 336 l_leg_offsets = [quickRnd(gbx["offset"][4], True), quickRnd(gbx["offset"][6], True), quickRnd(gbx["offset"][8], True)] 337 338 r_ee_pos = round(ee_zR, 3), round(ee_xR, 3) 339 l_ee_pos = round(ee_zL, 3), round(ee_xL, 3) 340 341 sr_angles = [quickRnd(gbx["angle"][1]), quickRnd(gbx["angle"][2]), quickRnd(gbx["angle"][9]), quickRnd(gbx["angle"][10])] 342 sr_offsets = [quickRnd(gbx["offset"][1], True), quickRnd(gbx["offset"][2], True), quickRnd(gbx["offset"][9], True), quickRnd(gbx["offset"][10], True)] 343 waist_angle = quickRnd(gbx["angle"][0]) 344 waist_offset = quickRnd(gbx["offset"][0], True) 345 346 bipedalGame.menuOneText(l_leg_angles, l_leg_offsets, r_leg_angles, r_leg_offsets, l_ee_pos, r_ee_pos, sr_angles, sr_offsets, waist_angle, waist_offset, encoded_command) 347 348 previous_menu = menu 349 350 # Keyboard Control 351 352 if keyboard.is_pressed('1'): 353 nanoik.drawRadarSide(solvedik_left[0], solvedik_left[1], solvedik_left[2], link_1, link_2, "blue") 354 show_key('1') 355 elif keyboard.is_pressed('2'): 356 nanoik.drawRadarSide(solvedik_right[0], solvedik_right[1], solvedik_right[2], link_1, link_2, "red") 357 show_key('2') 358 elif keyboard.is_pressed('3'): 359 nanoik.drawRadarFront((gbx["angle"][10] + 70), ee_zL, ee_zR, foot_dist, "green") 360 show_key('3') 361 elif keyboard.is_pressed('w'): 362 ee_zL = ee_zL - speed["ver"][speed_setting] 363 show_key('w') 364 elif keyboard.is_pressed('s'): 365 ee_zL = ee_zL + speed["ver"][speed_setting] 366 show_key('s') 367 elif keyboard.is_pressed('d'): 368 ee_xL = ee_xL + speed["hor"][speed_setting] 369 show_key('d') 370 elif keyboard.is_pressed('a'): 371 ee_xL = ee_xL - speed["hor"][speed_setting] 372 show_key('a') 373 elif keyboard.is_pressed('t'): 374 ee_zR = ee_zR - speed["ver"][speed_setting] 375 show_key('t') 376 elif keyboard.is_pressed('g'): 377 ee_zR = ee_zR + speed["ver"][speed_setting] 378 show_key('g') 379 elif keyboard.is_pressed('h'): 380 ee_xR = ee_xR + speed["hor"][speed_setting] 381 show_key('h') 382 elif keyboard.is_pressed('f'): 383 ee_xR = ee_xR - speed["hor"][speed_setting] 384 show_key('f') 385 elif keyboard.is_pressed('i'): 386 ee_zL = ee_zL - speed["ver"][speed_setting] 387 ee_zR = ee_zR - speed["ver"][speed_setting] 388 show_key('i') 389 elif keyboard.is_pressed('k'): 390 ee_zL = ee_zL + speed["ver"][speed_setting] 391 ee_zR = ee_zR + speed["ver"][speed_setting] 392 show_key('k') 393 elif keyboard.is_pressed('l'): 394 ee_xL = ee_xL + speed["hor"][speed_setting] 395 ee_xR = ee_xR + speed["hor"][speed_setting] 396 show_key('l') 397 elif keyboard.is_pressed('j'): 398 ee_xL = ee_xL - speed["hor"][speed_setting] 399 ee_xR = ee_xR - speed["hor"][speed_setting] 400 show_key('j') 401 402 elif keyboard.is_pressed('z'): 403 ee_zL = ee_zL - speed["sr"][speed_setting] 404 ee_zR = ee_zR + speed["sr"][speed_setting] 405 406 ee_zL_sr = ee_zL_sr - speed["sr"][speed_setting] 407 ee_zR_sr = ee_zR_sr + speed["sr"][speed_setting] 408 409 show_key('z') 410 elif keyboard.is_pressed('x'): 411 ee_zL = ee_zL + speed["sr"][speed_setting] 412 ee_zR = ee_zR - speed["sr"][speed_setting] 413 414 ee_zL_sr = ee_zL_sr + speed["sr"][speed_setting] 415 ee_zR_sr = ee_zR_sr - speed["sr"][speed_setting] 416 417 show_key('x') 418 419 elif keyboard.is_pressed('c'): 420 gbx["angle"][0] += speed["angle"][speed_setting] 421 if gbx["balancing"][0] == False: 422 quick_command = "0m" + quickRnd(gbx["angle"][0]) + "\n" 423 ser.write(quick_command.encode("utf-8")) 424 show_key('c') 425 elif keyboard.is_pressed('v'): 426 gbx["angle"][0] -= speed["angle"][speed_setting] 427 if gbx["balancing"][0] == False: 428 quick_command = "0m" + quickRnd(gbx["angle"][0]) + "\n" 429 ser.write(quick_command.encode("utf-8")) 430 show_key('v') 431 elif keyboard.is_pressed('b'): 432 gbx["angle"][1] += speed["angle"][speed_setting] 433 gbx["angle"][2] -= speed["angle"][speed_setting] 434 gbx["offset"][9] -= speed["angle"][speed_setting] 435 gbx["offset"][10] += speed["angle"][speed_setting] 436 437 full_angle_9 = gbx["angle"][9] + gbx["offset"][9] 438 full_angle_10 = gbx["angle"][10] + gbx["offset"][10] 439 440 gbx["old_offset"][9] = gbx["offset"][9] 441 gbx["old_offset"][10] = gbx["offset"][10] 442 443 quick_command = "1m" + quickRnd(gbx["angle"][1]) + ";2m" + quickRnd(gbx["angle"][2]) 444 quick_command += ";9m" + quickRnd(full_angle_9) + ";am" + quickRnd(full_angle_10) + "\n" 445 446 show_key('b') 447 ser.write(quick_command.encode("utf-8")) 448 elif keyboard.is_pressed('n'): 449 gbx["angle"][1] -= speed["angle"][speed_setting] 450 gbx["angle"][2] += speed["angle"][speed_setting] 451 gbx["offset"][9] += speed["angle"][speed_setting] 452 gbx["offset"][10] -= speed["angle"][speed_setting] 453 454 full_angle_9 = gbx["angle"][9] + gbx["offset"][9] 455 full_angle_10 = gbx["angle"][10] + gbx["offset"][10] 456 457 gbx["old_offset"][9] = gbx["offset"][9] 458 gbx["old_offset"][10] = gbx["offset"][10] 459 460 quick_command = "1m" + quickRnd(gbx["angle"][1]) + ";2m" + quickRnd(gbx["angle"][2]) 461 quick_command += ";9m" + quickRnd(full_angle_9) + ";am" + quickRnd(full_angle_10) + "\n" 462 463 show_key('n') 464 ser.write(quick_command.encode("utf-8")) 465 466 elif keyboard.is_pressed('4'): 467 gbx["offset"][3] += speed["angle"][speed_setting] 468 gbx["offset"][4] += speed["angle"][speed_setting] 469 show_key('4') 470 elif keyboard.is_pressed('5'): 471 gbx["offset"][3] -= speed["angle"][speed_setting] 472 gbx["offset"][4] -= speed["angle"][speed_setting] 473 show_key('5') 474 elif keyboard.is_pressed('6'): 475 gbx["offset"][5] += speed["angle"][speed_setting] 476 gbx["offset"][6] += speed["angle"][speed_setting] 477 show_key('6') 478 elif keyboard.is_pressed('7'): 479 gbx["offset"][5] -= speed["angle"][speed_setting] 480 gbx["offset"][6] -= speed["angle"][speed_setting] 481 show_key('7') 482 elif keyboard.is_pressed('8'): 483 gbx["offset"][7] += speed["angle"][speed_setting] 484 gbx["offset"][8] += speed["angle"][speed_setting] 485 show_key('8') 486 elif keyboard.is_pressed('9'): 487 gbx["offset"][7] -= speed["angle"][speed_setting] 488 gbx["offset"][8] -= speed["angle"][speed_setting] 489 show_key('9') 490 491 # Arrow keys 492 493 elif keyboard.is_pressed('up'): 494 switch_speed('up') 495 elif keyboard.is_pressed('down'): 496 switch_speed('down') 497 elif keyboard.is_pressed('o'): 498 switch_speed('up', 2) 499 elif keyboard.is_pressed('p'): 500 switch_speed('down', 2) 501 502 elif keyboard.is_pressed('right'): 503 os.system('clear') 504 print("NAVIGATING TO MENU 2") 505 506 sleep(1) 507 508 on_startup = False 509 menu = 2 510 previous_menu 511 512 elif menu == 2: 513 if menu != previous_menu: 514 os.system('clear') 515 print("Forward Kinematics and Balancing Menu") 516 print("This is where gearboxes can be adjusted one at a time") 517 print("Press Q,W,E,R,T,Y,A,S,D,F,G,H,etc. to move one servo at a time") 518 print("Press 1,2 to enter or exit balance mode for thighs") 519 print("Press 3,4 to enter or exit balance mode for waist") 520 print("") 521 print("PID is: (" + quickRnd(balance_pid[0]) + ", " + quickRnd(balance_pid[1]) + ", " + quickRnd(balance_pid[2]) + ")") 522 523 previous_menu = menu 524 525 # Left leg 526 527 if keyboard.is_pressed('q'): 528 gbx["offset"][4] += speed["angle"][speed_setting] 529 fwd_key(4, 'q') 530 elif keyboard.is_pressed('w'): 531 gbx["offset"][4] -= speed["angle"][speed_setting] 532 fwd_key(4, 'w') 533 elif keyboard.is_pressed('e'): 534 gbx["offset"][6] += speed["angle"][speed_setting] 535 fwd_key(6, 'e') 536 elif keyboard.is_pressed('r'): 537 gbx["offset"][6] -= speed["angle"][speed_setting] 538 fwd_key(6, 'r') 539 elif keyboard.is_pressed('t'): 540 gbx["offset"][8] += speed["angle"][speed_setting] 541 fwd_key(8, 't') 542 elif keyboard.is_pressed('y'): 543 gbx["offset"][8] -= speed["angle"][speed_setting] 544 fwd_key(8, 'y') 545 546 # Right leg 547 548 elif keyboard.is_pressed('a'): 549 gbx["offset"][3] += speed["angle"][speed_setting] 550 fwd_key(3, 'a') 551 elif keyboard.is_pressed('s'): 552 gbx["offset"][3] -= speed["angle"][speed_setting] 553 fwd_key(3, 's') 554 elif keyboard.is_pressed('d'): 555 gbx["offset"][5] += speed["angle"][speed_setting] 556 fwd_key(5, 'd') 557 elif keyboard.is_pressed('f'): 558 gbx["offset"][5] -= speed["angle"][speed_setting] 559 fwd_key(5, 'f') 560 elif keyboard.is_pressed('g'): 561 gbx["offset"][7] += speed["angle"][speed_setting] 562 fwd_key(7, 'g') 563 elif keyboard.is_pressed('h'): 564 gbx["offset"][7] -= speed["angle"][speed_setting] 565 fwd_key(7, 'h') 566 567 # Hips 568 569 elif keyboard.is_pressed('u'): 570 gbx["angle"][1] += speed["angle"][speed_setting] 571 quick_command = "1m" + str(gbx["angle"][1]) + "\n" 572 ser.write(quick_command.encode("utf-8")) 573 show_key('u') 574 elif keyboard.is_pressed('i'): 575 gbx["angle"][1] -= speed["angle"][speed_setting] 576 quick_command = "1m" + str(gbx["angle"][1]) + "\n" 577 ser.write(quick_command.encode("utf-8")) 578 show_key('i') 579 elif keyboard.is_pressed('z'): 580 gbx["angle"][2] += speed["angle"][speed_setting] 581 quick_command = "2m" + str(gbx["angle"][2]) + "\n" 582 ser.write(quick_command.encode("utf-8")) 583 show_key('z') 584 elif keyboard.is_pressed('x'): 585 gbx["angle"][2] -= speed["angle"][speed_setting] 586 quick_command = "2m" + str(gbx["angle"][2]) + "\n" 587 ser.write(quick_command.encode("utf-8")) 588 show_key('x') 589 590 # Feet 591 592 elif keyboard.is_pressed('c'): 593 gbx["offset"][9] += speed["angle"][speed_setting] 594 fwd_key(9, 'c') 595 elif keyboard.is_pressed('v'): 596 gbx["offset"][9] -= speed["angle"][speed_setting] 597 fwd_key(9, 'v') 598 elif keyboard.is_pressed('b'): 599 gbx["offset"][10] += speed["angle"][speed_setting] 600 fwd_key(10, 'b') 601 elif keyboard.is_pressed('n'): 602 gbx["offset"][10] -= speed["angle"][speed_setting] 603 fwd_key(10, 'n') 604 605 # Balance 606 607 elif keyboard.is_pressed('1'): 608 quick_command = "3b;4b\n" 609 gbx["balancing"][3] = True 610 gbx["balancing"][4] = True 611 show_key('1') 612 ser.write(quick_command.encode("utf-8")) 613 elif keyboard.is_pressed('2'): 614 quick_command = "3x;4x\n" 615 gbx["balancing"][3] = False 616 gbx["balancing"][4] = False 617 show_key('2') 618 ser.write(quick_command.encode("utf-8")) 619 elif keyboard.is_pressed('3'): 620 quick_command = "0b\n" 621 gbx["balancing"][0] = True 622 show_key('3') 623 ser.write(quick_command.encode("utf-8")) 624 elif keyboard.is_pressed('4'): 625 quick_command = "0x\n" 626 gbx["balancing"][0] = False 627 show_key('4') 628 ser.write(quick_command.encode("utf-8")) 629 elif keyboard.is_pressed('5'): 630 quick_command = "3l15;3n45;4l15;4n45;0l15;0n45\n" 631 show_key('5') 632 ser.write(quick_command.encode("utf-8")) 633 elif keyboard.is_pressed('6'): 634 quick_command = "3l30;3n60;4l30;4n60;0l30;0n60\n" 635 show_key('6') 636 ser.write(quick_command.encode("utf-8")) 637 elif keyboard.is_pressed('7'): 638 quick_command = "3l40;3n80;4l40;4n80;0l40;0n80\n" 639 show_key('7') 640 ser.write(quick_command.encode("utf-8")) 641 elif keyboard.is_pressed('8'): 642 quick_command = "3l60;3n90;4l60;4n90;0l60;0n90\n" 643 show_key('8') 644 ser.write(quick_command.encode("utf-8")) 645 elif keyboard.is_pressed('9'): 646 switch_pid('up', 0) 647 elif keyboard.is_pressed('0'): 648 switch_pid('down', 0) 649 elif keyboard.is_pressed('o'): 650 switch_pid('up', 1) 651 elif keyboard.is_pressed('p'): 652 switch_pid('down', 1) 653 elif keyboard.is_pressed('l'): 654 switch_pid('up', 2) 655 elif keyboard.is_pressed('m'): 656 switch_pid('down', 2) 657 658 # Arrow keys 659 660 elif keyboard.is_pressed('up'): 661 switch_speed('up') 662 elif keyboard.is_pressed('down'): 663 switch_speed('down') 664 elif keyboard.is_pressed('o'): 665 switch_speed('up', 2) 666 elif keyboard.is_pressed('p'): 667 switch_speed('down', 2) 668 669 elif keyboard.is_pressed('left'): 670 os.system('clear') 671 print("RETURNING TO MENU 1") 672 673 sleep(1) 674 675 on_startup = False 676 menu = 1 677 previous_menu = -1