Update

client_s2/main.py

@@ -1,4 +1,6 @@
-SOCKET_PATH = "/tmp/compLib"
+import time
+
+from compLib.CompLibClient import CompLibClient
 
 
 # def send(data, size):
@@ -30,61 +32,143 @@ def main():
     #     send(encoder_read_positions_request.SerializeToString(), encoder_read_positions_request.ByteSize())
     # print("--- %s seconds ---" % (time.time() - start_time))
 
-    # from compLib.IRSensor import IRSensor
-    # IRSensor.read_all()
-    #
+    from compLib.IRSensor import IRSensor
+    IRSensor.enable()
+
+    startTime = time.time()
+    while time.time() - startTime < 10:
+        print(IRSensor.read_all())
+        time.sleep(0.01)
+
     # from compLib.Encoder import Encoder
     # print(Encoder.read_all_positions())
     # print(Encoder.read_all_velocities())
 
     # from compLib.Motor import Motor
-    # Motor.speed(0, 50)
-    # Motor.speed(3, -50)
-    #
-    # import time
-    # time.sleep(2)
-    #
-    # Motor.speed(0, 0)
-    # Motor.speed(3, -0)
 
-    # Motor.power(0, 0)
-    # Motor.power(3, 0)
+    # Motor.speed(0, -50)
+    # Motor.speed(3, 50)
 
-    # import math
-    # from compLib.Movement import Movement
-    # Movement.turn_degrees(90, math.pi * 2)
-    # Movement.turn_degrees(-90, math.pi * 2)
-    #
-    # Movement.turn_degrees(90, math.pi * 2)
-    # Movement.turn_degrees(90, -math.pi * 2)
-    #
-    # Movement.turn_degrees(90, math.pi * 2)
-    # Movement.turn_degrees(-90, -math.pi * 2)
+    # Motor.power(0, -50)
+    # Motor.power(3, 50)
 
-    # from compLib.Movement import Movement
-    # Movement.drive_distance(0.1, 0.5)
-    # Movement.drive_distance(-0.1, 0.5)
-    #
-    # Movement.drive_distance(0.1, 0.5)
-    # Movement.drive_distance(0.1, -0.5)
-    #
-    # Movement.drive_distance(0.1, 0.5)
-    # Movement.drive_distance(-0.1, -0.5)
+    # time.sleep(5)
 
-    from compLib.Movement import Movement
-    import math
-    Movement.drive_distance(0.5, 0.5)
-    Movement.turn_degrees(90, math.pi * 2)
 
-    Movement.drive_distance(0.5, 0.5)
-    Movement.turn_degrees(90, math.pi * 2)
+#
+# import time
+# time.sleep(2)
+#
+# Motor.speed(0, 0)
+# Motor.speed(3, -0)
 
-    Movement.drive_distance(0.5, 0.5)
-    Movement.turn_degrees(90, math.pi * 2)
+# Motor.power(0, 0)
+# Motor.power(3, 0)
 
-    Movement.drive_distance(0.5, 0.5)
-    Movement.turn_degrees(90, math.pi * 2)
+# import math
+# from compLib.Movement import Movement
+# Movement.turn_degrees(90, math.pi * 2)
+# Movement.turn_degrees(-90, math.pi * 2)
+#
+# Movement.turn_degrees(90, math.pi * 2)
+# Movement.turn_degrees(90, -math.pi * 2)
+#
+# Movement.turn_degrees(90, math.pi * 2)
+# Movement.turn_degrees(-90, -math.pi * 2)
+
+# from compLib.Movement import Movement
+# Movement.drive_distance(0.1, 0.5)
+# Movement.drive_distance(-0.1, 0.5)
+#
+# Movement.drive_distance(0.1, 0.5)
+# Movement.drive_distance(0.1, -0.5)
+#
+# Movement.drive_distance(0.1, 0.5)
+# Movement.drive_distance(-0.1, -0.5)
+
+# from compLib.Movement import Movement
+# import math
+# import time
+# Movement.drive_distance(0.5, 0.5)
+# time.sleep(1)
+# Movement.turn_degrees(90, math.pi * 2)
+# time.sleep(1)
+#
+# Movement.drive_distance(0.5, 0.5)
+# time.sleep(1)
+# Movement.turn_degrees(90, math.pi * 2)
+# time.sleep(1)
+#
+# Movement.drive_distance(0.5, 0.5)
+# time.sleep(1)
+# Movement.turn_degrees(90, math.pi * 2)
+# time.sleep(1)
+#
+# Movement.drive_distance(0.5, 0.5)
+# time.sleep(1)
+# Movement.turn_degrees(90, math.pi * 2)
+# time.sleep(1)
+
+# import time
+#
+# from compLib.IRSensor import IRSensor
+# from compLib.Motor import Motor
+#
+# IRSensor.enable()
+#
+# DRIVE_SPEED = 2.0
+# COLOR_BREAK = 900
+# KP = 0.25
+# KD = 0.0
+#
+#
+# def drive(leftSpeed, rightSpeed):
+#     Motor.speed(0, -rightSpeed)
+#     Motor.power(3, leftSpeed)
+#
+#
+# def follow(sleepTime=0.1):
+#     lastError = 0
+#     sensorsBlack = 0
+#
+#     while sensorsBlack < 3:
+#         data = IRSensor.read_all()
+#
+#         sensorsBlack = 0
+#         for i in range(len(data)):
+#             if data[i] > COLOR_BREAK:
+#                 sensorsBlack += 1
+#
+#         error = lastError
+#         if data[2] > COLOR_BREAK:
+#             error = 0
+#         elif data[0] > COLOR_BREAK:
+#             error = -1.5
+#         elif data[4] > COLOR_BREAK:
+#             error = 1.5
+#         elif data[1] > COLOR_BREAK:
+#             error = -1
+#         elif data[3] > COLOR_BREAK:
+#             error = 1
+#         elif error == 1.5:
+#             error = 3
+#         elif error == -1.5:
+#             error = -3
+#
+#         lastError = error
+#
+#         adjustment = KP * error + KD * (error - lastError)
+#         leftSpeed = DRIVE_SPEED + adjustment
+#         rightSpeed = DRIVE_SPEED - adjustment
+#
+#         print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
+#         drive(leftSpeed, rightSpeed)
+#
+#     drive(0, 0)
+#     time.sleep(sleepTime)
 
 
 if __name__ == '__main__':
+    CompLibClient.use_tcp_socket("dev03.local")
+    # follow()
     main()