George_Andreou/Robotex-Folkrace
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Update algorithm

Browse source
This commit is contained in:
Γιώργος Αντρέου 2026-07-08 02:32:44 +03:00
parent 60625566b6
commit f37bab61c0
1 changed files with 74 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

102
folkrace.py
View file

@ -15,72 +15,110 @@ front = adafruit_hcsr04.HCSR04(trigger_pin=board.D12, echo_pin=board.D13)
right = adafruit_hcsr04.HCSR04(trigger_pin=board.D10, echo_pin=board.D11) right = adafruit_hcsr04.HCSR04(trigger_pin=board.D10, echo_pin=board.D11)
left = adafruit_hcsr04.HCSR04(trigger_pin=board.D6, echo_pin=board.D9) left = adafruit_hcsr04.HCSR04(trigger_pin=board.D6, echo_pin=board.D9)
speed = 0.6
def forward(): def forward():
m1.throttle = 0.5 m1.throttle = speed
m2.throttle = 0.5 m2.throttle = speed
m3.throttle = 0.5 m3.throttle = speed
m4.throttle = 0.5 m4.throttle = speed
def backward(): def backward():
m1.throttle = -0.5 m1.throttle = -speed
m2.throttle = -0.5 m2.throttle = -speed
m3.throttle = -0.5 m3.throttle = -speed
m4.throttle = -0.5 m4.throttle = -speed
def stleft(): def stleft():
m1.throttle = -0.5 m1.throttle = -speed
m2.throttle = 0.5 m2.throttle = speed
m3.throttle = 0.5 m3.throttle = speed
m4.throttle = -0.5 m4.throttle = -speed
def stright(): def stright():
m1.throttle = 0.5 m1.throttle = speed
m2.throttle = -0.5 m2.throttle = -speed
m3.throttle = -0.5 m3.throttle = -speed
m4.throttle = 0.5 m4.throttle = speed
def dgright(): def dgright():
m1.throttle =0.5 m1.throttle =speed
m2.throttle = 0 m2.throttle = 0
m3.throttle = 0 m3.throttle = 0
m4.throttle =0.5 m4.throttle =speed
def dgleft(): def dgleft():
m1.throttle = 0 m1.throttle = 0
m2.throttle =0.5 m2.throttle =speed
m3.throttle =0.5 m3.throttle =speed
m4.throttle = 0 m4.throttle = 0
def turnright(): def turnright():
m1.throttle = 0.5 m1.throttle = speed
m2.throttle = 0.5 m2.throttle = speed
m3.throttle = 0.5 m3.throttle = speed
m4.throttle = 0 m4.throttle = 0
def turnleft(): def turnleft():
m1.throttle = 0.5 m1.throttle = speed
m2.throttle = 0.5 m2.throttle = speed
m3.throttle = 0 m3.throttle = 0
m4.throttle = 0.5 m4.throttle = speed
def rotright():
m1.throttle = speed
m2.throttle = - speed
m3.throttle = 0
m4.throttle = 0
def rotleft():
m1.throttle = - speed
m2.throttle = speed
m3.throttle = 0
m4.throttle = 0
while True: while True:
print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}") print("F=", f"{front.distance:.2f}", "R=", f"{right.distance:.2f}", "L=", f"{left.distance:.2f}")
try: try:
if front.distance <= 20: # What to do when there is an obstacle ahead if front.distance <= 35: # What to do when there is an obstacle ahead
if left.distance > right.distance: # Diagonally left if left.distance > right.distance: # Diagonally left
dgleft() dgleft()
time.sleep(0.8) time.sleep(0.8)
if right.distance > left.distance: # Diagonally right if right.distance > left.distance: # Diagonally right
dgright() dgright()
time.sleep(0.8) time.sleep(0.8)
if left.distance < 30: # Strafe right
if front.distance < 10:
backward()
time.sleep(0.7)
if left.distance > right.distance: # Diagonally left
rotleft()
time.sleep(0.5)
if right.distance > left.distance: # Diagonally right
rotright()
time.sleep(0.5)
if left.distance < 50: # Strafe right
turnright() turnright()
#stright() time.sleep(0.4)
time.sleep(0.5) if right.distance < 50: # Strafe left
if right.distance < 30: # Strafe left
turnleft() turnleft()
#stleft() time.sleep(0.4)
if left.distance < 15: # Strafe right
#turnright()
dgright()
time.sleep(0.5) time.sleep(0.5)
rotright()
time.sleep(0.4)
if right.distance < 15: # Strafe left
#turnleft()
dgleft()
time.sleep(0.5)
rotleft()
time.sleep(0.4)
else: else:
forward() forward()