109 lines
2.5 KiB
Python
109 lines
2.5 KiB
Python
import time
|
|
|
|
from compLib.CompLibClient import CompLibClient
|
|
from compLib.IRSensor import IRSensor
|
|
from compLib.Motor import Motor
|
|
|
|
DRIVE_SPEED = 5.0
|
|
COLOR_BREAK = 1500.0
|
|
KP = 2.0
|
|
KD = 0.0
|
|
|
|
|
|
def drive(left_speed, right_speed):
|
|
print(left_speed, right_speed)
|
|
|
|
right_speed *= -1.0
|
|
|
|
Motor.speed(0, right_speed)
|
|
Motor.speed(3, left_speed)
|
|
|
|
|
|
def follow(sleep_time=0.1):
|
|
last_error = 0
|
|
sensors_black = 0
|
|
while sensors_black <= 3:
|
|
sensor_values = IRSensor.read_all()
|
|
|
|
sensors_black = 0
|
|
for sensor in sensor_values:
|
|
if sensor > COLOR_BREAK:
|
|
sensors_black += 1
|
|
|
|
error = last_error
|
|
if sensor_values[2] > COLOR_BREAK:
|
|
error = 0
|
|
elif sensor_values[0] > COLOR_BREAK:
|
|
error = -1.5
|
|
elif sensor_values[4] > COLOR_BREAK:
|
|
error = 1.5
|
|
elif sensor_values[1] > COLOR_BREAK:
|
|
error = -1
|
|
elif sensor_values[3] > COLOR_BREAK:
|
|
error = 1
|
|
elif error == 1.5:
|
|
error = 3.5
|
|
elif error == -1.5:
|
|
error = -3.5
|
|
|
|
last_error = error
|
|
|
|
adjustment = KP * error + KD * (error - last_error)
|
|
left_speed = DRIVE_SPEED + adjustment
|
|
right_speed = DRIVE_SPEED - adjustment
|
|
|
|
print(sensor_values)
|
|
print(f"{left_speed} {right_speed} {adjustment} {error}")
|
|
drive(left_speed, right_speed)
|
|
|
|
drive(0, 0)
|
|
time.sleep(sleep_time)
|
|
|
|
|
|
def follow_simple():
|
|
left_speed = DRIVE_SPEED
|
|
right_speed = DRIVE_SPEED
|
|
sensor_values = IRSensor.read_all()
|
|
while True:
|
|
sensor_values = IRSensor.read_all()
|
|
# for i in range(len(sensor_values)):
|
|
# sensor_values[i] = (sensor_values[i] + new_sensor_values[i]) / 2.0
|
|
|
|
print(sensor_values)
|
|
|
|
if sensor_values[0] > COLOR_BREAK and sensor_values[4] > COLOR_BREAK:
|
|
break
|
|
|
|
if sensor_values[0] > COLOR_BREAK:
|
|
left_speed = -DRIVE_SPEED / 2
|
|
right_speed = DRIVE_SPEED
|
|
elif sensor_values[4] > COLOR_BREAK:
|
|
left_speed = DRIVE_SPEED
|
|
right_speed = -DRIVE_SPEED / 2
|
|
elif sensor_values[2] > COLOR_BREAK:
|
|
left_speed = DRIVE_SPEED
|
|
right_speed = DRIVE_SPEED
|
|
|
|
drive(left_speed, right_speed)
|
|
|
|
|
|
def main():
|
|
CompLibClient.use_unix_socket()
|
|
|
|
IRSensor.enable()
|
|
time.sleep(0.1)
|
|
|
|
# while True:
|
|
# print(IRSensor.read_all())
|
|
|
|
# follow_simple()
|
|
# drive(5, 5)
|
|
# time.sleep(5)
|
|
# follow()
|
|
# follow()
|
|
# follow()
|
|
# follow()
|
|
# follow(0.2)
|
|
|
|
|
|
main()
|