174 lines
4.3 KiB
Python
174 lines
4.3 KiB
Python
import time
|
|
|
|
from compLib.CompLibClient import CompLibClient
|
|
|
|
|
|
# def send(data, size):
|
|
# with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
|
|
# sock.connect(SOCKET_PATH)
|
|
# sock.sendall(size.to_bytes(1, byteorder='big'))
|
|
# sock.sendall(data)
|
|
#
|
|
# response_size_bytes = sock.recv(1)
|
|
# response_size = int.from_bytes(response_size_bytes, byteorder="big")
|
|
# # print(f"Response size: {response_size}")
|
|
#
|
|
# response_bytes = sock.recv(response_size)
|
|
# generic_response = CompLib_pb2.GenericResponse()
|
|
#
|
|
# generic_response.ParseFromString(response_bytes)
|
|
# # print(f"Response: {generic_response}")
|
|
#
|
|
# # reponseBytes =
|
|
|
|
|
|
def main():
|
|
# encoder_read_positions_request = CompLib_pb2.EncoderReadPositionsRequest()
|
|
# # readSensorsRequest.header = CompLib_pb2.Header()
|
|
# encoder_read_positions_request.header.message_type = encoder_read_positions_request.DESCRIPTOR.full_name
|
|
#
|
|
# start_time = time.time()
|
|
# for i in range(100000):
|
|
# send(encoder_read_positions_request.SerializeToString(), encoder_read_positions_request.ByteSize())
|
|
# print("--- %s seconds ---" % (time.time() - start_time))
|
|
|
|
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)
|
|
|
|
# Motor.power(0, -50)
|
|
# Motor.power(3, 50)
|
|
|
|
# time.sleep(5)
|
|
|
|
|
|
#
|
|
# import time
|
|
# time.sleep(2)
|
|
#
|
|
# Motor.speed(0, 0)
|
|
# Motor.speed(3, -0)
|
|
|
|
# Motor.power(0, 0)
|
|
# Motor.power(3, 0)
|
|
|
|
# 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()
|