def go_ball(distrib):
robot.robot_com("-S -m2 -t1")
if (robot.color == "green"):
if (distrib == 1):
robot.go_arc(1100, 700, pi)
robot.go_direct(1370, 1046, pi, -1)
deversoir.deversoir()
robot.go_direct(1170, 1046, pi, 1)
if (distrib == 2):
go_arc(-800, 1800, 3 * pi / 2)
go_direct(-800, 1850, 3 * pi / 2, -1)
robot.robot_com("-S -m2 -t0")
time.sleep(1)
robot.robot_com("-B -x0.72 -m3")
time.sleep(10)
robot.robot_com("-B -x0 -m3")
go_arc(-800, 1800, 3 * pi / 2)
if (robot.color == "orange"):
if (distrib == 1):
robot.go_arc(-1100, 700, 0)
robot.go_direct(-1370, 1046, 0, -1)
deversoir.deversoir()
robot.go_direct(-1170, 1046, 0, 1)
if (distrib == 2):
robot.go_arc(800, 1800, 3 * pi / 2)
robot.go_direct(800, 1850, 3 * pi / 2, -1)
robot.robot_com("-S -m2 -t0")
time.sleep(1)
robot.robot_com("-B -x0.72 -m3")
time.sleep(10)
robot.robot_com("-B -x0 -m3")
go_arc(800, 1800, 3 * pi / 2)
def go_bee():
if (robot.color == "green"):
robot.go_direct(1000, 1800, pi, 1)
robot.robot_com("-S -m3 -t1")
time.sleep(1)
robot.robot_com("-S -m3 -t0")
else:
robot.robot_com("-S -m4 -t1")
time.sleep(1)
robot.robot_com("-S -m4 -t0")
def go_domo():
if (robot.color == "green"):
robot.go_direct(470, 1120, pi, 1)
robot.turn(pi / 6, 0)
else:
robot.go_direct(-470, 1120, 0, 1)
robot.turn(-pi / 6, 0)
wait_arrived_no_opponent()
robot.turn(pi / 2, 1)
if (robot.color == "green"):
robot.turn(0, 1)
else:
robot.turn(pi / 2, 1)
def go_domo():
if (robot.color == "green"):
robot.go_direct(440, 140, pi, 1)
robot.turn(0.8, 0)
time.sleep(3)
robot.go_direct(400, 140, pi, 1)
robot.turn(0.8, 0)
time.sleep(3)
else:
robot.go_direct(-440, 140, 0, 1)
robot.turn(-0.8, 0)
time.sleep(3)
robot.go_direct(400, 140, 0, 1)
robot.turn(-0.8, 0)
time.sleep(3)
wait_arrived_no_opponent()
if (robot.color == "green"):
robot.turn(0, 1)
else:
robot.turn(pi, 1)
# 0x090 0x27 /*P8.7, button 6*/
# 0x094 0x27 /*P8.8, button 7 /
# 0x044 0x27 /*P8.32 button 5*/
# 0x0dc 0x27 /*P9.23 button 3*/
import time
import boot
import robot
import opponent
boot.fast_conf()
while (True):
i = 0
for i in range(0, 4):
robot.go_direct(0, 300, 1.57, 1, True)
for i in range(0, 2):
robot.go_arc(300, 300, 3.14, 1, True)
def init_robot():
global black_button_pressed
try:
GPIO.setup(robot.trig_button, GPIO.IN)
GPIO.setup(robot.col_button, GPIO.IN)
GPIO.setup(robot.but1_button, GPIO.IN)
GPIO.setup(robot.but2_button, GPIO.IN)
GPIO.add_event_detect(robot.but2_button,
GPIO.FALLING,
callback=black_button_callback,
bouncetime=300)
except:
pass
boot_ok = False
while (boot_ok == False):
black_button_pressed = False
print("BOOT ok.")
if exists(robot.asserv_out_file) == False:
print("start first robot deamon")
robot.my_lcd.write("no deamon...")
quit()
if exists(robot.asserv_in_file) == False:
print("start first robot deamon")
robot.my_lcd.write("no deamon...")
quit()
print("ca va passer?")
#config angulaire
# fast_conf()
print("stop asserv")
robot.com("-B -m0")
print("motor 1 back")
robot.com("-B -x-0 -m1")
print("motor 2 back")
robot.com("-B -x-0 -m2")
print("les choses serieuses")
robot.my_lcd.write("deamon connected...")
# reponse=robot.com("-E")
# print(reponse)
print("changer 3 fois la couleur pour selectionner la couleur")
robot.my_lcd.write("chg 3 times col")
GPIO.wait_for_edge(robot.col_button, GPIO.RISING)
if (black_button_pressed == True):
print("black button pressed, it should continue")
continue
print("changer 2 fois la couleur pour selectionner la couleur")
robot.my_lcd.write("chg 2 times col")
GPIO.wait_for_edge(robot.col_button, GPIO.RISING)
if (black_button_pressed == True):
print("black button pressed, it should continue")
continue
print("changer 1 fois la couleur pour selectionner la couleur")
robot.my_lcd.write("chg 1 times col")
GPIO.wait_for_edge(robot.col_button, GPIO.RISING)
if (black_button_pressed == True):
print("black button pressed, it should continue")
continue
robot.my_lcd.write("color : ")
robot.my_lcd.write("press blue then")
while GPIO.input(robot.but1_button) == 1:
if GPIO.input(robot.col_button) == 1:
robot.color = "jaune"
robot.my_lcd.lcd_display_string("jaune ", 1, 8)
else:
robot.color = "violet"
robot.my_lcd.lcd_display_string("violet", 1, 8)
if (black_button_pressed == True):
print("black button pressed, it should continue")
continue
time.sleep(0.1)
if (black_button_pressed == True):
print("black button pressed, it should continue")
if GPIO.input(robot.trig_button) == 0:
robot.my_lcd.write("remove trigger")
GPIO.wait_for_edge(robot.trig_button, GPIO.RISING)
if (black_button_pressed == True):
print("black button pressed, it should continue")
continue
robot.my_lcd.write("insert trigger")
GPIO.wait_for_edge(robot.trig_button, GPIO.FALLING)
if (black_button_pressed == True):
print("black button pressed, it should continue")
continue
robot.my_lcd.write("press blue again")
GPIO.wait_for_edge(robot.but1_button, GPIO.FALLING)
if (black_button_pressed):
continue
robot.my_lcd.write("red button and blue again")
GPIO.wait_for_edge(robot.but1_button, GPIO.FALLING)
if (black_button_pressed):
continue
print("couleur robot", robot.color)
if (robot.color == "jaune"):
reponse = robot.com("-Y -x500 -y 500 -t1.57079632679")
robot.my_lcd.write("righ !")
fthewall("derriere")
robot.turn(-1.57079632679, 0)
while (robot.com("-E") != "yes"):
time.sleep(0.1)
else:
reponse = robot.com("-Y -x500 -y 2500 -t4.71238898038")
robot.my_lcd.write("left !")
fthewall("devant")
robot.turn(1.57079632679, 0)
while (robot.com("-E") != "yes"):
time.sleep(0.1)
print("F*** The Wall!")
robot.my_lcd.write("f the wall : ")
time.sleep(1)
robot.my_lcd.write("go! ")
fthewall("gauche")
print("placement en position initiale")
if (robot.color == "jaune"):
print("pour les JAUNE")
# reponse=robot.com("-G -x470 -y250 -t3.14159265359 -m0")
robot.go_direct(470, 250, 3.14159265359, -1, False)
else:
print("pour le VIOLET")
# reponse=robot.com("-G -x470 -y2750 -t3.14159265359 -m0")
robot.go_direct(470, 2750, 3.14159265359, -1, False)
print(reponse)
truc = ""
while (robot.com("-E") != "yes"):
time.sleep(0.1)
print("wait for trigger")
GPIO.wait_for_edge(robot.trig_button, GPIO.RISING)
if (black_button_pressed == True):
print("black button pressed, it should continue")
continue
time.sleep(0.1)
boot_ok = True
robot.com("-D -x0.0001 -y0.0004 -t-0.000 -m0")
robot.com("-D -x0.5 -y-0.2 -t-0.000 -m1")
boot.init_robot()
thread_1 = timer.timer()
thread_1.start()
#thread_2 = timer.unbreak()
#thread_2.start()
#depart:450;250;pi
robot.set_lidar()
score = 0
if (robot.color == "jaune"):
#sortie
robot.go_arc(270, 450, 1.50, 1, False)
#direction accelerateur
robot.go_direct(250, 1600, 1.50, 1, False)
servo.servo(0, 1.3)
time.sleep(0.5)
robot.go_direct(250, 1680, 1.50, 1, False)
servo.servo(0, 0)
time.sleep(0.2)
score += 20
robot.my_lcd.write("SCORE:")
robot.my_lcd.write("20 ")
#direction goldenium
robot.go_direct(265, 2100, 1.57, 1, False)
servo.servo(0, 1.4)
time.sleep(0.5)
robot.go_direct(265, 2220, 1.57, 1, False)
servo.servo(0, 0)
time.sleep(0.2)
time.sleep(0.5)
close_servo()
servo(0,0)
GPIO.setup(robot.trig_button,GPIO.IN)
robot.com("-X -x500 -y500 -m0")
robot.com("-D -x0.0001 -y0.0004 -t-0.000 -m0")
robot.com("-D -x0.5 -y-0.2 -t-0.000 -m1")
GPIO.wait_for_edge(robot.trig_button, GPIO.RISING)
thread_1 = timer.timer()
thread_1.start()
robot.set_lidar()
robot.com("-Y -x1000 -y250 -t1.57")
robot.go_direct(1000,1250,1.57,1,False)
open_servo()
robot.go_direct(1000,350,1.57,-1,False)
# time.sleep(0.5)
# robot.go_direct(250,2200,1.50,1,False)
# servo(0,1.5)
# time.sleep(0.5)
# robot.go_direct(250,2300,1.50,1,False)
# servo(0,0)
# time.sleep(0.5)
# robot.go_direct(1500,1300,1.50,-1,False)
# open_servo()
# robot.go_arc(600,375,1.50,1,False)
# robot.go_direct(0,-300,1.50,-1,True)
time.sleep(0.3)
robot.com("-S -m0 -t-0.5")
time.sleep(0.3)
robot.com("-S -m0 -t0")
time.sleep(0.3)
robot.com("-S -m3 -t0.5")
time.sleep(0.3)
robot.com("-S -m3 -t-0.5")
time.sleep(0.3)
robot.com("-S -m3 -t0.5")
time.sleep(0.3)
robot.com("-S -m3 -t-0.5")
time.sleep(0.3)
robot.com("-S -m3 -t0")
time.sleep(0.3)
robot.go_direct(0, 100, 0, 0, True)
robot.com("-S -m1 -t-1.5")
time.sleep(5)
robot.com("-S -m0 -t0.5")
time.sleep(0.3)
robot.com("-S -m0 -t-0.5")
time.sleep(0.3)
robot.com("-S -m0 -t0")
time.sleep(1)
robot.com("-S -m1 -t1.5")
robot.go_direct(0, -100, 0, 0, True)
time.sleep(1)
time.sleep(0.1)
#config pid
robot.com("-D -x0.002 -y0.0001 -t-0.000 -m0")
#boot.fast_conf()
robot.com("-X -x5 -y12 -m1")
time.sleep(0.1)
#config acceleration lineaire, vitesse puis acceleration
robot.com("-X -x500 -y00 -m0")
time.sleep(0.1)
#config pid
robot.com("-X -x300 -y100 -m0")
robot.com("-D -x0.0001 -y0.0004 -t-0.000 -m0")
robot.com("-D -x0.5 -y-0.2 -t-0.000 -m1")
#boot.fast_conf()
reponse=robot.com("-Y -x200 -y500 -t0")
robot.set_lidar()
while(True):
time.sleep(0.1)
robot.go_direct(1200,500,0,0,False)
time.sleep(0.3)
robot.go_direct(200,500,0,0,False)
time.sleep(0.3)