Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #!/usr/bin/env python3
- from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, MoveTank, SpeedDPS, SpeedRPS, SpeedRPM, MoveSteering
- from ev3dev2.led import Leds
- from ev3dev2.button import Button
- from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor, ColorSensor
- from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
- from time import sleep
- #definindo as variaveis
- btn = Button()
- us = UltrasonicSensor()
- usl = UltrasonicSensor(INPUT_2)
- usr = UltrasonicSensor(INPUT_3)
- cs = ColorSensor()
- csl = ColorSensor(INPUT_1)
- csr = ColorSensor(INPUT_4)
- mm = MediumMotor(OUTPUT_A)
- tank_pair = MoveTank(OUTPUT_B, OUTPUT_C)
- steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
- #as funcoes de procurar, atacar, retornar e saida
- def saida():
- #print("saida")
- #mm.on_for_degrees(speed = 70, degrees = -145, brake = True, block = True)
- if csl.value() > 26:
- tank_pair.on(left_speed = 0, right_speed = 40)
- while(csr.value() < 26):
- sleep(0.01)
- tank_pair.off(brake = True)
- tank_pair.on_for_degrees(left_speed = -50, right_speed = -50, degrees = 90, brake = True, block = True)
- tank_pair.on_for_rotations(left_speed = 65, right_speed = -40, rotations = 1, brake = False, block = True)
- tank_pair.on_for_degrees(left_speed = 70, right_speed = 70, degrees = 270, brake = False, block = True)
- return
- if csr.value() > 26:
- tank_pair.on(left_speed = 40, right_speed = 0)
- while(csl.value() < 26):
- sleep(0.01)
- tank_pair.off(brake = True)
- tank_pair.on_for_degrees(left_speed = -50, right_speed = -50, degrees = 90, brake = True, block = True)
- tank_pair.on_for_rotations(left_speed = -40, right_speed = 65, rotations = 1, brake = False, block = True)
- tank_pair.on_for_degrees(left_speed = 70, right_speed = 70, degrees = 270, brake = False, block = True)
- return
- def voltar():
- #print("voltar")
- #mm.on_for_degrees(speed = 70, degrees = -145, brake = True, block = True)
- while cs.value() > 26:
- tank_pair.on(left_speed = -70, right_speed = -70)
- tank_pair.off()
- tank_pair.on_for_rotations(left_speed = -50, right_speed = -50, rotations = 1, brake = False, block = True)
- return
- def atacar():
- #print("atacar")
- while True:
- tank_pair.on(left_speed = 70, right_speed = 70)
- #mm.on_for_degrees(speed = 70, degrees = 145, brake = True, block = False)
- if us.distance_centimeters > 15:
- tank_pair.off()
- return True
- if csl.value() > 26 and csr.value() > 26:
- tank_pair.off(brake = True)
- return False
- if csl.value() > 26 or csr.value() > 26:
- tank_pair.off(brake = True)
- break
- #if us.distance_centimeters < 10:
- #mm.on_for_degrees(speed = 70, degrees = 145, brake = True, block = True)
- #mm.on_for_degrees(speed = 70, degrees = -145, brake = True, block = False)
- saida()
- def procurar():
- #print("procurar")
- #mm.on_for_degrees(speed = 70, degrees = -145, brake = True)
- sleep(0.01)
- if us.distance_centimeters < 65:
- tank_pair.on(left_speed = 70, right_speed = 70)
- sleep(0.3)
- if us.distance_centimeters < 15:
- return False
- else:
- return True
- while us.distance_centimeters > 65:
- #steer_pair.on(steering = 100, speed = 100)
- tank_pair.on(left_speed = -30, right_speed = 30)
- if us.distance_centimeters < 65:
- #steer_pair.off(brake = True)
- tank_pair.off(brake = True)
- return True
- if csl.value() > 26 or csr.value() > 26:
- saida()
- if btn.any():
- break
- def procuraproporcional(): #something that imo should work better, though you've got to remember that the sensors are quite shit
- mm.on_for_degrees(speed = 70, degrees = -145, brake = True)
- sleep(0.01)
- if usl.distance_centimeters < 65 and usr.distance_centimeters < 65:
- tank_pair.on(left_speed = 70, right_speed = 70)
- sleep(0.3)
- if usl.distance_centimeters < 15 and usr.distance_centimeters < 15:
- return False
- else:
- return True
- #pensando aqui, delta min pode ser por volta de 6 cm pra poder alinhar certo
- while usr.distance_centimeters > 65 and usl.distance_centimeters > 65:
- tank_pair.on(left_speed = -30, right_speed = 30)
- if btn.any():
- break
- if cs.value() > 26:
- saida()
- if usr.distance_centimeters < 75 and (abs(usr.distance_centimeters - usl.distance_centimeters) > 6):
- while abs(usr.distance_centimeters - usl.distance_centimeters) < 6:
- deltadistr = abs(usr.distance_centimeters - usl.distance_centimeters)
- errorr = deltadistr - 6
- if errorr > 30:
- errorr = 30
- if errorr < 15:
- errorr = 15
- if btn.any():
- break
- if cs.value() > 26:
- saida()
- tank_pair.on(left_speed = -errorr, right_speed = errorr)
- if usl.distance_centimeters < 75 and (abs(usr.distance_centimeters - usl.distance_centimeters) > 6):
- while abs(usr.distance_centimeters - usl.distance_centimeters) < 6:
- deltadistl = abs(usr.distance_centimeters - usl.distance_centimeters)
- errorl = deltadistl - 6
- if errorl > 30:
- errorl = 30
- if errorl < 15:
- errorl = 15
- if btn.any():
- break
- if cs.value() > 26:
- saida()
- tank_pair.on(left_speed = errorl, right_speed = -errorl)
- #inicio
- while cs.value() < 26:
- tank_pair.on(left_speed = 50, right_speed = 50)
- tank_pair.off(brake = True)
- tank_pair.on_for_degrees(left_speed = -50, right_speed = -50, degrees = 90, brake = True, block = True)
- #girar
- tank_pair.on_for_rotations(left_speed = 50, right_speed = -50, rotations = 1, brake = False, block = True)
- #movimentacao inicial pra frente
- tank_pair.on_for_rotations(left_speed = 50, right_speed = 30, rotations = 1, brake = False, block = True)
- tank_pair.on_for_rotations(left_speed = 50, right_speed = 70, rotations = 1, brake = False, block = True)
- while not btn.any():
- if not procurar():
- if not atacar():
- #voltar()
- sleep(0.001)
- sleep(.01)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement