robot control

control.py

@@ -8,7 +8,7 @@ from xbox import *
 import time
 
 # DEBUG
-LEGS_LOG_LEVEL = logging.DEBUG
+LEGS_LOG_LEVEL = logging.WARNING
 CONTROLLER_LOG_LEVEL = logging.CRITICAL
 # Variables configurations
 
@@ -60,8 +60,8 @@ if REAL == 1:
     packetHandler = PacketHandler(1.0)
 
     portHandler.openPort()
-    portHandler.setBaudRate(1000000)
+    x = portHandler.setBaudRate(1000000)
-
+    print(x)
     ADDR_GOAL_POSITION = 30
 
     ids = []
@@ -230,22 +230,68 @@ def normalize(matrix, slider_max, speed):
     return (matrix / slider_max) * speed
 
 
+counter = 0
+
+
+def convert_to_robot(positions):
+    global counter
+    counter += 1
+    if counter % 2 != 0:
+        return
+    print(positions)
+    index = [
+        11, 12, 13,
+        41, 42, 43,
+        21, 22, 23,
+        51, 52, 53,
+        61, 62, 63,
+        31, 32, 33]
+    dir = [-1] * 18
+
+    for i in range(len(positions)):
+        value = int(512 + positions[i] * 150 * dir[i])
+        packetHandler.write2ByteTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, value)
+        # packetHandler.writeTxOnly(portHandler, index[i], ADDR_GOAL_POSITION, 2,
+        #                          bytes(value))
+
+
+def stand(x1, y1, x2, y2):
+    return [
+        0, 0, 0,
+        0, 0, 0,
+        x1, 0, 0,
+        0, -0.5, 0,
+        0, -0.5, 0,
+        y1, 0, 0
+    ]
+
+
 def walk(t, sx, sy, sr):
     xboxdata = xbox.get_data()
+    val = xboxdata["x1"]
+
     max_slider = 0.200
     controllerLogger.debug(xboxdata)
     if xbox.mode == 0:
-        return static()
+        positions = static()
     elif xbox.mode == 1:
-        return jump(xboxdata["y2"])
+        positions = jump(xboxdata["y2"])
     elif xbox.mode == 2:
-        return dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], max_slider * xboxdata["y2"],
+        positions = dino_naive(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], max_slider * xboxdata["y2"],
-                          max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
+                               max_slider * xboxdata["x2"], max_slider * (xboxdata["r2"] + 1))
     elif xbox.mode == 3:
-        return naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
+        positions = naive_walk(t, max_slider * xboxdata["x1"], max_slider * xboxdata["y1"])
     elif xbox.mode == 4:
-        return dev(t, max_slider * xboxdata["y1"], max_slider * xboxdata["x1"], xboxdata["x2"] * max_slider,
+        positions = dev(t, max_slider * xboxdata["y1"], -1 * max_slider * xboxdata["x1"], xboxdata["x2"] * max_slider,
-                   max_slider * xboxdata["y2"])
+                        max_slider * xboxdata["y2"])
+    elif xbox.mode == 5:
+        positions = stand(xboxdata["x1"], xboxdata["y1"], xboxdata["x2"], xboxdata["y2"])
+
+    print(positions)
+    if REAL == 1:
+        convert_to_robot(positions)
+
+    return positions
 
 
 def static():
@@ -344,7 +390,7 @@ def dev(t, speed_x, speed_y, speed_r, y2):
         real_position[patte][0] = wanted_x
         real_position[patte][1] = wanted_y
 
-    return legs(real_position), [center_x, center_y]
+    return legs(real_position)
 
 
 def jump(sy):
@@ -430,7 +476,7 @@ def dino_naive(t, speed_x, speed_y, hz, hy, hx):
     real_position[5][2] = 0.05 + hz
     real_position[5][0] = 0.25 + hx
     real_position[5][1] = hy
-    print(hx)
+
     return legs(real_position)
 
 
@@ -447,7 +493,6 @@ def robot_input():
 
     # Dino mode
     packetHandler.write2ByteTxRx(portHandler, 12, ADDR_GOAL_POSITION, 650)
-    packetHandler.write2ByteTxRx(portHandler, 13, ADDR_GOAL_POSITION, 400)
     packetHandler.write2ByteTxRx(portHandler, 42, ADDR_GOAL_POSITION, 650)
     packetHandler.write2ByteTxRx(portHandler, 43, ADDR_GOAL_POSITION, 400)
 

xbox.py

@@ -31,12 +31,12 @@ class Xbox:
                     axis = event["axis"]
                     if axis in [1, 4]:
                         value = -event["value"]
-                        if abs(value) < 0.1:
+                        if abs(value) < 0.2:
                             value = 0
                         self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
                     else:
                         value = event["value"]
-                        if abs(value) < 0.1:
+                        if abs(value) < 0.2:
                             value = 0
                         self.data[{0: "x1", 1: "y1", 4: "y2", 3: "x2", 5: "r2", 2: "l2"}[event["axis"]]] = value
                 elif "button" in keys: