96 lines
No EOL
2.2 KiB
Python
96 lines
No EOL
2.2 KiB
Python
from compLib.Motor import Motor
|
|
from compLib.IRSensor import IRSensor
|
|
from compLib.CompLibClient import CompLibClient
|
|
|
|
import time
|
|
import math
|
|
|
|
DRIVE_SPEED = 20.0
|
|
COLOR_BREAK = 850
|
|
KP = 7.5
|
|
KD = 0.0
|
|
|
|
SAMPLE_TIME_S = 0.001
|
|
CUTOFF_FREQ_HZ = 50.0
|
|
|
|
RC = 1.0 / (2.0 * math.pi * CUTOFF_FREQ_HZ)
|
|
|
|
FIRST_COEFF = SAMPLE_TIME_S / (SAMPLE_TIME_S + RC)
|
|
SECOND_COEFF = RC / (SAMPLE_TIME_S + RC)
|
|
|
|
out_old = 0.0
|
|
start_time = time.time()
|
|
|
|
|
|
def drive(leftSpeed, rightSpeed):
|
|
rightSpeed *= -0.906
|
|
|
|
Motor.multiple_power((0, min(max(-100, rightSpeed), 100)), (3, min(max(-100, leftSpeed), 100)))
|
|
|
|
def follow(sleepTime = 0.3):
|
|
global out_old
|
|
lastError = 0
|
|
sensorsBlack = 0
|
|
while sensorsBlack < 3:
|
|
sensor_data = IRSensor.read_all()
|
|
|
|
sensorsBlack = 0
|
|
for i in range(0, 5):
|
|
if sensor_data[i] > COLOR_BREAK:
|
|
sensorsBlack += 1
|
|
|
|
|
|
middle_sensor = sensor_data[2]
|
|
filtered_sensor = FIRST_COEFF * middle_sensor + SECOND_COEFF * out_old
|
|
out_old = filtered_sensor
|
|
|
|
sample_time = str(time.time() - start_time).replace(".", ",")
|
|
print(f"{sample_time} {middle_sensor} {int(filtered_sensor)}")
|
|
|
|
error = lastError
|
|
if sensor_data[2] > COLOR_BREAK:
|
|
error = 0
|
|
elif sensor_data[0] > COLOR_BREAK:
|
|
error = -1.5
|
|
elif sensor_data[4] > COLOR_BREAK:
|
|
error = 1.5
|
|
elif sensor_data[1] > COLOR_BREAK:
|
|
error = -1
|
|
elif sensor_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)
|
|
|
|
time.sleep(SAMPLE_TIME_S)
|
|
|
|
drive(0, 0)
|
|
time.sleep(sleepTime)
|
|
|
|
def main():
|
|
CompLibClient.use_unix_socket()
|
|
|
|
IRSensor.enable()
|
|
time.sleep(0.5)
|
|
|
|
# while True:
|
|
# print(IRSensor.read_all())
|
|
# time.sleep(0.01)
|
|
|
|
follow()
|
|
follow()
|
|
follow()
|
|
follow()
|
|
follow(0.2)
|
|
|
|
main() |