def test_follow_line(self):
test_args = ["program", "-t", "config_small"]
with patch.object(sys, 'argv', test_args):
sim = Process(target=__main__.main, daemon=True)
sim.start()
cs.client_socket = None
sleep(9)
from ev3dev2.sensor.lego import ColorSensor
tank = MoveTank(OUTPUT_A, OUTPUT_D)
tank.cs = ColorSensor(INPUT_2)
try:
# Follow the line for 4500ms
tank.follow_line(
kp=11.3/4, ki=0.05/4, kd=3./4,
speed=SpeedPercent(10),
follow_for=follow_for_ms,
ms=14500,
target_light_intensity=50
)
except Exception:
tank.stop()
raise
# DEFINIÇÕES ent_motor_esq = OUTPUT_C ent_motor_dir = OUTPUT_D ent_motor_grande = OUTPUT_B ent_motor_medio = OUTPUT_A ent_sc_esq = INPUT_3 ent_sc_dir = INPUT_4 ent_us_lat = INPUT_2 ent_us_fr = INPUT_1 steering_pair = MoveSteering(ent_motor_esq, ent_motor_dir) tank = MoveTank(ent_motor_esq, ent_motor_dir) tank.cs = ColorSensor(ent_sc_esq) garra_g = LargeMotor(ent_motor_grande) garra_m = MediumMotor(ent_motor_medio) cor_esq = ColorSensor(ent_sc_esq) cor_dir = ColorSensor(ent_sc_dir) usl = UltrasonicSensor(ent_us_lat) usf = UltrasonicSensor(ent_us_fr) sound = Sound() btn = Button() #Funções #Cor #Locomoção
log.info("%s: %s %s, left motor moved %s (NEW MIN)" % (tank, kx_to_tweak, kx, delta_left_motor_pos))
else:
log.info("%s: %s %s, left motor moved %s" % (tank, kx_to_tweak, kx, delta_left_motor_pos))
tank.stop()
return min_delta_kx
if __name__ == "__main__":
# logging
logging.basicConfig(level=logging.DEBUG, format="%(asctime)s %(levelname)5s: %(message)s")
log = logging.getLogger(__name__)
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.cs = ColorSensor()
speed = SpeedPercent(30)
# Find the best integer for kp (in increments of 1) then fine tune by
# finding the best float (in increments of 0.1)
kp = find_kp_ki_kd(tank, 1, 20, 1, speed, 'kp', 0, 0, 0)
kp = find_kp_ki_kd(tank, kp - 1, kp + 1, 0.1, speed, 'kp', kp, 0, 0)
print("\n\n\n%s\nkp %s\n%s\n\n\n" % ("" * 10, kp, "*" * 10))
# Find the best float ki (in increments of 0.1)
ki = find_kp_ki_kd(tank, 0, 1, 0.1, speed, 'ki', kp, 0, 0)
print("\n\n\n%s\nki %s\n%s\n\n\n" % ("" * 10, ki, "*" * 10))
# Find the best integer for kd (in increments of 1) then fine tune by
# finding the best float (in increments of 0.1)
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms, follow_for_forever
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sound import Sound
from ev3dev2.display import Display
from ev3dev2.button import Button
from time import sleep, time
import threading
import os
speeker = Sound()
tank = MoveTank(OUTPUT_A, OUTPUT_B)
cs = ColorSensor()
tank.cs = cs
screen = Display()
button = Button()
# os.system('setfont CyrAsia-Terminus12x6')
def color_find(endtime):
start_time = time()
triggerd = False
while True:
if cs.color == 5:
speeker.beep()
tank.stop()
sleep(1)
exit()
if time() - start_time >= endtime and not triggerd:
tank.stop()
speeker.beep()
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms
from ev3dev2.sensor.lego import ColorSensor
import ev3dev.ev3 as ev3
import signal
tank = MoveTank(OUTPUT_A, OUTPUT_B)
colorSensorFollow = ev3.ColorSensor('in4')
colorSensorStop = ev3.ColorSensor('in3')
assert colorSensorFollow.connected, "LightSensorFollow(ColorSensor) is not connected"
assert colorSensorStop.connected, "LightSensorFollow(ColorSensor) is not connected"
tank.cs = colorSensorFollow
# setup gracefully shutdown
def signal_handler(sig, frame):
tank.stop()
print('Shutting down gracefully')
exit(0)
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C to exit')
# Follow the line for 4500ms
while (colorSensorStop.value() > 40):
tank.follow_line(
kp=1, ki=0, kd=0,
speed=SpeedPercent(10),
target_light_intensity=37,
follow_left_edge = False,
# DEFINIÇÕES
ent_motor_esq = OUTPUT_C
ent_motor_dir = OUTPUT_D
ent_motor_grande = OUTPUT_B
ent_motor_medio = OUTPUT_A
ent_sc_esq = INPUT_3
ent_sc_dir = INPUT_4
ent_us_lat = INPUT_2
ent_us_fr = INPUT_1
steering_pair = MoveSteering(ent_motor_esq, ent_motor_dir)
tank_pair = MoveTank(ent_motor_esq, ent_motor_dir)
tank_pair.cs = ColorSensor(ent_sc_esq)
garra_g = LargeMotor(ent_motor_grande)
garra_m = MediumMotor(ent_motor_medio)
cor_esq = ColorSensor(ent_sc_esq)
cor_dir = ColorSensor(ent_sc_dir)
usl = UltrasonicSensor(ent_us_lat)
usf = UltrasonicSensor(ent_us_fr)
sound = Sound()
btn = Button()
# FUNÇÕES
def distancia(sensor):
a1 = sensor.distance_centimeters
#!/usr/bin/env python3 from time import sleep from ev3dev2.sound import Sound from ev3dev2.motor import OUTPUT_A, OUTPUT_D, OUTPUT_B, OUTPUT_C, LargeMotor, MediumMotor, SpeedPercent, MoveTank, MoveDifferential, follow_for_ms from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sensor.lego import ColorSensor from ev3dev2.button import * from ev3dev2.wheel import EV3Tire from ev3dev2.sound import Sound #initiate all motors sensors etc. # this is common so that it is controlled in one place and everyone uses the same names for motors etc. STUD_MM = 8 mdiff = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3Tire, 16 * STUD_MM) tank = MoveTank(OUTPUT_A, OUTPUT_D) leftMotor = LargeMotor(OUTPUT_A) rightMotor = LargeMotor(OUTPUT_D) medMotor = MediumMotor(OUTPUT_C) medMotor2 = MediumMotor(OUTPUT_B) tank.cs = ColorSensor(INPUT_3) color1 = ColorSensor(INPUT_1) color2 = ColorSensor(INPUT_2) color3 = ColorSensor(INPUT_3) color4 = ColorSensor(INPUT_4) btn = Button() sound = Sound()
#!/usr/bin/env micropython
from ev3dev2.motor import LargeMotor, OUTPUT_C, OUTPUT_B, follow_for_ms
from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM, MoveTank, MoveSteering, SpeedPercent
from time import sleep
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
lmB = LargeMotor(OUTPUT_B)
lmC = LargeMotor(OUTPUT_C)
# lmB.on_for_seconds(90, 1, True, False)
# lmC.on_for_seconds(90, 1)
tank_pair = MoveTank(OUTPUT_B, OUTPUT_C)
#tank_pair.on_for_seconds(50,50,1)
#tank_pair.on_for_seconds(-50,-50,1)
tank_pair.cs = ColorSensor(INPUT_1)
tank_pair.follow_line(kp=11.3,
ki=0.05,
kd=3.2,
speed=SpeedPercent(5),
follow_for=follow_for_ms,
ms=9000)
#!/usr/bin/env python3
from ev3dev2.motor import Motor, MediumMotor, LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank, follow_for_forever
from ev3dev2.sensor.lego import ColorSensor
lifting_motor = MediumMotor('outA')
left_motor = OUTPUT_B
right_motor = OUTPUT_C
tank_drive = MoveTank(left_motor, right_motor)
# Setpoints
speed = 15
tank_drive.cs = ColorSensor()
def move(move_dir):
if move_dir == 'forward':
tank_drive.on(SpeedPercent(speed), SpeedPercent(speed))
if move_dir == 'reverse':
tank_drive.on(SpeedPercent(-speed), SpeedPercent(-speed))
elif move_dir == '':
tank_drive.off()
def turn(turn_dir, custom_turn_degree=0):
left_turn_degree = 420
right_turn_degree = 420
u_turn_degree = 360
# Turn on left and right motor with defined speed for defined rotations
if turn_dir == 'left':
tank_drive.on(SpeedPercent(0), SpeedPercent(speed))
elif turn_dir == 'right':
