bipedalGame_blueooth-serial.py (8783B)
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 sys.path.insert(1, '../registry') 7 8 from time import sleep 9 import serial 10 import keyboard 11 import bluetooth 12 import os 13 14 import nanoik_v2 as nanoik 15 import bipedalGame_data as bipedalGame 16 17 server_socket=bluetooth.BluetoothSocket( bluetooth.RFCOMM ) 18 port = 1 19 20 ser = serial.Serial('/dev/ttyUSB0', 9600) 21 22 # n17_sRockL1_address = 0x11 23 # n17_sRockR1_address = 0x12 24 25 # Our global variables 26 previous_menu = 0 27 menu = 0 28 encoded_command = "" 29 30 limb = 0 31 link_1 = 0 32 link_2 = 0 33 34 hypotenuse = 0 35 foot_dist = 0 36 37 ee_zL = 0 38 ee_xL = 0.001 39 40 ee_zR = 0 41 ee_xR = 0.001 42 43 gbx_L1 = 100 44 gbx_L2 = 100 45 gbx_L3 = 100 46 47 gbx_R1 = 100 48 gbx_R2 = 100 49 gbx_R3 = 100 50 51 gbx_sr_all = 100 52 53 gbx_waist = 100 54 55 ra1L_old = 0 56 ra2L_old = 0 57 ra3L_old = 0 58 59 ra1R_old = 0 60 ra2R_old = 0 61 ra3R_old = 0 62 63 on_startup = True 64 65 def quickRnd(val): 66 newVal = str(int(round((val * 10), 1))) 67 return newVal 68 69 def show_key(keyboard_key): 70 os.system('clear') 71 print("KEYBOARD KEY [" + keyboard_key + "] PRESSED") 72 73 sleep(0.25) 74 75 global on_startup 76 global previous_menu 77 78 on_startup = False 79 previous_menu = -1 80 81 def broadcaster_use(keyboard_key, menu_num): 82 if menu_num == 1: 83 encoded_command = bipedalGame.enc_message_one(keyboard_key) 84 elif menu_num == 2: 85 encoded_command = bipedalGame.enc_message_two(keyboard_key) 86 ser.write(encoded_command.encode("utf-8")) 87 88 show_key(keyboard_key) 89 90 print("Waiting for Bluetooth connection...") 91 server_socket.bind(("",port)) 92 server_socket.listen(1) 93 client_socket,address = server_socket.accept() 94 print("Accepted connection from ",address) 95 96 while True: 97 res = client_socket.recv(1024) 98 client_socket.send(res) 99 100 if menu == 0: 101 os.system('clear') 102 limb = 0 103 link_1 = 0 104 link_2 = 0 105 106 hypotenuse = 0 107 ee_z = 0 108 ee_x = 0.01 109 110 print("Welcome to SylvieOS 2020!") 111 print("Make sure both of your robot legs are in line and parallel to each other.") 112 print("") 113 print("Protip: 1 meter = 1, 50 centimeters = 0.5. Measure joints from the pivot point/center.") 114 115 hypotenuse = float(input("Enter the distance between Joint 1 and Joint 3 i.e. Hypotenuse: ")) 116 link_1 = float(input("Enter the distance between Joint 1 and Joint 2 i.e. Thigh length: ")) 117 link_2 = float(input("Enter the distance between Joint 2 and Joint 3 i.e. Leg length: ")) 118 119 if hypotenuse > 0: 120 ee_zL = hypotenuse 121 ee_zR = hypotenuse 122 menu = 1 123 124 elif menu == 1: 125 solvedik_left = nanoik.solveKinematicsSide(ee_zL, ee_xL, link_1, link_2) 126 solvedik_right = nanoik.solveKinematicsSide(ee_zR, ee_xR, link_1, link_2) 127 128 if menu != previous_menu: 129 if on_startup == False: 130 gbx_L1 = gbx_L1 + (solvedik_left[0] - ra1L_old) 131 gbx_L2 = gbx_L2 + (ra2L_old - solvedik_left[1]) # To reverse motor direction, swap these! 132 gbx_L3 = gbx_L3 + (solvedik_left[2] - ra3L_old) 133 134 gbx_R1 = gbx_R1 + (ra1R_old - solvedik_right[0]) 135 gbx_R2 = gbx_R2 + (ra2R_old - solvedik_right[1]) 136 gbx_R3 = gbx_R3 + (ra3R_old - solvedik_right[2]) 137 138 encoded_command = "none,none," + quickRnd(gbx_L1) + "," + quickRnd(gbx_L2) + "," + quickRnd(gbx_L3) + "," + quickRnd(gbx_R1) + "," + quickRnd(gbx_R2) + "," + quickRnd(gbx_R3) + ",none,none\n" 139 140 ser.write(encoded_command.encode("utf-8")) 141 142 sleep(0.25) 143 144 os.system('clear') 145 146 l_leg_angles = [quickRnd(gbx_L1), quickRnd(gbx_L2), quickRnd(gbx_L3)] 147 r_leg_angles = [quickRnd(gbx_R1), quickRnd(gbx_R2), quickRnd(gbx_R3)] 148 149 l_ee_pos = round(ee_zL, 3), round(ee_xL, 3) 150 r_ee_pos = round(ee_zR, 3), round(ee_xR, 3) 151 152 sr_angles = str(int(round((gbx_sr_all * 10), 1))) 153 waist_angle = str(int(round((gbx_waist * 10), 1))) 154 155 bipedalGame.menuOneText(l_leg_angles, r_leg_angles, l_ee_pos, r_ee_pos, sr_angles, waist_angle, encoded_command) 156 157 previous_menu = menu 158 159 ra1L_old = solvedik_left[0] 160 ra2L_old = solvedik_left[1] 161 ra3L_old = solvedik_left[2] 162 163 ra1R_old = solvedik_right[0] 164 ra2R_old = solvedik_right[1] 165 ra3R_old = solvedik_right[2] 166 167 # Bluetooth Control 168 169 if str(res)[2] == 'w': 170 ee_zL = ee_zL - 0.1 171 show_key('w') 172 elif str(res)[2] == 's': 173 ee_zL = ee_zL + 0.1 174 show_key('s') 175 elif str(res)[2] == 'd': 176 ee_xL = ee_xL + 0.1 177 show_key('d') 178 elif str(res)[2] == 'a': 179 ee_xL = ee_xL - 0.1 180 show_key('a') 181 elif str(res)[2] == 't': 182 ee_zR = ee_zR - 0.1 183 show_key('t') 184 elif str(res)[2] == 'g': 185 ee_zR = ee_zR + 0.1 186 show_key('g') 187 elif str(res)[2] == 'h': 188 ee_xR = ee_xR + 0.1 189 show_key('h') 190 elif str(res)[2] == 'f': 191 ee_xR = ee_xR - 0.1 192 show_key('f') 193 elif str(res)[2] == 'i': 194 ee_zL = ee_zL - 0.1 195 ee_zR = ee_zR - 0.1 196 show_key('i') 197 elif str(res)[2] == 'k': 198 ee_zL = ee_zL + 0.1 199 ee_zR = ee_zR + 0.1 200 show_key('k') 201 elif str(res)[2] == 'l': 202 ee_xL = ee_xL + 0.1 203 ee_xR = ee_xR + 0.1 204 show_key('l') 205 elif str(res)[2] == 'j': 206 ee_xL = ee_xL - 0.1 207 ee_xR = ee_xR - 0.1 208 show_key('j') 209 210 elif str(res)[2] == 'c': 211 gbx_waist = gbx_waist + 0.5 212 show_key('c') 213 elif str(res)[2] == 'v': 214 gbx_waist = gbx_waist - 0.5 215 show_key('v') 216 elif str(res)[2] == 'b': 217 gbx_sr_all = gbx_sr_all + 0.1 218 broadcaster_use('b', 1) 219 elif str(res)[2] == 'n': 220 gbx_sr_all = gbx_sr_all - 0.1 221 broadcaster_use('n', 1) 222 elif str(res)[2] == '4': 223 broadcaster_use('4', 1) 224 elif str(res)[2] == '5': 225 broadcaster_use('5', 1) 226 elif str(res)[2] == '6': 227 broadcaster_use('6', 1) 228 elif str(res)[2] == '7': 229 broadcaster_use('7', 1) 230 elif str(res)[2] == '8': 231 broadcaster_use('8', 1) 232 elif str(res)[2] == '9': 233 broadcaster_use('9', 1) 234 elif str(res)[2] == '2': 235 os.system('clear') 236 print("NAVIGATING TO MENU 2") 237 238 sleep(1) 239 240 on_startup = False 241 menu = 2 242 previous_menu 243 244 elif menu == 2: 245 if menu != previous_menu: 246 os.system('clear') 247 print("Individual joint tweaking menu") 248 print("This is where gearboxes can be adjusted one at a time") 249 print("Press QWERTYASDFGH to move one gearbox by 0.5 degrees") 250 251 previous_menu = menu 252 253 # Left leg 254 255 if str(res)[2] == 'q': 256 broadcaster_use('q', 2) 257 elif str(res)[2] == 'w': 258 broadcaster_use('w', 2) 259 elif str(res)[2] == 'e': 260 broadcaster_use('e', 2) 261 elif str(res)[2] == 'r': 262 broadcaster_use('r', 2) 263 elif str(res)[2] == 't': 264 broadcaster_use('t', 2) 265 elif str(res)[2] == 'y': 266 broadcaster_use('y', 2) 267 268 # Right leg 269 270 elif str(res)[2] == 'a': 271 broadcaster_use('a', 2) 272 elif str(res)[2] == 's': 273 broadcaster_use('s', 2) 274 elif str(res)[2] == 'd': 275 broadcaster_use('d', 2) 276 elif str(res)[2] == 'f': 277 broadcaster_use('f', 2) 278 elif str(res)[2] == 'g': 279 broadcaster_use('g', 2) 280 elif str(res)[2] == 'h': 281 broadcaster_use('h', 2) 282 elif str(res)[2] == 'u': 283 broadcaster_use('u', 2) 284 elif str(res)[2] == 'i': 285 broadcaster_use('i', 2) 286 elif str(res)[2] == 'z': 287 broadcaster_use('z', 2) 288 elif str(res)[2] == 'x': 289 broadcaster_use('x', 2) 290 elif str(res)[2] == 'c': 291 broadcaster_use('c', 2) 292 elif str(res)[2] == 'v': 293 broadcaster_use('v', 2) 294 elif str(res)[2] == 'b': 295 broadcaster_use('b', 2) 296 elif str(res)[2] == 'n': 297 broadcaster_use('n', 2) 298 elif str(res)[2] == '1': 299 os.system('clear') 300 print("RETURNING TO MENU 1") 301 302 sleep(1) 303 304 on_startup = False 305 menu = 1 306 previous_menu = -1 307 308 client_socket.close() 309 server_socket.close()