bipedalGame_keyboard-i2c_old.py (12437B)
1 # Keyboard and i2c 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 7 # import board 8 # import busio 9 10 from time import sleep 11 12 import smbus 13 import keyboard 14 import os 15 16 import nanoik_v2 as nanoik 17 import multibyte_i2c as mb_i2c 18 19 # Nvidia Jetson Nano i2c Bus 0 and 1 20 21 # bus = smbus.SMBus(1) 22 23 # i2c_bus0 = (busio.I2C(board.SCL, board.SDA)) 24 # i2c_bus1 = (busio.I2C(board.SCL, board.SDA)) 25 26 # These are the adresses we setup in the Arduino Program 27 n17_waist_address = 0x10 28 29 # n17_sRockL1_address = 0x11 30 # n17_sRockR1_address = 0x12 31 32 n23_thighL1_address = 0x13 33 n23_thighR1_address = 0x14 34 35 n23_kneeL1_address = 0x15 36 n23_kneeR1_address = 0x16 37 38 n23_legL1_address = 0x17 39 n23_legR1_address = 0x18 40 41 n17_sRockL2_address = 0x19 42 n17_sRockR2_address = 0x20 43 44 # Our global variables 45 previous_menu = 0 46 menu = 0 47 48 limb = 0 49 link_1 = 0 50 link_2 = 0 51 52 hypotenuse = 0 53 foot_dist = 0 54 55 ee_zL = 0 56 ee_xL = 0.001 57 58 ee_zR = 0 59 ee_xR = 0.001 60 61 ee_zL_sr = 0 62 ee_zR_sr = 0 63 64 gbx_L1 = 100 65 gbx_L2 = 100 66 gbx_L3 = 100 67 68 gbx_R1 = 100 69 gbx_R2 = 100 70 gbx_R3 = 100 71 72 gbx_sr_all = 100 73 74 # gbx_sr_L1 = 100 75 # gbx_sr_L2 = 100 76 77 # gbx_sr_R1 = 100 78 # gbx_sr_R2 = 100 79 80 gbx_waist = 100 81 82 ra1L_old = 0 83 ra2L_old = 0 84 ra3L_old = 0 85 86 ra1R_old = 0 87 ra2R_old = 0 88 ra3R_old = 0 89 90 on_startup = True 91 92 while True: 93 if menu == 0: 94 os.system('clear') 95 limb = 0 96 link_1 = 0 97 link_2 = 0 98 99 hypotenuse = 0 100 ee_z = 0 101 ee_x = 0.01 102 103 print("Welcome to SylvieOS 2020!") 104 print("Make sure both of your robot legs are in line and parallel to each other.") 105 print("") 106 print("Protip: 1 meter = 1, 50 centimeters = 0.5. Measure joints from the pivot point/center.") 107 108 hypotenuse = float(input("Enter the distance between Joint 1 and Joint 3 i.e. Hypotenuse: ")) 109 foot_dist = float(input("Enter the distance between the two foot gearboxes: ")) 110 link_1 = float(input("Enter the distance between Joint 1 and Joint 2 i.e. Thigh length: ")) 111 link_2 = float(input("Enter the distance between Joint 2 and Joint 3 i.e. Leg length: ")) 112 113 if hypotenuse > 0: 114 ee_zL = hypotenuse 115 ee_zR = hypotenuse 116 ee_zL_sr = hypotenuse 117 ee_zR_sr = hypotenuse 118 menu = 1 119 120 elif menu == 1: 121 solvedik_left = nanoik.solveKinematicsSide(ee_zL, ee_xL, link_1, link_2) 122 solvedik_right = nanoik.solveKinematicsSide(ee_zR, ee_xR, link_1, link_2) 123 # solvedik_front = nanoik.solveKinematicsFront(ee_zL, ee_zR, foot_dist) 124 125 if menu != previous_menu: 126 if on_startup == False: 127 gbx_L1 = gbx_L1 + (solvedik_left[0] - ra1L_old) 128 gbx_L2 = gbx_L2 + (ra2L_old - solvedik_left[1]) 129 gbx_L3 = gbx_L3 + (solvedik_left[2] - ra3L_old) 130 131 gbx_R1 = gbx_R1 + (ra1R_old - solvedik_right[0]) 132 gbx_R2 = gbx_R2 + (ra2R_old - solvedik_right[1]) 133 gbx_R3 = gbx_R3 + (ra3R_old - solvedik_right[2]) 134 135 #mb_i2c.writeToBytes(1, n23_thighL1_address, str(int(round((gbx_L1 * 10), 1)))) 136 #mb_i2c.writeToBytes(1, n23_kneeL1_address, str(int(round((gbx_L2 * 10), 1)))) 137 #mb_i2c.writeToBytes(1, n23_legL1_address, str(int(round((gbx_L3 * 10), 1)))) 138 139 #mb_i2c.writeToBytes(1, n23_thighR1_address, str(int(round((gbx_R1 * 10), 1)))) 140 #mb_i2c.writeToBytes(1, n23_kneeR1_address, str(int(round((gbx_R2 * 10), 1)))) 141 #mb_i2c.writeToBytes(1, n23_legR1_address, str(int(round((gbx_R3 * 10), 1)))) 142 143 #mb_i2c.writeToBytes(1, n17_sRockL2_address, str(int(round((gbx_sr_L2 * 10), 1)))) 144 #mb_i2c.writeToBytes(1, n17_sRockR2_address, str(int(round((gbx_sr_R2 * 10), 1)))) 145 146 #mb_i2c.writeToBytes(1, n17_waist_address, str(int(round((gbx_waist * 10), 1)))) 147 148 sleep(0.25) 149 150 os.system('clear') 151 152 print("Current left leg angles: ", str(int(round((gbx_L1 * 10), 1))), str(int(round((gbx_L2 * 10), 1))), str(int(round((gbx_L3 * 10), 1)))) 153 print("Current right leg angles: ", str(int(round((gbx_R1 * 10), 1))), str(int(round((gbx_R2 * 10), 1))), str(int(round((gbx_R3 * 10), 1)))) 154 print("") 155 print("Left Leg End Effector position: ", round(ee_zL, 3), round(ee_xL, 3)) 156 print("Right Leg End Effector position: ", round(ee_zR, 3), round(ee_xR, 3)) 157 print("") 158 print("Current rocking joints angles: ", str(int(round((gbx_sr_all * 10), 1)))) 159 print("Current waist joint angle: ", str(int(round((gbx_waist * 10), 1)))) 160 print("") 161 print("Use WASD,TFGH,IJKL,ZX,CV to move the lower body.") 162 163 previous_menu = menu 164 165 ra1L_old = solvedik_left[0] 166 ra2L_old = solvedik_left[1] 167 ra3L_old = solvedik_left[2] 168 169 ra1R_old = solvedik_right[0] 170 ra2R_old = solvedik_right[1] 171 ra3R_old = solvedik_right[2] 172 173 if keyboard.is_pressed('1'): 174 os.system('clear') 175 print("KEYBOARD KEY [1] PRESSED!") 176 177 sleep(0.125) 178 179 nanoik.drawRadarSide(solvedik_left[0], solvedik_left[1], solvedik_left[2], link_1, link_2, "blue") 180 181 on_startup = False 182 previous_menu = -1 183 elif keyboard.is_pressed('2'): 184 os.system('clear') 185 print("KEYBOARD KEY [2] PRESSED!") 186 187 sleep(0.125) 188 189 nanoik.drawRadarSide(solvedik_right[0], solvedik_right[1], solvedik_right[2], link_1, link_2, "red") 190 191 on_startup = False 192 previous_menu = -1 193 elif keyboard.is_pressed('3'): 194 os.system('clear') 195 print("KEYBOARD KEY [3] PRESSED!") 196 197 sleep(0.125) 198 199 nanoik.drawRadarFront((gbx_sr_all - 100), ee_zL, ee_zR, foot_dist, "green") 200 201 on_startup = False 202 previous_menu = -1 203 elif keyboard.is_pressed('w'): 204 os.system('clear') 205 print("KEYBOARD KEY [W] PRESSED!") 206 207 sleep(0.25) 208 209 ee_zL = ee_zL - 0.0025 210 211 on_startup = False 212 previous_menu = -1 213 elif keyboard.is_pressed('s'): 214 os.system('clear') 215 print("KEYBOARD KEY [S] PRESSED!") 216 217 sleep(0.25) 218 219 ee_zL = ee_zL + 0.0025 220 221 on_startup = False 222 previous_menu = -1 223 elif keyboard.is_pressed('d'): 224 os.system('clear') 225 print("KEYBOARD KEY [D] PRESSED!") 226 227 sleep(0.25) 228 229 ee_xL = ee_xL + 0.0025 230 231 on_startup = False 232 previous_menu = -1 233 elif keyboard.is_pressed('a'): 234 os.system('clear') 235 print("KEYBOARD KEY [A] PRESSED!") 236 237 sleep(0.25) 238 239 ee_xL = ee_xL - 0.0025 240 241 on_startup = False 242 previous_menu = -1 243 elif keyboard.is_pressed('t'): 244 os.system('clear') 245 print("KEYBOARD KEY [T] PRESSED!") 246 247 sleep(0.25) 248 249 ee_zR = ee_zR - 0.0025 250 251 on_startup = False 252 previous_menu = -1 253 elif keyboard.is_pressed('g'): 254 os.system('clear') 255 print("KEYBOARD KEY [G] PRESSED!") 256 257 sleep(0.25) 258 259 ee_zR = ee_zR + 0.0025 260 261 on_startup = False 262 previous_menu = -1 263 elif keyboard.is_pressed('h'): 264 os.system('clear') 265 print("KEYBOARD KEY [H] PRESSED!") 266 267 sleep(0.25) 268 269 ee_xR = ee_xR + 0.0025 270 271 on_startup = False 272 previous_menu = -1 273 elif keyboard.is_pressed('f'): 274 os.system('clear') 275 print("KEYBOARD KEY [F] PRESSED!") 276 277 sleep(0.25) 278 279 ee_xR = ee_xR - 0.0025 280 281 on_startup = False 282 previous_menu = -1 283 elif keyboard.is_pressed('i'): 284 os.system('clear') 285 print("KEYBOARD KEY [I] PRESSED!") 286 287 sleep(0.25) 288 289 ee_zL = ee_zL - 0.0025 290 ee_zR = ee_zR - 0.0025 291 292 on_startup = False 293 previous_menu = -1 294 elif keyboard.is_pressed('k'): 295 os.system('clear') 296 print("KEYBOARD KEY [K] PRESSED!") 297 298 sleep(0.25) 299 300 ee_zL = ee_zL + 0.0025 301 ee_zR = ee_zR + 0.0025 302 303 on_startup = False 304 previous_menu = -1 305 elif keyboard.is_pressed('l'): 306 os.system('clear') 307 print("KEYBOARD KEY [L] PRESSED!") 308 309 sleep(0.25) 310 311 ee_xL = ee_xL + 0.0025 312 ee_xR = ee_xR + 0.0025 313 314 on_startup = False 315 previous_menu = -1 316 elif keyboard.is_pressed('j'): 317 os.system('clear') 318 print("KEYBOARD KEY [J] PRESSED!") 319 320 sleep(0.25) 321 322 ee_xL = ee_xL - 0.0025 323 ee_xR = ee_xR - 0.0025 324 325 on_startup = False 326 previous_menu = -1 327 elif keyboard.is_pressed('z'): 328 os.system('clear') 329 print("KEYBOARD KEY [Z] PRESSED!") 330 331 sleep(0.25) 332 333 ee_zL = ee_zL - 0.0025 334 ee_zR = ee_zR + 0.0025 335 336 ee_zL_sr = ee_zL_sr - 0.0025 337 ee_zR_sr = ee_zR_sr + 0.0025 338 339 gbx_sr_all = 100 + nanoik.solveKinematicsFront(ee_zL_sr, ee_zR_sr, foot_dist) 340 341 on_startup = False 342 previous_menu = -1 343 elif keyboard.is_pressed('x'): 344 os.system('clear') 345 print("KEYBOARD KEY [X] PRESSED!") 346 347 sleep(0.25) 348 349 ee_zL = ee_zL + 0.0025 350 ee_zR = ee_zR - 0.0025 351 352 ee_zL_sr = ee_zL_sr + 0.0025 353 ee_zR_sr = ee_zR_sr - 0.0025 354 355 gbx_sr_all = 100 + nanoik.solveKinematicsFront(ee_zL_sr, ee_zR_sr, foot_dist) 356 357 on_startup = False 358 previous_menu = -1 359 elif keyboard.is_pressed('c'): 360 os.system('clear') 361 print("KEYBOARD KEY [C] PRESSED!") 362 363 sleep(0.25) 364 365 gbx_waist = gbx_waist + 0.5 366 367 on_startup = False 368 previous_menu = -1 369 elif keyboard.is_pressed('v'): 370 os.system('clear') 371 print("KEYBOARD KEY [V] PRESSED!") 372 373 sleep(0.25) 374 375 gbx_waist = gbx_waist - 0.5 376 377 on_startup = False 378 previous_menu = -1 379 elif keyboard.is_pressed('4'): 380 os.system('clear') 381 print("KEYBOARD KEY [4] PRESSED!") 382 383 sleep(0.25) 384 385 mb_i2c.writeToBytes(1, n23_thighL1_address, "2002") 386 mb_i2c.writeToBytes(1, n23_thighR1_address, "2003") 387 388 on_startup = False 389 previous_menu = -1 390 elif keyboard.is_pressed('5'): 391 os.system('clear') 392 print("KEYBOARD KEY [5] PRESSED!") 393 394 sleep(0.25) 395 396 mb_i2c.writeToBytes(1, n23_thighL1_address, "2003") 397 mb_i2c.writeToBytes(1, n23_thighR1_address, "2002") 398 399 on_startup = False 400 previous_menu = -1 401 elif keyboard.is_pressed('6'): 402 os.system('clear') 403 print("KEYBOARD KEY [6] PRESSED!") 404 405 sleep(0.25) 406 407 mb_i2c.writeToBytes(1, n23_kneeL1_address, "2002") 408 mb_i2c.writeToBytes(1, n23_kneeR1_address, "2002") 409 410 on_startup = False 411 previous_menu = -1 412 elif keyboard.is_pressed('7'): 413 os.system('clear') 414 print("KEYBOARD KEY [7] PRESSED!") 415 416 sleep(0.25) 417 418 mb_i2c.writeToBytes(1, n23_kneeL1_address, "2003") 419 mb_i2c.writeToBytes(1, n23_kneeR1_address, "2003") 420 421 on_startup = False 422 previous_menu = -1 423 elif keyboard.is_pressed('8'): 424 os.system('clear') 425 print("KEYBOARD KEY [8] PRESSED!") 426 427 sleep(0.25) 428 429 mb_i2c.writeToBytes(1, n23_legL1_address, "2002") 430 mb_i2c.writeToBytes(1, n23_legR1_address, "2003") 431 432 on_startup = False 433 previous_menu = -1 434 elif keyboard.is_pressed('9'): 435 os.system('clear') 436 print("KEYBOARD KEY [9] PRESSED!") 437 438 sleep(0.25) 439 440 mb_i2c.writeToBytes(1, n23_legL1_address, "2003") 441 mb_i2c.writeToBytes(1, n23_legR1_address, "2002") 442 443 on_startup = False 444 previous_menu = -1