def gyroRotateAbsoluteAngle():
"""Test code for rotating using the gyror"""
tank_drive = MoveTank(OUTPUT_A, OUTPUT_D)
gyro = GyroSensor()
logger = logToDisplay()
for sleepTime in [2, 0.5]:
gyro.mode = 'GYRO-RATE'
gyro.mode = 'GYRO-ANG'
sleep(sleepTime)
startingAngle = gyro.angle
endingAngle = 90
while True:
currentAngle = gyro.angle
logger.log("Current angle {}".format(currentAngle))
if (currentAngle >= endingAngle - 2
and currentAngle <= endingAngle + 2):
tank_drive.stop()
break
elif (currentAngle > endingAngle):
leftSpeed = SpeedPercent(-5)
rightSpeed = SpeedPercent(5)
else:
leftSpeed = SpeedPercent(5)
rightSpeed = SpeedPercent(-5)
tank_drive.on(leftSpeed, rightSpeed)
sleep(0.1)
def Calibrate():
gs = GyroSensor(INPUT_2)
debug_print('GS Calibration begin')
for x in range(2):
gs.mode = 'GYRO-RATE'
gs.mode = 'GYRO-ANG'
time.sleep(.5)
gsnow = gs.angle
debug_print('GS Calibration finish ' + str(gsnow))
def main():
'''The main function of our program'''
# set the console just how we want it
reset_console()
set_cursor(OFF)
set_font('Lat15-Terminus24x12')
# print something to the output panel in VS Code
debug_print('Hello VS Code!')
Xob = 8
Yob = 8
v = 1
x = 0
y = 0
tetha = 0
l = 4
xc = [x]
yc = [y]
motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
gy = GyroSensor()
gy.mode = 'GYRO-RATE'
gy.mode = 'GYRO-ANG'
units = gy.units
angle = gy.value()
print(str(angle))
sleep(1)
while True:
x = -v * math.sin(tetha) + x
y = v * math.cos(tetha) + y
deltaX = (Xob - x) * math.cos(tetha) + (Yob - y) * math.sin(tetha)
curvatura = -(2 * deltaX)/(l**2)
tetha += v * curvatura
xc.append(x)
yc.append(y)
girar(tetha, gy, ang, motor_pair)
avanzar(motor_pair, gy)
sleep(1)
print(str(x) + " - " + str(y))
if math.fabs(x) >= Xob or math.fabs(y) >= Yob:
break
def test_gyro():
gs = GyroSensor(INPUT_2)
# for mode in gs.modes:
# gs.mode = mode
# print('The current gyro mode is: {}'.format(gs.mode))
# print('The angle is at {} degrees'.format(gs.angle))
# print('The rate of rotation is {} degrees/second'.format(gs.rate))
# # print('Here\'s both as a tuple: {}'.format(gs.angle_and_rate))
# # print('Tilt angle: {} degrees?'.format(gs.tilt_angle))
# # print('Tilt rate: {} degrees/second?'.format(gs.tilt_rate))
# # if gs.mode in (GyroSensor.MODE_GYRO_ANG, GyroSensor.MODE_GYRO_G_A, GyroSensor.MODE_TILT_ANG):
# # print('Waiting for angle to change by 90 degrees clockwise: {}'.format(gs.wait_until_angle_changed_by(90, True)))
# # print('Waiting for angle to change by 90 degrees any direction: {}'.format(
# # gs.wait_until_angle_changed_by(90, False))
# # )
gs.mode = GyroSensor.MODE_GYRO_G_A
while not button.any():
print('Angle: {}'.format(gs.angle_and_rate))
sleep(0.3)
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, UltrasonicSensor, GyroSensor
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
from time import sleep
from threading import Thread
import socket
print("Fertig")
sound = Sound()
driver = MoveTank(OUTPUT_A, OUTPUT_B)
us = UltrasonicSensor(INPUT_4)
gs = GyroSensor(INPUT_2)
gs.mode = "GYRO-ANG"
us.mode = "US-DIST-CM"
def server_program():
host = socket.gethostname()
port = 5000 # initiate port no above 1024
server_socket = socket.socket()
server_socket.bind((host, port))
server_socket.listen(2)
conn, address = server_socket.accept()
print("Connection from: " + str(address))
while True:
data = conn.recv(1024).decode()
def Robotrun4():
robot = MoveSteering(OUTPUT_A, OUTPUT_B)
tank = MoveTank(OUTPUT_A, OUTPUT_B)
colorLeft = ColorSensor(INPUT_1)
colorRight = ColorSensor(INPUT_3)
gyro = GyroSensor(INPUT_2)
motorC = LargeMotor(OUTPUT_C)
motorD = LargeMotor(OUTPUT_D)
motorB = LargeMotor(OUTPUT_B)
motorA = LargeMotor(OUTPUT_A)
Constants.STOP = False
gyro.reset()
GyroDrift()
gyro.reset()
show_text("Robot Run 2")
motorC.off(brake=True)
#GyroTurn(steering=-50, angle=5)
acceleration(degrees=DistanceToDegree(30), finalSpeed=30)
lineFollowPID(degrees=DistanceToDegree(30), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3))
acceleration(degrees=DistanceToDegree(5), finalSpeed=30)
accelerationMoveBackward(degrees = DistanceToDegree(7), finalSpeed=50, steering=0)
motorC.on_for_seconds(speed=15, seconds=1, brake=False)
GyroTurn(steering=50, angle=20)
acceleration(degrees=DistanceToDegree(20), finalSpeed=30)
GyroTurn(steering=-55, angle=22)
acceleration(degrees=DistanceToDegree(17), finalSpeed=30)
gyro.mode = "GYRO-ANG"
while gyro.value() < -10 and False == Constants.STOP:
motorA.on(speed = 20)
lineFollowPID(degrees=DistanceToDegree(15), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3))
lineFollowTillIntersectionPID(kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3), color2=ColorSensor(INPUT_1))
lineFollowPID(degrees=DistanceToDegree(10), kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3))
lineFollowTillIntersectionPID(kp=1.3, ki=0.01, kd=15, color=ColorSensor(INPUT_3), color2=ColorSensor(INPUT_1))
if gyro.angle > 2 and False == Constants.STOP:
GyroTurn(steering=-50, angle=gyro.angle)
elif gyro.angle < -2 and False == Constants.STOP:
ang = -1 * gyro.angle
GyroTurn(steering=50, angle=ang)
accelerationMoveBackward(degrees = DistanceToDegree(5), finalSpeed=50, steering=0)
acceleration(degrees=DistanceToDegree(12), finalSpeed=50, steering=2.5)
motorC.on_for_degrees(speed=-25, degrees=150, brake=True)
motorD.on_for_degrees(speed=-25, degrees=150, brake=True)
acceleration(degrees=DistanceToDegree(12), finalSpeed=45, steering=4)
#acceleration(degrees=DistanceToDegree(12), finalSpeed=45, steering=5)
#Moving treadmill
if False == Constants.STOP:
tank.on_for_seconds(left_speed=1, right_speed=20, seconds=5.5)
#motorB.on_for_seconds(speed=15, seconds=10)
motorC.on_for_seconds(speed=25, seconds=2, brake=False)
motorD.on_for_seconds(speed=25, seconds=2, brake=False)
accelerationMoveBackward(degrees = DistanceToDegree(5), finalSpeed=20, steering=0)
while colorLeft.reflected_light_intensity < Constants.WHITE:
robot.on(steering=0, speed=-20)
accelerationMoveBackward(degrees = DistanceToDegree(2), finalSpeed=10, steering=0)
GyroTurn(steering=-50, angle=gyro.angle)
MoveBackwardBlack(10)
ang = -1 * (88 + gyro.angle)
GyroTurn(steering=-100, angle=ang)
# wall square
if False == Constants.STOP:
robot.on_for_seconds(steering=5, speed=-10, seconds=2.7, brake=False)
# moving towards row machine
acceleration(degrees=DistanceToDegree(3), finalSpeed=50, steering=0)
ang = math.fabs(89 + gyro.angle)
show_text(str(ang))
if ang > 2 and False == Constants.STOP:
GyroTurn(steering=-100, angle=ang)
acceleration(degrees=DistanceToDegree(26), finalSpeed=50, steering=0)
GyroTurn(steering=100, angle=68)
#acceleration(degrees=DistanceToDegree(1), finalSpeed=20, steering=0)
if False == Constants.STOP:
motorC.on_for_seconds(speed=-10, seconds=1.5, brake=False)
GyroTurn(steering=100, angle=2)
sleep(0.2)
GyroTurn(steering=-100, angle=2)
motorC.on_for_seconds(speed=-10, seconds=0.2, brake=True)
motorC.off(brake=True)
acceleration(degrees=DistanceToDegree(1), finalSpeed=20, steering=0)
accelerationMoveBackward(degrees = DistanceToDegree(10), finalSpeed=20, steering=0)
GyroTurn(steering=-100, angle=10)
#DOING Row Machine
if False == Constants.STOP:
motorC.on_for_seconds(speed=20, seconds=2)
ang = 90 + gyro.angle
GyroTurn(steering=-100, angle=ang)
acceleration(degrees=DistanceToDegree(28), finalSpeed=50, steering=0)
lineSquare()
#Moving towards weight machine
show_text(str(gyro.angle))
ang = math.fabs(87 + gyro.angle)
show_text(str(ang))
GyroTurn(steering=100, angle=ang)
acceleration(degrees=DistanceToDegree(22), finalSpeed=30, steering=0)
if False == Constants.STOP:
motorD.on_for_degrees(speed=-20, degrees=160)
GyroTurn(steering=-100, angle=ang)
accelerationMoveBackward(degrees = DistanceToDegree(20), finalSpeed=20, steering=0)
if False == Constants.STOP:
motorD.on_for_seconds(speed=20, seconds=2, brake=True)
lineSquare()
if False == Constants.STOP:
GyroTurn(steering=-40, angle=85)
lineFollowRightPID(degrees=DistanceToDegree(30), kp=1.3, ki=0.01, kd=15, color=colorLeft)
lineFollowTillIntersectionRightPID(kp=1.3, ki=0.01, kd=15, color=colorLeft, color2=colorRight)
lineFollowRightPID(degrees=DistanceToDegree(34), kp=1.3, ki=0.01, kd=15, color=colorLeft)
if False == Constants.STOP:
GyroTurn(steering=50, angle=20)
acceleration(degrees=DistanceToDegree(12), finalSpeed=30, steering=2)
lineSquare()
if False == Constants.STOP:
GyroTurn(steering=100, angle=75)
motorC.on_for_seconds(speed=-10, seconds=1, brake=False)
acceleration(degrees=DistanceToDegree(6.5), finalSpeed=30, steering=0)
motorC.on_for_seconds(speed=10, seconds=2, brake=True)
if False == Constants.STOP:
GyroTurn(steering=50, angle=75)
acceleration(degrees=DistanceToDegree(5), finalSpeed=30, steering=0)
while Constants.STOP == False:
acceleration(degrees=DistanceToDegree(3), finalSpeed=31, steering=0)
accelerationMoveBackward(degrees = DistanceToDegree(3), finalSpeed=30, steering=0)
motorC.off(brake=False)
motorD.off(brake=False)
import math
Xob = 8
Yob = 8
v = 1
x = 0
y = 0
tetha = 0
l = 4
xc = [x]
yc = [y]
motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
gy = GyroSensor()
gy.mode = 'GYRO-RATE'
gy.mode = 'GYRO-ANG'
units = gy.units
angle = gy.value()
print(str(angle))
sleep(1)
def avanzar():
motor_pair.on_for_seconds(steering=0, speed=50, seconds=1)
angle = gy.value()
print(str(angle))
RMC.on(-10)
LMC.on(10)
while(abs(start - gy.value()) <= 90):
sleep(0.001)
RMC.off()
LMC.off()
# Initialization of Sensors & Sensor Vars
RMC.off()
LMC.off()
leds.all_off()
cl.mode='COL-COLOR'
gy.mode='GYRO-ANG'
while cl.value() != HOME:
nagHumans()
sleep(0.25)
# **************************************** MAIN ******************************
foundGoal = False
right = 1
start = int(round(time.time() * 1000))
while not foundGoal:
RMC.on(right)
LMC.on(100)
if int(round(time.time() * 1000)) - start >= 250:
right = right + 1
#!/usr/bin/env python3 import os import sys import time # TODO: implement fast read/write from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_D from ev3dev2.sensor.lego import GyroSensor, TouchSensor touch = TouchSensor() gyro = GyroSensor() gyro.mode = gyro.MODE_GYRO_G_A offset = 0 left = LargeMotor(OUTPUT_D) left.duty_cycle_sp = 0 left.command = left.COMMAND_RUN_DIRECT right = LargeMotor(OUTPUT_A) right.duty_cycle_sp = 0 right.command = right.COMMAND_RUN_DIRECT alpha = 0.8 dc_gain = 1.0 p_gain = 1 i_gain = 0.029 d_gain = 0.049 #dt = 30 / 1000 dt = 250 / 1000
#!/usr/bin/python3
from multiprocessing import Process, Value
from time import sleep
from ev3dev2.motor import OUTPUT_A, OUTPUT_C, SpeedPercent, MoveDifferential
from ev3dev2.sensor import INPUT_4
from ev3dev2.sensor.lego import GyroSensor
from ev3dev2.wheel import EV3EducationSetTire
from ev3_toys.logger import get_logger
# done = False
gs = GyroSensor(INPUT_4)
# reset the angle and rate values for the Gyro by changing the mode
gs.mode = GyroSensor.MODE_GYRO_CAL
sleep(1)
gs.mode = GyroSensor.MODE_GYRO_RATE
gs.mode = GyroSensor.MODE_GYRO_ANG
gs.mode = GyroSensor.MODE_GYRO_G_A
sleep(1)
motors = MoveDifferential(OUTPUT_C, OUTPUT_A, EV3EducationSetTire, 103)
def check_gyro(val: Value, logger, gyro: GyroSensor):
# write to a file for testing
while val.value == 0:
logger.info('(Angle, Rate) = {}\n'.format(gyro.angle_and_rate))
sleep(0.5)
#!/usr/bin/env python3
from ev3dev2.motor import (OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, MediumMotor,
MoveSteering, SpeedPercent)
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, GyroSensor, UltrasonicSensor
medMotor = MediumMotor(OUTPUT_C)
ultrasonic_sensor_front = UltrasonicSensor(INPUT_4)
ultrasonic_sensor_side = UltrasonicSensor(INPUT_2)
ultrasonic_sensor_front.mode = 'US-DIST-CM'
ultrasonic_sensor_side.mode = 'US-DIST-CM'
gyro = GyroSensor(INPUT_1)
gyro.mode = GyroSensor.MODE_GYRO_ANG
drivetrain = MoveSteering(OUTPUT_B, OUTPUT_D)
front_dist = ultrasonic_sensor_front.distance_centimeters
side_dist = ultrasonic_sensor_side.distance_centimeters
while True:
if ultrasonic_sensor_front.distance_centimeters <= 10:
if ultrasonic_sensor_side.distance_centimeters <= 15:
drivetrain.on(steering=100, speed=20)
gyro.wait_until_angle_changed_by(90)
drivetrain.on(steering=0, speed=0)
else:
drivetrain.on(steering=-100, speed=20)
gyro.wait_until_angle_changed_by(90)
drivetrain.on(steering=0, speed=0)
from ev3dev2.sensor.lego import GyroSensor
gy = GyroSensor()
gy.mode = GyroSensor.MODE_TILT_ANG
gy.reset()
def is_in_angle():
gy.mode = GyroSensor.MODE_TILT_ANG
return abs(gy.tilt_angle) > 2
os.system('setfont Lat15-TerminusBold14') # Large font
# os.system('setfont Lat15-TerminusBold32x16') # Very large font
# =======================================================================
# Hardware Definitions
# =======================================================================
# Access the two motors
leftMotor = LargeMotor(OUTPUT_B)
rightMotor = LargeMotor(OUTPUT_D)
print('Motors: Ready')
# The gyro is calibrated as soon as it has power
# So if the robot is not perfectly still there will be drift
# Calibrate the gyro and set it up for measuring angles
gyro = GyroSensor(INPUT_1)
gyro.mode = 'GYRO-CAL' # Calibrate - Robot must be perfectly still
gyro.mode = 'GYRO-ANG' # Angle mode
print('Gyro Sensor: Calibrated')
# =======================================================================
# Main Procedure
# =======================================================================
# Initialise the target motion
# Move forward while turning 20deg clockwise, then straight
forwardSpeed = 30
targetAngle = gyro.angle + 20
# Main program loop
# Pressing the Back button will halt the program
halt = False
while not halt:
import sys
from time import sleep
import ev3dev.ev3 as ev3
from ev3dev2.sensor.lego import GyroSensor
import config
from util import util
FWDLEFT = ev3.LargeMotor("outA") # outA means the motor connect to EV3 port A.
FWDRIGHT = ev3.LargeMotor("outD")
BWDLEFT = ev3.LargeMotor("outB")
BWDRIGHT = ev3.LargeMotor("outC")
GYRO = GyroSensor()
GYRO.mode = "GYRO-ANG"
def stop_when_rotation_complete(angle):
c_angle = GYRO.angle
iteration = 0
last_angle = 0
angle = int(angle)
while abs(GYRO.angle - c_angle) < angle:
if last_angle == GYRO.angle:
iteration += 1
else:
iteration = 0
last_angle = GYRO.angle
MOVE_TANK = MoveTank(OUT_LEFT, OUT_RIGHT)
MOVE_STEERING = MoveSteering(OUT_LEFT, OUT_RIGHT)
DEGREE_SUCCESS_RATE = 0.92
DEGREE_SUCCESS_RATE_NEG = 0.88
METER_IN_MS = 8200
BRAKE_TYPES_LIST = [
("coast", "lets the motor continue to turn until stopped by friction"),
("hold", "an active, forceful effort to stop the motor turning"),
("brake", "passively and less forcefully tries to stop the motor turning"),
]
BRAKE_TYPE = "brake"
GYRO = GyroSensor(INPUT_2)
GYRO.mode = 'GYRO-ANG'
SPEED = 25 # in percent
CUR_DEG = 0
# Call the function to turn the robot with a angle that is pre-defined
def fix_rotation():
turn(-CUR_DEG)
# Call the function to move forward
def forward(t):
print("start")
wait_time = t / 1000 # in seconds
# Connect sensor to sensor port 3
gyro = GyroSensor(INPUT_2)
# allow for some time to load the new drivers
time.sleep(0.5)
print(
"#######################################################################")
print(
"############# starting in gyro angle mode #########################")
print(
"#######################################################################")
#gyro.mode = 'GYRO-ANG'
gyro.mode = 'GYRO-FAS'
gyroVal = 0
prev_gyroVal = -1
time.sleep(2) # give some time to stabilize gyro before reading values
startTime = time.time()
badValCount = 0
while (1):
gyroVal = gyro.angle
# work-around for bogus intermittent readings of '0' (possibly a f/w bug in BrickPi3)
if gyroVal == 0 and abs(gyroVal - prev_gyroVal) > 1:
badValCount += 1
#!/usr/bin/env python3
from ev3dev.ev3 import *
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, MediumMotor, MoveSteering, SpeedPercent
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, GyroSensor, UltrasonicSensor
medMotor = MediumMotor(OUTPUT_C)
ultrasonic_sensor_front = UltrasonicSensor(INPUT_4)
ultrasonic_sensor_side = UltrasonicSensor(INPUT_2)
ultrasonic_sensor_front.mode = 'US-DIST-CM'
ultrasonic_sensor_side.mode = 'US-DIST-CM'
gyro = GyroSensor(INPUT_1)
gyro.mode = 'MODE_GYRO_ANG'
drivetrain = MoveSteering(OUTPUT_B, OUTPUT_D)
drivetrain.on(steering=0, speed=20)
if ultrasonic_sensor_front.distance_centimeters <= 2.0:
drivetrain.on(steering=0, speed=0)
#stop robot if something is at front.
if ultrasonic_sensor_side.distance_centimeters <= 5.0:
drivetrain.on(steering=-100, speed=20)
gyro.wait_until_angle_changed_by(90)
drivetrain.on(steering=0, speed=0)
elif ultrasonic_sensor_side.distance_centimeters > 5.0:
drivetrain.on(steering=100, speed=10)
gyro.wait_until_angle_changed_by(90)
format='%(asctime)s %(levelname)5s: %(message)s')
log = logging.getLogger(__name__)
log.info("Starting Gyro Boy")
# Initialize
running = True
state = "reset"
timer = StopWatch()
timer.start()
# Brick variables
sound = Sound()
leds = Leds()
console = Console()
gyro_sensor = GyroSensor(INPUT_2)
gyro_sensor.mode = GyroSensor.MODE_GYRO_RATE
arms_motor = MediumMotor(OUTPUT_C)
left_motor = LargeMotor(OUTPUT_D)
right_motor = LargeMotor(OUTPUT_A)
color_sensor = ColorSensor(INPUT_1)
ultra_sonic_sensor = UltrasonicSensor(INPUT_4)
class SensorThread(Thread):
"""Sensor thread, based on Color and Ultrasonic sensors."""
def run(self):
global state
global drive_speed
global steering
log.debug("Sensor Thread running!")
import bluetooth
import threading
from time import sleep
from ev3dev2.sound import Sound
from ev3dev2.sensor.lego import GyroSensor, TouchSensor, UltrasonicSensor
from ev3dev2._platform.ev3 import INPUT_1, INPUT_2, INPUT_3, INPUT_4
# SERVER_MAC = 'CC:78:AB:50:B2:46'
SERVER_MAC = '00:17:E9:B2:1E:41'
mission_ongoing = True
s = Sound()
gs = GyroSensor(INPUT_3)
gs.mode = 'GYRO-ANG'
ts_left = TouchSensor(INPUT_1)
ts_right = TouchSensor(INPUT_4)
us_front = UltrasonicSensor(INPUT_2)
us_front.mode = 'US-DIST-CM'
def connect():
port = 3
sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
print('Connecting...')
sock.connect((SERVER_MAC, port))
print('Connected to ', SERVER_MAC)
return sock, sock.makefile('r'), sock.makefile('w')
lml.reset()
lmr.reset()
sleep(0.1)
mt.on(8.1, 8)
while True:
if lml.position > 299 or lmr.position > 299:
mt.off()
break
sleep(0.1)
mt.on(0, trs)
gs.mode = 'GYRO-RATE'
gs.mode = 'GYRO-ANG'
#紅綠燈前左彎
while True:
lml.off()
if gs.angle < -40 :
mt.off()
break
sleep(0.1)
lmr.reset()
lml.reset()
mt.on(10.6, 10.5)
definir a melhor velocidade para a garra abrir e fechar
definir quantas rotações (ou segundos) são necessarias para andar 1 casa
definir os angulos de rotação
definir multiprocessing para andar e fazer a leitura ao mesmo tempo
'''
##########################################################################################
#motores
steer_pair = MoveSteering(OUTPUT_A, OUTPUT_D)
motor_garra = MediumMotor(OUTPUT_B)
#sensores
gyro = GyroSensor()
us = UltrasonicSensor()
# Put the gyro sensor into ANGLE mode.
gyro.mode = 'GYRO-ANG'
us.mode = 'US-DIST-CM'
#constantes
ROTACOES_CASA = 2.2 #cada casa é +/- 2.2 rotações
DISTANCIA_PROCURA = 40 #distância maxima a que o objeto tem de estar
DISTANCIA_MIN = 10 #distância final do robot ao objeto
VELOCIDADE_PADRAO = 30
VELOCIDADE_PROCURA = 20 #velocidade durante a procura, mais lenta para maior precisão
VELOCIDADE_AJUSTE = 10 #velocidade para ajustar a posição do robot, super lento para não andar em excesso aos zig zags
#variaveis globais
#angulos por padrão
ANGULO_AJUSTAMENTO = 2 #angulo para ajustar a direção do robot (andar o mais reto possivel) -- talvez mudar para 0 --