def __init__(self, logger=None):
self._logger = logger or logging.getLogger(__name__)
super().__init__(thread_name='EV3Gyro')
self._gyro = GyroSensor(address='in2')
self._gyro.mode = GyroSensor.MODE_GYRO_ANG
self._gyro.reset()
self._angle = 0
def __init__(self, sensorList=[]):
try:
self.tank = MoveTank(OUTPUT_B, OUTPUT_C)
self.outputList = [OUTPUT_B, OUTPUT_C]
except:
self.tank = None
try:
self.cs = ColorSensor()
except:
self.cs = None
try:
self.gyro = GyroSensor()
except:
self.gyro = None
try:
self.ultrasonicSensor = UltrasonicSensor()
except:
self.ultrasonicSensor = None
try:
self.large = LargeMotor(OUTPUT_D)
self.outputList.append(OUTPUT_D)
except:
self.large = None
try:
self.medium = MediumMotor(OUTPUT_D)
self.outputList.append(OUTPUT_D)
except:
self.medium = None
def __init__(self, output_a, output_d):
self.right_motor = LargeMotor(output_a)
self.left_motor = LargeMotor(output_d)
self.btn = Button()
self.cl = ColorSensor()
self.us = UltrasonicSensor()
self.us.mode='US-DIST-CM'
self.gy = GyroSensor()
self.gy.mode='GYRO-ANG'
self.defaultSpeed = 450
self.right_sp = 450
self.left_sp = 450
self.lastColor1 = None
self.lastColor2 = None
self.lastColor3 = None
self.node = [(0,0)]
# self.direction = None
self.directionStr = ['top', 'right', 'bottom', 'left']
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 __init__(self):
"""Constructor for the internal state of the robot, e.g. in which direction he is
currently looking"""
self.direction = Directions.up
self.gy = GyroSensor()
self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_D, motor_class= LargeMotor)
self.zero_point = ColorSensor().reflected_light_intensity
self.s = Sensors()
def __init__(self, *args, **kwargs):
self.exposed_tank_left = MoveTank(LargeMotor(OUTPUT_A), LargeMotor(OUTPUT_B))
self.exposed_tank_right = MoveTank(LargeMotor(OUTPUT_C), LargeMotor(OUTPUT_D))
self.exposed_gyro_up = GyroSensor(INPUT_1)
self.exposed_gyro_side = GyroSensor(INPUT_2)
self.exposed_stop = TouchSensor(INPUT_3)
super().__init__(*args, **kwargs)
def __init__(self):
self.tank_pair = MoveTank(OUTPUT_A, OUTPUT_B)
self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)
self.line_sensor = ColorSensor(INPUT_2)
self.line_counter = ColorSensor(INPUT_3)
self.sound = Sound()
self.gyro = GyroSensor(INPUT_1)
self.gyro.reset()
self.dir_state = "S"
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 __init__(self):
self.button = Button()
self.tank = MoveSteering(OUTPUT_C, OUTPUT_B)
self.cs = ColorSensor()
self.cs.mode = ColorSensor.MODE_COL_REFLECT
self.gs = GyroSensor()
self.gs.reset()
self.before_direction = self.gyro()
def __init__(self, wheel1 = OUTPUT_B, wheel2 = OUTPUT_C):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motor1 = LargeMotor(wheel1)
self.motor2 = LargeMotor(wheel2)
self.color1 = ColorSensor(INPUT_1)
self.color4 = ColorSensor(INPUT_4)
self._black1 = 0
self._black4 = 0
self._white1 = 100
self._white4 = 100
def __init__(self, motor1Port, motor2Port, gyroPort=None, wheelDiameter=None): #init if GyroSensor != None: self.GS = GyroSensor(gyroPort) else: self.GS = GyroSensor() self.M1 = LargeMotor(motor1Port) self.M2 = LargeMotor(motor2Port) self.motor_stop = True self.wheelDiameter = wheelDiameter self.time = 0 self.MDistanceRunning = True self.distance = 0 self.pauseDistance = [] self.GS.mode = 'GYRO-ANG'
class EV:
def __init__(self):
self.button = Button()
self.tank = MoveSteering(OUTPUT_C, OUTPUT_B)
self.cs = ColorSensor()
self.cs.mode = ColorSensor.MODE_COL_REFLECT
self.gs = GyroSensor()
self.gs.reset()
self.before_direction = self.gyro()
def steer(self, steer, speed=SPEED, interval=INTERVAL):
"""
steer the motor by given params for time intarval [ms]
"""
if self.is_white(): # clockwise
self.tank.on_for_seconds(steer, speed, interval / 1000)
else:
self.tank.on_for_seconds(-steer, speed, interval / 1000)
data = (self._update_direction(), not self.is_white())
sleep(interval / 1000)
return data
def turn_degrees(self, radian):
self.tank.turn_degrees(SPEED, math.degrees(radian))
def on_for_millis(self, millis):
self.tank.on_for_seconds(0, SPEED, millis / 1000)
def gyro(self):
return self.gs.value()
def is_white(self):
return self.cs.value() > 30
# def _send(self, data):
# data = str(data).encode()
# self.socket.send(data)
# print(data)
def _update_direction(self):
current_direction = self.gyro()
m_direction = (current_direction + self.before_direction) / 2
self.before_direction = current_direction
return m_direction
def gyroMove():
"""Test code for gyro PID drive"""
gyro_drive = MoveTankWithGyro(OUTPUT_A, OUTPUT_D)
gyro_drive.gyro = GyroSensor()
sleep(2)
gyro_drive.calibrate_gyro()
target_angle = 90
# Pivot 90 degrees
gyro_drive.pivot_gyro(
speed=SpeedPercent(5),
target_angle=target_angle
)
# Drive straight at the angle
gyro_drive.follow_gyro(
kp=11.3, ki=0.05, kd=3.2,
speed=SpeedPercent(30),
target_angle=target_angle,
follow_for=follow_for_ms,
ms=5000
)
def initmotors():
lw = Motor(OUTPUT_C)
rw = Motor(OUTPUT_B)
# for x in (lw,rw):
# x.polarity = 'inversed'
# x.ramp_up_sp = 2000
# x.ramp_down_sp = 2000
# lw.polarity = 'inversed'
# rw.polarity = 'inversed'
lw.ramp_up_sp = 2000
rw.ramp_up_sp = 2000
lw.ramp_down_sp = 1000
rw.ramp_down_sp = 1000
global mdiff
global mtank
mdiff = MoveDifferential(OUTPUT_C, OUTPUT_B, MCTire, 129.3)
mtank = MoveTank(OUTPUT_C, OUTPUT_B)
mtank.gyro = GyroSensor()
mdiff.set_polarity('inversed')
def __init__(self):
self.tankDrive = MoveTank(OUTPUT_A, OUTPUT_D)
self.motorA = LargeMotor(OUTPUT_A)
self.motorD = LargeMotor(OUTPUT_D)
self.myGyroSensor = GyroSensor()
self.myGripper = RobotGripper()
self.myUltrasonicSensor = UltrasonicSensor()
self.myColorSensor = ColorSensor()
self.Turning = False
self.Moving = False
self.myAngle = 0
self.positionX = 0
self.positionY = 0
self.myDefaultSpeed = 50
self.desiredBarCode = "BWWB"
self.myGPS = IGPS()
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 GyroDrift(gyro=GyroSensor(INPUT_2)):
sound = Sound()
gyro.mode = 'GYRO-RATE'
while math.fabs(gyro.rate) > 0:
show_text("Gyro drift rate: " + str(gyro.rate))
sound.beep()
sleep(0.5)
gyro.mode='GYRO-ANG'
def setup():
global tank_drive, colorSensorLeft, colorSensorRight, ultra, sound, touchSensor, gyroSensor
tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
tank_drive.gyro = GyroSensor('in4')
colorSensorLeft = ColorSensor('in1')
colorSensorRight = ColorSensor('in2')
colorSensorLeft.mode = 'COL-COLOR'
colorSensorRight.mode = 'COL-COLOR'
#ultra = UltrasonicSensor('in3')
touchSensor = TouchSensor('in3')
gyroSensor = GyroSensor('in4')
gyroSensor.calibrate()
sound = Sound()
def povorot(rotation, m1, m2):
gyr = GyroSensor()
k = gyr.angle + rotation
print("povorot")
print(gyr.angle, file=sys.stderr)
print(k, file=sys.stderr)
while abs(gyr.angle-k) > 10:
print(gyr.angle, file = sys.stderr)
m1.on_for_seconds(35,0.2, block=False)
m2.on_for_seconds(-35,0.2, block=False)
time.sleep(0.2)
def __init__(self):
self.gyro = GyroSensor()
self.move = movement.Movement(self.gyro)
self.LEFT = self.move.left_turn
self.RIGHT = self.move.right_turn
self.sensor = ColorSensor()
while True:
self.follow_line(ColorSensor.COLOR_WHITE,
line_color2=ColorSensor.COLOR_YELLOW)
def __init__(self):
"""
Performs Alexa Gadget initialization routines and ev3dev resource allocation.
"""
super().__init__()
# Connect two large motors on output ports B and C
self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
self.grip = MediumMotor(OUTPUT_A)
self.sound = Sound()
self.leds = Leds()
self.ir = InfraredSensor()
self.touch = TouchSensor()
self.gyro = GyroSensor()
self.isComing = False
self.isTaking = False
self.isBringing = False
self.isTurning = False
# Start threads
threading.Thread(target=self._proximity_thread, daemon=True).start()
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)
def GyroTurn(steering, angle, gyro = GyroSensor(INPUT_2), steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)):
"""Function to do precise turns using gyro sensor
Input steering and angle to turn. Angle must be a +ve value
gyro: gyro sensor if different than default
steer_pair: MoveSteering if different than default
"""
if True == Constants.STOP: return
gyro.mode='GYRO-ANG'
steer_pair.on(steering = steering, speed = 15)
gyro.wait_until_angle_changed_by(abs(angle))
steer_pair.off()
def set_up_sensors_motors(self):
"""
Creates all sensors and motors or returns None or the error
"""
try:
self.leds = Leds()
self.left_color_sensor = ColorSensor(LEFT_COLOR_SENSOR_INPUT)
self.cs = self.left_color_sensor
self.left_gyro_sensor = GyroSensor(LEFT_GYRO_SENSOR_INPUT)
self.right_gyro_sensor = GyroSensor(RIGHT_GYRO_SENSOR_INPUT)
self.gyro = self.right_gyro_sensor
self.right_color_sensor = ColorSensor(RIGHT_COLOR_SENSOR_INPUT)
self.left_medium_motor = MediumMotor(LEFT_MEDIUM_MOTOR_PORT)
self.right_medium_motor = MediumMotor(RIGHT_MEDIUM_MOTOR_PORT)
self.left_large_motor = LargeMotor(LEFT_LARGE_MOTOR_PORT)
self.right_large_motor = LargeMotor(RIGHT_LARGE_MOTOR_PORT)
except Exception as e:
self.write_to_console(str(e), column=1, row=3, reset_console=True, inverse=True, alignment='L', font_size='M')
return e
else:
return None
def __init__(self,
wheel1=OUTPUT_B,
wheel2=OUTPUT_C,
wheel3=OUTPUT_A,
wheel4=OUTPUT_D):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motor1 = LargeMotor(wheel1)
self.motor2 = LargeMotor(wheel2)
self.motor3 = MediumMotor(wheel3)
self.motor4 = MediumMotor(wheel4)
self.color1 = ColorSensor(INPUT_1)
self.color4 = ColorSensor(INPUT_4)
self._black1 = 0
self._black4 = 0
self._white1 = 100
self._white4 = 100
self.gyro.mode = 'GYRO-ANG'
self.led = Leds()
self.btn = Button()
self.btn._state = set()
class GyroCar:
def __init__(self, left_motor_port, right_motor_port, gyro_mode):
self.__moveSteering = MoveSteering(left_motor_port, right_motor_port)
self.__gyro = GyroSensor()
self.__gyro.mode = gyro_mode
def __run_forward(self):
self.__moveSteering.on(0, 20)
def run_rotations(self, rotations):
self.__moveSteering.on_for_rotations(0, 20, rotations)
def stop(self):
self.__moveSteering.off()
def __back_and_turn(self, steering):
self.__moveSteering.on_for_rotations(-steering, 20, -1)
def reset_angle(self):
self.__gyro.reset()
def is_on_flat(self):
print("is_on_flat: %d" % self.__gyro.angle)
time.sleep(1)
return (math.fabs(self.__gyro.angle) < 10)
def is_on_slope(self):
print("is_on_slope: %d" % self.__gyro.angle)
time.sleep(1)
return (math.fabs(self.__gyro.angle) >= 10)
def run(self):
self.__run_forward()
@property
def angle(self):
return self.__gyro.angle
class Gyro(SimplePeriodicWorkerThread):
def __init__(self, logger=None):
self._logger = logger or logging.getLogger(__name__)
super().__init__(thread_name='EV3Gyro')
self._gyro = GyroSensor(address='in2')
self._gyro.mode = GyroSensor.MODE_GYRO_ANG
self._gyro.reset()
self._angle = 0
def perform_cycle(self):
# Occasionally, the EV3 gyro sensor gives some exeptions. Usually it happens
# for a few seconds after it is started. Maybe its initialization is not yet
# complete. According to experiments, we can ignore this.
try:
self._angle = self._gyro.angle
except:
self._logger.warning('Unable to get angle from EV3 gyro sensor')
def get_orientation(self):
# It is ok to read a little outdated data
return self._angle
def reset(self):
self._gyro.reset()
def __init__(self, motorL=None, motorR=None, motorM=None, gyroPort=None, colorL=None, colorR=None, colorM=None):
if motorL: self.motorL = LargeMotor(motorL)
if motorR: self.motorR = LargeMotor(motorR)
if motorM: self.motorM = Motor(motorM)
if gyroPort:
self.gyro = GyroSensor(gyroPort)
self.gyro.mode = 'GYRO-CAL'
time.sleep(0.2)
self.gyro.mode = 'GYRO-ANG'
if colorL: self.colorL = ColorSensor(colorL)
if colorR: self.colorR = ColorSensor(colorR)
if colorM: self.colorM = ColorSensor(colorM)
if motorL and motorR:
self.drive = MoveSteering(motorL, motorR)
self.move = MoveTank(motorL, motorR)
print("Successful initialization!")
def GyroDrift(gyro=GyroSensor(INPUT_2)):
'''
Checking if the Gyro sensor's output is drifing and moving when the actual sensor is not.
we need to do this becuase if the sensor is drifting then our GyroTurn's will not work correctly
'''
sound = Sound() #Creating sound Shortcut
gyro.mode = 'GYRO-RATE' #setting gyro mode
while math.fabs(
gyro.rate
) > 0: #while gyro is drifting loop, if it is not drifting then the loop will not take place
show_text(
"Gyro drift rate: " +
str(gyro.rate)) #Showing the rate of how much gyro is drifting
sound.beep() # Beep to indicate that it is displaying current value
sleep(0.5) #waiting for value to change
gyro.mode = 'GYRO-ANG' #reseting gyro mode for our use in other functions
def GyroTurn(steering,
angle,
gyro=GyroSensor(INPUT_2),
steer_pair=MoveSteering(OUTPUT_A, OUTPUT_B)):
"""Function to do precise turns using gyro sensor
Input steering and angle to turn. Angle must be a +ve value
gyro: gyro sensor if different than default
steer_pair: MoveSteering if different than default
"""
if True == Constants.STOP: return #
gyro.mode = 'GYRO-ANG' #setting gyro value mode
steer_pair.on(
steering=steering, speed=15
) #starting to turn using MoveSteering. If steering equals 50 then it will do a pivot turn, if it is 100 then it will do a spin turn
gyro.wait_until_angle_changed_by(
abs(angle)) #keeps turning until the correct angle is reached
steer_pair.off() #stopping the turning after target angle is reached
