#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank from ev3dev2.sensor import INPUT_1 from ev3dev2.sensor.lego import TouchSensor from ev3dev2.led import Leds drive = MoveTank(OUTPUT_B, OUTPUT_C) drive.on_for_seconds(SpeedPercent(70), SpeedPercent(70), 30)
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)
#!/usr/bin/env python3
# (MicroPython does not yet support Display as of 2020)
from ev3dev2.motor import LargeMotor, MediumMotor, MoveTank, OUTPUT_B, OUTPUT_C, OUTPUT_A
from ev3dev2.display import Display
from ev3dev2.sound import Sound
MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A)
TANK_DRIVER = MoveTank(left_motor_port=OUTPUT_B,
right_motor_port=OUTPUT_C,
motor_class=LargeMotor)
SCREEN = Display()
SPEAKER = Sound()
SCREEN.image_filename(filename='/home/robot/image/Pinch left.bmp',
clear_screen=True)
SCREEN.update()
TANK_DRIVER.on_for_rotations(left_speed=75,
right_speed=75,
rotations=2,
brake=True,
block=True)
MEDIUM_MOTOR.on_for_rotations(speed=75, rotations=3, brake=True, block=True)
TANK_DRIVER.on_for_rotations(left_speed=-75,
right_speed=-75,
rotations=2,
brake=True,
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.led import Leds
from ev3dev2.sensor.lego import ColorSensor
# TODO: Add code here
now = ColorSensor()
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.on_for_degrees(30, 30, 30) # (A)
while True: # search "one"
if now.color == 2 or now.color == 3 or now.color == 4 or now.color == 5:#
one = now.color # color number
tank.on_for_degrees(15,15,60) # (B)
break
elif now.color == 0 or now.color == 1 or now.color == 6 or now.color == 7: # no color or black or white
tank.on_for_degrees(15,15,45) # go ahead slight
while True: # search "two"
if now.color == 2 or now.color == 3 or now.color == 4 or now.color == 5: #
two = now.color # color number
tank.on_for_degrees(15,15,60) # (B)
break
elif now.color == 0 or now.color == 1 or now.color == 6 or now.color == 7: # no color or black or white
tank.on_for_degrees(15,15,45) # go ahead slight
while True: # search "three"
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_3, INPUT_4
from ev3dev2.sensor.lego import ColorSensor, UltrasonicSensor
from ev3dev2.sound import Sound
from time import sleep
check_white = 0
chage_lotation = 0
cs = ColorSensor(INPUT_4)
ul = UltrasonicSensor(INPUT_3)
tank_drive = MoveTank(left_motor_port=OUTPUT_A, right_motor_port=OUTPUT_B)
#라인 따라 가기
while True:
if ul.distance_centimeters() < 60:
break
else:
if cs.ambient_light_intensity < 30:
if check_white == 1:
check_white = 0
if chage_lotation == 0:
chage_lotation = 1
else:
chage_lotation = 0
else:
tank_drive.on(100, 100)
else:
check_white = 1
if chage_lotation == 0:
tank_drive.on(50, 100)
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, MoveTank, MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D
from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
import xml.etree.ElementTree as ET
import threading
import time
from sys import stderr
tank_block = MoveTank(OUTPUT_B, OUTPUT_C)
largeMotor_Left = LargeMotor(OUTPUT_B)
largeMotor_Right = LargeMotor(OUTPUT_C)
#_________________________________________________________________________________________________________________________________
def Tank_seconds(stop, left_speed, right_speed, seconds):
# turn the motors on for a number of seconds
print("In tank_seconds", file=stderr)
start_time = time.time()
tank_block.on(right_speed=right_speed, left_speed=left_speed)
while time.time() < start_time + seconds:
if stop():
break
tank_block.off()
print('Leaving Tank_seconds', file=stderr)
#stopProcessing=False
#Tank_seconds(lambda:stopProcessing, left_speed=30, right_speed=30, rotations=5)
"""
Wrapper for the Motor functionality of ev3
Default motor pair set to A, B. Update using SetTankFunc
"""
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
tank = MoveTank(OUTPUT_A, OUTPUT_B)
MotorTable = dict()
MotorTable['A'] = OUTPUT_A
MotorTable['B'] = OUTPUT_B
MotorTable['C'] = OUTPUT_C
def SetTankFunc(param):
data = param.split()
global tank
tank = MoveTank(MotorTable[data[0]], MotorTable[data[1]])
def MoveTankFunc(param):
if tank is None:
return
data = param.split()
tank.on(SpeedPercent(int(data[0])), SpeedPercent(int(data[1])))
def MoveTankDegreesFunc(param):
if tank is None:
return
data = param.split()
tank.on_for_degrees(SpeedPercent(int(data[0])), SpeedPercent(int(data[1])),
# NOTE: change to micropython for optimized performance (but no sounds)
'''example scripts that you can copy to your own files'''
import os
import sys
import time
import random
from ev3dev2.sound import Sound
from ev3dev2.power import PowerSupply
from ev3dev2.button import Button
from ev3dev2.motor import OUTPUT_A, OUTPUT_D, MoveTank
from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM
from ev3dev2.sensor import INPUT_1, INPUT_2
from ev3dev2.sensor.lego import ColorSensor, InfraredSensor
mt = MoveTank(OUTPUT_A, OUTPUT_D)
irs = InfraredSensor(INPUT_2)
######## MANUAL CONTROLS (copy-paste it in main()) ##################################################
manual_controls = True
if manual_controls:
irs.on_channel1_top_left = turn_left
irs.on_channel1_top_right = turn_right
irs.on_channel1_bottom_left = reverse
irs.on_channel1_beacon = straight_forward
def turn_left(state):
while state:
from time import sleep
from ev3dev2.motor import OUTPUT_A,OUTPUT_B,MoveTank,SpeedPercent
import Anda as go
import mapa as ma
rodas=MoveTank(OUTPUT_A,OUTPUT_B)
class mapa: # com regra da mao direita
memoria_cor = {}
Car = go.Anda()
def intervalo(max,min,valor):
if(valor > min and valor < max):
return True
return False
def testa(graus):
rodas.on_for_seconds(-20,-20,1)
Car.virar(graus)
rodas.on_for_seconds(-20,-20,1)
while(intervalo(cor_recebida-5,cor_recebida+5,sensor1.value()) or intevalo(131,120,cor_recebidsensor1.value()) or intevalo(1$
if(cor_atual <131 and cor_atual>120):
Car.frente()
if(cor_atual <14 and cor_atual > 0):
Car.virar(180)
break
else:
memoria_cor[cor_recebida] = graus
return 0
break
def procura(cor_recebida): # Aqui quer dizer que ele viu uma nova cor
boolean = 1
class EV3DEV(object):
def __init__(self):
self.exit = True
self.callback_exit = True
# Connect sensors and buttons.
self.btn = Button()
self.ir = InfraredSensor()
self.ts = TouchSensor()
self.power = PowerSupply()
self.tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
print('EV3 Node init starting')
rospy.init_node('ev3_robot', anonymous=True, log_level=rospy.DEBUG)
print('EV3 Node init complete')
rospy.Subscriber('ev3/active_mode',
String,
self.active_mode_callback,
queue_size=1)
self.power_init()
print('READY!')
def active_mode_callback(self, data):
try:
rospy.logdebug('Active mode: {}'.format(data))
self.active_mode = data.data
self.check_thread()
if data.data == 'charge':
thread = self.ir_init()
sleep(1)
self.charge(100, 15)
thread.do_run = False
thread.join()
elif data.data == 'halt':
self.halt()
elif data.data == 'return':
self.return_home()
elif data.data == 'square':
self.square()
elif data.data == 'snake':
self.snake()
elif data.data == 'spin':
self.spin()
elif data.data == 'tracking':
self.ros_drive('tracking', 'cmd_vel')
sleep(1)
self.halt()
elif data.data == 'joystick':
self.ros_drive('joystick', 'ev3/cmd_vel')
except Exception as e:
rospy.logdebug(e)
def power_init(self):
thread = threading.Thread(target=self.power_thread, args=("task", ))
thread.daemon = True
thread.start()
return thread
def power_thread(self, arg):
while True:
try:
print('{} V'.format(self.power.measured_voltage / 1000000))
print('{} A'.format(self.power.measured_current / 1000000))
sleep(2)
except Exception as e:
rospy.logdebug(e)
break
def ir_init(self):
thread = threading.Thread(target=self.ir_sensor_thread)
thread.daemon = True
thread.start()
return thread
def ir_sensor_thread(self):
t = threading.currentThread()
while getattr(t, "do_run", True):
self.distance = self.ir.proximity
sleep(0.2)
print("Stopping IR thread...")
def check_thread(self):
while not self.exit:
sleep(0.5)
while not self.callback_exit:
sleep(0.5)
def charge(self, speed, min_distance):
while self.distance > min_distance:
self.tank_drive.on(SpeedPercent(speed), SpeedPercent(speed))
self.halt()
def square(self):
for i in range(0, 4):
self.tank_drive.on_for_rotations(SpeedPercent(50),
SpeedPercent(50), 2)
self.tank_drive.on_for_rotations(SpeedPercent(50),
SpeedPercent(-50), 0.94)
self.halt()
def snake(self):
self.halt()
turn1 = 60
turn2 = 30
t = 0.3
for i in range(0, 3):
# "on" function used here with sleep in order to not stop between
# steps and has a smooth transition
self.tank_drive.on(SpeedPercent(turn1), SpeedPercent(turn2))
sleep(t)
self.tank_drive.on(SpeedPercent(turn2), SpeedPercent(turn1))
sleep(t)
self.tank_drive.on(SpeedPercent(turn2), SpeedPercent(turn1))
sleep(t)
self.tank_drive.on(SpeedPercent(turn1), SpeedPercent(turn2))
sleep(t)
self.tank_drive.on(SpeedPercent(0), SpeedPercent(0))
def spin(self):
speed = 100
t = 4
self.tank_drive.on_for_seconds(SpeedPercent(-speed),
SpeedPercent(speed), t)
self.tank_drive.on_for_seconds(SpeedPercent(speed),
SpeedPercent(-speed), t)
self.halt()
def halt(self):
self.tank_drive.on(SpeedPercent(0), SpeedPercent(0))
def return_home(self):
self.tank_drive.on_for_rotations(SpeedPercent(-50), SpeedPercent(-50),
6)
self.halt()
def random_turn(self):
self.tank_drive.on(SpeedPercent(-50), SpeedPercent(-50))
sleep(0.8)
t = uniform(0, 2)
self.tank_drive.on(SpeedPercent(50), SpeedPercent(-50))
sleep(t)
def ros_drive(self, action, topic):
thread = threading.Thread(target=self.ros_drive_thread,
args=(action, topic))
thread.daemon = True
thread.start()
return thread
def ros_drive_thread(self, action, topic):
self.exit = False
sub = rospy.Subscriber(topic, Twist, self.ros_drive_callback)
while self.active_mode == action:
sleep(0.5)
sub.unregister()
print('topic {} unregistered'.format(topic))
self.halt()
self.exit = True
def ros_drive_callback(self, data):
try:
print('x: {} z: {}'.format(data.linear.x, data.angular.z))
self.callback_exit = False
x = data.linear.x
z = data.angular.z
default_speed = 20
speed_factor = 100
turn_factor = 0.628
if self.ts.is_pressed:
self.random_turn()
else:
if x == 0 and z != 0:
if z > 0:
print('left')
mdiff.turn_left(SpeedPercent(default_speed),
degrees(abs(z)),
brake=False,
block=False)
elif z < 0:
print('right')
mdiff.turn_right(SpeedPercent(default_speed),
degrees(abs(z)),
brake=False,
block=False)
elif x > 0:
print('forward')
steering_drive.on(
degrees(z) * turn_factor,
SpeedPercent(x * speed_factor))
elif x < 0:
print('backward')
steering_drive.on(
degrees(z) * turn_factor,
SpeedPercent(x * speed_factor))
elif x == 0 and z == 0:
print('stop')
steering_drive.on(0, SpeedPercent(0))
self.callback_exit = True
except Exception as e:
print(e)
def main():
sound = Sound()
tank_drive = MoveTank(OUTPUT_A, OUTPUT_D)
measurem = Motor(OUTPUT_B)
cs_left = ColorSensor(INPUT_1)
cs_middle = ColorSensor(INPUT_2)
cs_right = ColorSensor(INPUT_3)
us_back = UltrasonicSensor(INPUT_4)
while (not color_Blue or not color_Green or not color_Red):
cs_left_color = cs_left.color_name
cs_middle_color = cs_middle.color_name
cs_right_color = cs_right.color_name
us_back_dist = us_back.distance_centimeters
if (cs_left_color == 'White'):
stay_in_box(tank_drive, 'cs_left')
continue
if (cs_right_color == 'White'):
stay_in_box(tank_drive, 'cs_right')
continue
if (cs_middle_color == 'White'):
stay_in_box(tank_drive, 'cs_middle')
continue
if (us_back_dist > 5):
stay_in_box(tank_drive, 'us_back')
continue
# if rover in correct position to sample
# middle sensor good
if (cs_middle_color == 'Blue' or cs_middle_color == 'Green'
or cs_middle_color == 'Red'):
# also right sensor good
if (cs_right_color == 'Blue' or cs_right_color == 'Green'
or cs_right_color == 'Red'):
detect_colors(tank_drive, cs_middle_color, sound, measurem)
continue
#if rover not in correct position but has detected colour
if (cs_left_color == 'Blue' or cs_left_color == 'Green'
or cs_left_color == 'Red' or cs_middle_color == 'Blue'
or cs_middle_color == 'Green' or cs_middle_color == 'Red'):
turn_left_slow(tank_drive)
continue
if (cs_right_color == 'Blue' or cs_right_color == 'Green'
or cs_right_color == 'Red'):
turn_right_slow(tank_drive)
continue
if (ts_left_feel == 1):
deal_with_touch(tank_drive, 'ts_left')
continue
if (ts_right_feel == 1):
deal_with_touch(tank_drive, 'ts_right')
continue
if (ts_back_feel == 1):
deal_with_touch(tank_drive, 'ts_back')
continue
if (us_front_dist <= 15):
avoid_collision(tank_drive, 'us_front')
continue
drive_forward(tank_drive)
def red():
tank = MoveTank(OUTPUT_A, OUTPUT_B)
m = MediumMotor(OUTPUT_C)
now = ColorSensor()
tank.on_for_rotations(50, 50, 0.46) # turn 90 degrees clock wise
#lineTrace()
while True:
if now.color == 5:
m.on_for_rotations((-90), 0.2)
break
else:
tank.on(15, 15)
tank.on_for_rotations(50, -50, 0.93) # turn 180 degrees clock wise
#linetrace()
while True:
if now.color == 5:
m.on_for_rotations((5), 0.2)
break
else:
tank.on(15, 15)
tank.on_for_rotations(-15, -15, 1)
tank.on_for_rotations(10, -10, 0.93) # turn 90 degrees clock wise
tank.on_for_rotations(50, 50, 2)
tank.on_for_rotations(-30, 30, 0.46)
return True
from ev3dev2.wheel import EV3Tire
from aibot.constants import *
# ----------------------------------------------------------------------
# sensor objects
cs_l = ColorSensor(ADDR_CS_L)
cs_r = ColorSensor(ADDR_CS_R)
ls_f = LightSensor(ADDR_LS_F)
gs = GyroSensor(ADDR_GS)
# motor objects
mot_r = Motor(ADDR_MOT_R)
mot_l = Motor(ADDR_MOT_L)
motors = MoveTank(ADDR_MOT_L, ADDR_MOT_R)
# set object of motors
motors.gyro = gs
motors.cs_r = cs_r
motors.cs_l = cs_l
motors.ls_f = ls_f
# ----------------------------------------------------------------------
# helper methods
def print_cs():
cs_l = motors.cs_l.reflected_light_intensity
from ev3dev2.sensor.lego import TouchSensor, ColorSensor
from ev3dev2.led import Leds
from ev3dev2.button import Button
import time
#our own code
from robot_utils import *
from robot_io import *
from maze import *
from gather_data import start_data_gathering
from run_thor import *
# state constants
NORMAL_SPEED = 42
td = MoveTank(OUTPUT_B, OUTPUT_C) #tank drive
sd = MoveSteering(OUTPUT_B, OUTPUT_C) #steer drive
cs = ColorSensor(INPUT_4)
btn = Button()
def l_shape():
tank_drive = td
accelerate_to(tank_drive, 0, 0, NORMAL_SPEED, NORMAL_SPEED, 1)
time.sleep(2)
accelerate_to(tank_drive, NORMAL_SPEED, NORMAL_SPEED, 0, 0, 1)
turn_90(tank_drive, True)
accelerate_to(tank_drive, 0, 0, NORMAL_SPEED, NORMAL_SPEED, 1)
time.sleep(1)
accelerate_to(tank_drive, NORMAL_SPEED, NORMAL_SPEED, 0, 0, 1)
class Kraz33Hors3:
def __init__(self,
back_foot_motor_port: str = OUTPUT_B,
front_foot_motor_port: str = OUTPUT_C,
gear_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.tank_driver = MoveTank(left_motor_port=back_foot_motor_port,
right_motor_port=front_foot_motor_port,
motor_class=LargeMotor)
self.gear_motor = MediumMotor(address=gear_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def drive_once_by_ir_beacon(self, speed: float = 100):
# forward
if self.ir_sensor.top_left(
channel=self.ir_beacon_channel) and self.ir_sensor.top_right(
channel=self.ir_beacon_channel):
self.tank_driver.on_for_seconds(left_speed=speed,
right_speed=-speed,
seconds=1,
brake=False,
block=True)
# backward
elif self.ir_sensor.bottom_left(channel=self.ir_beacon_channel
) and self.ir_sensor.bottom_right(
channel=self.ir_beacon_channel):
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=speed,
seconds=1,
brake=False,
block=True)
# move crazily
elif self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.gear_motor.on(speed=speed, brake=False, block=False)
self.tank_driver.on_for_seconds(left_speed=-speed / 3,
right_speed=-speed / 3,
seconds=1,
brake=False,
block=True)
else:
self.gear_motor.off(brake=False)
def keep_driving_by_ir_beacon(self, speed: float = 100):
while True:
self.drive_once_by_ir_beacon(speed=speed)
def back_whenever_touched(self, speed: float = 100):
while True:
if self.touch_sensor.is_pressed:
self.tank_driver.on_for_seconds(left_speed=-speed,
right_speed=speed,
seconds=1,
brake=False,
block=True)
def main(self):
Process(target=self.back_whenever_touched, daemon=True).start()
self.keep_driving_by_ir_beacon()
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, MoveTank from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM from ev3dev2.sensor.lego import ColorSensor, GyroSensor from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 driveBase = MoveTank(OUTPUT_B, OUTPUT_C) mtB = LargeMotor(OUTPUT_B) gy1 = GyroSensor(INPUT_1) cl2 = ColorSensor(INPUT_2) cl3 = ColorSensor(INPUT_3) cl4 = ColorSensor(INPUT_4) sensors = [None, gy1, cl2, cl3, cl4]
#!/usr/bin/env micropython
from ev3dev2.motor import LargeMotor, MediumMotor, SpeedPercent, MoveTank, OUTPUT_A, OUTPUT_B #, OUTPUT_C # , OUTPUT_D
from ev3dev2.sensor.lego import TouchSensor #, GyroSensor, ColorSensor
# from ev3dev2.led import Leds
import time
# from math import cos, radians, degrees
# import os
# import sys
tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
# front_motor = MediumMotor(OUTPUT_C)
# top_motor = MediumMotor(OUTPUT_D)
# gs = GyroSensor()
# leds = Leds()
ts = TouchSensor()
ratio_degrees_to_inches = 360 / 8.44
rotate = 135.0 / 90.0
def m11_deliver_the_movie():
# ####################################
# Mission 11 - Deliver The Movie
# ####################################
tank_drive.on_for_degrees(SpeedPercent(50), SpeedPercent(50), ratio_degrees_to_inches * 11.75, brake=True)
tank_drive.on_for_degrees(SpeedPercent(-15), SpeedPercent(15), rotate * 92, brake=True)
tank_drive.on_for_degrees(SpeedPercent(50), SpeedPercent(50), ratio_degrees_to_inches * 59, brake=True)
tank_drive.on_for_degrees(SpeedPercent(-15), SpeedPercent(15), rotate * 55, brake=True)
tank_drive.on_for_degrees(SpeedPercent(50), SpeedPercent(50), ratio_degrees_to_inches * 4., brake=True)
tank_drive.on_for_degrees(SpeedPercent(-15), SpeedPercent(15), rotate * 29, brake=True)
#!/usr/bin/env python3
from ev3dev2.sensor.lego import UltrasonicSensor
from ev3dev2.sensor.lego import GyroSensor
from ev3dev2.motor import MoveTank, OUTPUT_A, OUTPUT_D
from ev3dev2.button import Button
import logging
logging.basicConfig(level=logging.DEBUG,
format='%(asctime)s %(levelname)5s: %(message)s')
log = logging.getLogger(__name__)
gs = GyroSensor()
us = UltrasonicSensor()
tp = MoveTank(OUTPUT_A, OUTPUT_D)
button = Button()
try:
while not button.any():
while not button.any():
tp.on(left_speed=50, right_speed=50)
dist = int(us.distance_centimeters_continuous)
log.info(dist)
if dist < 60:
break
tp.on_for_rotations(50, -50, 1.15)
except (KeyboardInterrupt):
print('interrupt')
tp.on(left_speed=0, right_speed=0)
# logger.info("{}".format(device.name))
ps4gamepad = devices[0].fn # PS4 gamepad
#ps4motion = devices[1].fn # PS4 accelerometer
#ps4touchpad = devices[2].fn # PS4 touchpad
gamepad = evdev.InputDevice(ps4gamepad)
leds = Leds()
remote_leds = remote_led.Leds()
sound = Sound()
waist_motor = LargeMotor(OUTPUT_A)
shoulder_control1 = LargeMotor(OUTPUT_B)
shoulder_control2 = LargeMotor(OUTPUT_C)
shoulder_motor = MoveTank(OUTPUT_B,OUTPUT_C)
elbow_motor = LargeMotor(OUTPUT_D)
roll_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_A)
pitch_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_B)
spin_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_C)
grabber_motor = remote_motor.MediumMotor(remote_motor.OUTPUT_D)
waist_motor.position = 0
shoulder_control1.position = 0
shoulder_control2.position = 0
shoulder_motor.position = 0
elbow_motor.position = 0
roll_motor.position = 0
pitch_motor.position = 0
spin_motor.position = 0
grabber_motor.position = 0
def Crane():
Sound.speak("").wait()
MA = MediumMotor("outA")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("outD")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 12
# change this to whatever speed what you want
left_wheel_speed = -300
right_wheel_speed = -300
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# change the value to how far you want the robot to go. V
while MB.position > -900:
if GY.value() == 0:
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 9
else:
if GY.value() < 0:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
left_wheel_speed = left_wheel_speed + gyro_adjust
right_wheel_speed = right_wheel_speed - gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement
right_wheel_speed = right_wheel_speed + gyro_adjust
left_wheel_speed = left_wheel_speed - gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate:
gyro_adjust = gyro_adjust + 1
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
# wait for the block to drop. V
sleep(1.5)
# pulling away from the crane. V
tank_drive.on_for_rotations(SpeedPercent(20), SpeedPercent(10), 0.50)
# Unlocking attachment. V
MD.on_for_degrees(SpeedPercent(50), 360)
# pulling away from unlocked attachment. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1)
# gyro 90 degree spin turn
while GY.value() < 91:
MB.run_forever(speed_sp=300)
MC.run_forever(speed_sp=-300)
# drive into home
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 4)
MB.stop(stop_action="coast")
MC.stop(stop_action="coast")
Launchrun()
def SetTankFunc(param):
data = param.split()
global tank
tank = MoveTank(MotorTable[data[0]], MotorTable[data[1]])
def Spider():
Sound.speak("").wait()
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
T1 = TouchSensor("")
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
gyro_correct_loops = 0
straight_correct_loops = 0
gyro_correct_straight = 0
# change this to whatever speed what you want
left_wheel_speed = 100
right_wheel_speed = 100
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# change the value to how far you want the robot to go
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 0.95) # Originally 0.9. he robot starts out from base
while GY.value() < 85: #gyro turns 85 degrees and faces towards the swing
left_wheel_speed = 100
right_wheel_speed = -100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
while MB.position > -2326: # this is the gyro program, the first line tells the bot to continue loop until it reaches a defined position
if GY.value() == 90: #this runs if the gyro is OK and already straight, sets a lot of variables as well
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 8
gyro_correct_loops = 0
gyro_correct_straight = 0
straight_correct_loops = 0
else: #if the gyro is off it runs this section of code
if GY.value() < 90:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
right_wheel_speed = right_wheel_speed - gyro_adjust #changes the wheel speeds accordingly
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
gyro_correct_loops = gyro_correct_loops + 1
if GY.value() == 0 and gyro_correct_straight == 0: #runs only if bot isn't already on the original line it started from
while straight_correct_loops < gyro_correct_loops + 1: #Basically this messes up the gyro again so the bot can correct back to the line it was orignally on
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1 #sets this to 1 so that it doesn't go off the line again
gyro_correct_loops = 0
straight_correct_loops = 0
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement, just reversed
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
gyro_correct_loops = gyro_correct_loops + 1
if abs (GY.value()) >= correct_rate:
gyro_adjust = gyro_adjust + 1
if GY.value() == 0 and gyro_correct_straight == 0:
while straight_correct_loops < gyro_correct_loops + 1:
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1
gyro_correct_loops = 0
straight_correct_loops = 0
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
while GY.value() < 120: #this turns the blocks into the circle
left_wheel_speed = 100
right_wheel_speed = -100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 0.18) #Originally 0.2. goes forward slightly to move the blocks all the way into the circle
tank_drive.on_for_rotations(SpeedPercent(30), SpeedPercent(30), 1) #drives back a bit
while GY.value() > 110: #turns to face the launch area backwards
left_wheel_speed = -100 #originaly 200
right_wheel_speed = 100 #originaly 200
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
# Returning to home. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 8.5) #drives back to base. keep speed at 50
MB.stop(stop_action="coast")
MC.stop(stop_action="coast")
Launchrun()
#!/usr/bin/env python3
# An EV3 Python (library v2) solution to Exercise 3
# of the official Lego Robot Educator lessons that
# are part of the EV3 education software
import ev3dev2
from ev3dev2.motor import MoveTank, OUTPUT_B, OUTPUT_C
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.button import Button
from time import sleep
from ev3dev2.motor import SpeedNativeUnits
leftSpeeds=[]
rightSpeeds=[]
btn = Button() # we will use any button to stop script
tank_pair = MoveTank(OUTPUT_B, OUTPUT_C)
leftMotor = ev3dev2.motor.Motor(OUTPUT_B)
rightMotor = ev3dev2.motor.Motor(OUTPUT_C)
# Connect an EV3 color sensor to any sensor port.
cl = ColorSensor()
while not btn.any():
leftSpeeds=[]
rightSpeeds=[]
for x in range(600):
# exit loop when any button pressed
# if we are over the black line (weak reflection)
rintensity=cl.reflected_light_intensity
lmotorpower=-((100/70)*(rintensity-70)+100)
rmotorpower=-(100-((100/70)*(rintensity-70)+100))
leftMotor.on(SpeedNativeUnits(lmotorpower))
rightMotor.on(SpeedNativeUnits(rmotorpower))
class Ev3rstorm:
def __init__(self,
left_foot_motor_port: str = OUTPUT_B,
right_foot_motor_port: str = OUTPUT_C,
bazooka_blast_motor_port: str = OUTPUT_A,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.tank_driver = MoveTank(left_motor_port=left_foot_motor_port,
right_motor_port=right_foot_motor_port,
motor_class=LargeMotor)
self.bazooka_blast_motor = MediumMotor(
address=bazooka_blast_motor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.leds = Leds()
self.speaker = Sound()
def seek_wheeler(self):
self.leds.animate_rainbow(group1=Leds.LEFT,
group2=Leds.RIGHT,
increment_by=0.1,
sleeptime=0.5,
duration=5,
block=True)
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
heading_difference = self.ir_sensor.heading(
channel=self.ir_beacon_channel) - (-3)
proximity_difference = self.ir_sensor.distance(
channel=self.ir_beacon_channel) - 70
if (heading_difference == 0) and (proximity_difference == 0):
self.tank_driver.off(brake=True)
self.leds.animate_rainbow(group1=Leds.LEFT,
group2=Leds.RIGHT,
increment_by=0.1,
sleeptime=0.5,
duration=5,
block=True)
self.bazooka_blast_motor.on_for_rotations(speed=100,
rotations=3,
brake=True,
block=True)
self.speaker.play_file(
wav_file='/home/robot/sound/Laughing 2.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
else:
# FIXME: make it work
self.tank_driver.on(left_speed=max(
min((3 * proximity_difference + 4 * heading_difference) /
5, 100), -100),
right_speed=max(
min((3 * proximity_difference -
4 * heading_difference) / 5, 100),
-100))
else:
self.tank_driver.off(brake=True)
def main(self):
while True:
self.seek_wheeler()
def __init__(self):
self.leftLeg = LargeMotor(OUTPUT_A)
self.rightLeg = LargeMotor(OUTPUT_B)
self.crane = LargeMotor(OUTPUT_C)
self.doubleWalk = MoveTank(OUTPUT_A, OUTPUT_B)
self.doubleJoystick = MoveJoystick(OUTPUT_A, OUTPUT_B)
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_D, SpeedPercent, MoveTank
print("Iniciando Script")
# TODO: Add code here
#m = LargeMotor(OUTPUT_A) motores individuais
#m2 = LargeMotor(OUTPUT_D)
#m.on_for_rotations(SpeedPercent(75), 10)
#m2.on_for_rotations(SpeedPercent(75), 10)
tankMotors = MoveTank(OUTPUT_A, OUTPUT_D)
tankMotors.on_for_rotations(
SpeedPercent(50), SpeedPercent(50),
10) #motores A e D rodam a 50% de velocidade por 10 rotacoes
class Ev3Robot:
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()
def write_color(self, file, value):
# opens a file
f = open(file, "w")
# writes a value to the file
f.write(str(value))
f.close()
def read_color(self, file):
# opens a file
f = open(file, "r")
# reads the value
color = int(f.readline().strip())
f.close()
return color
def pivot_right(self, degrees, speed=20):
# makes the robot pivot to the right until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=speed, right_speed=0)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def pivot_left(self, degrees, speed=20):
# makes the robot pivot to the left until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=0, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def old_spin_right(self, degrees, speed=20):
#old program; don't use
self.gyro.reset()
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed, right_speed=speed * -1)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=-30, right_speed=30)
self.gyro.wait_until_angle_changed_by(value1 - value2 - degrees)
self.stop()
def old_spin_left(self, degrees, speed=20):
#old program; don't use
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed * -1, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=8, right_speed=-8)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees + 5)
self.tank_pair.off()
def dog_gear(self, degrees, motor, speed=30):
if motor == 3:
self.motor3.on_for_degrees(degrees=degrees, speed=speed)
self.motor3.off()
else:
self.motor4.on_for_degrees(degrees=degrees, speed=speed)
def go_straight_forward(self, cm, speed=20, wiggle_factor=1):
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05 #divides by circumference of the wheel
# calculates the amount of degrees the robot has turned, then turns the
# opposite direction and repeats until the robot has gone the required
# number of centimeters
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor * -1,
speed=speed * -1)
self.steer_pair.off()
def go_straight_backward(self, cm, speed=20, wiggle_factor=1):
# see go_straight_forward
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor, speed=speed)
self.steer_pair.off()
def calibrate(self):
# turns the led lights orange, and waits for a button to be pressed
# (signal that the robot is on black), then calculates the reflected
# light intensity of the black line, then does the same on the white line
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._black1 = self.color1.reflected_light_intensity
self._black4 = self.color4.reflected_light_intensity
sleep(2)
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._white1 = self.color1.reflected_light_intensity
self._white4 = self.color4.reflected_light_intensity
sleep(3)
self.write_color("/tmp/black1", self._black1)
self.write_color("/tmp/black4", self._black4)
self.write_color("/tmp/white1", self._white1)
self.write_color("/tmp/white4", self._white4)
def read_calibration(self):
# reads the color files
self._black1 = self.read_color("/tmp/black1")
self._black4 = self.read_color("/tmp/black4")
self._white1 = self.read_color("/tmp/white1")
self._white4 = self.read_color("/tmp/white4")
def align_white(self, speed=20, t=96.8, direction=1):
# goes forward until one of the color sensors sees the white line.
while self.calibrate_RLI(self.color1) < t and self.calibrate_RLI(
self.color4) < t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
# determines which sensor sensed the white line, then moves the opposite
# motor until both sensors have sensed the white line
if self.calibrate_RLI(self.color4) > t:
while self.calibrate_RLI(self.color1) < t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) < t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align_black(self, speed=20, t=4.7, direction=1):
# see align_white
while self.calibrate_RLI(self.color1) > t and self.calibrate_RLI(
self.color4) > t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
if self.calibrate_RLI(self.color4) < t:
while self.calibrate_RLI(self.color1) > t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) > t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align(self, t, speed=-20, direction=1):
# aligns three times for extra accuracy
self.align_white(speed=speed, t=100 - t, direction=direction)
self.align_black(speed=-5, t=t, direction=direction)
self.align_white(speed=-5, t=100 - t, direction=direction)
def calibrate_RLI(self, color_sensor):
# returns a scaled value based on what black and white are
if (color_sensor.address == INPUT_1):
black = self._black1
white = self._white1
else:
black = self._black4
white = self._white4
return (color_sensor.reflected_light_intensity - black) / (white -
black) * 100
def stop(self):
self.tank_pair.off()
def spin_right(self, degrees, speed=30):
self.turn(degrees, 100, speed)
def spin_left(self, degrees, speed=30):
self.turn(degrees, -100, speed)
def turn(self, degrees, steering, speed=30):
# turns at a decreasing speed until it turns the required amount of degrees
value1 = self.gyro.angle
s1 = speed
d1 = 0
while d1 < degrees - 2:
value2 = self.gyro.angle
d1 = abs(value1 - value2)
slope = speed / degrees
s1 = (speed - slope * d1) * (degrees / 90) + 3
self.steer_pair.on(steering=steering, speed=s1)
self.steer_pair.off()
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank
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
speeker = Sound()
tank = MoveTank(OUTPUT_A, OUTPUT_B)
cs = ColorSensor()
screen = Display()
button = Button()
def drive(black, white, speed, endtime):
th = (black + white) / 2
start_time = time()
triggerd = False
while True:
# print(cs.color_name, cs.rgb, cs.hsv)
if cs.color == 5:
print("RED")
speeker.beep()
tank.stop()
sleep(1)
exit()
elif cs.reflected_light_intensity < th:
tank.on(0, speed)
else:
tank.on(speed, 0)
def Redcircle():
Sound.speak("").wait()
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
#Setting the Gyro. V
GY.mode = 'GYRO-ANG'
GY.mode = 'GYRO-RATE'
GY.mode = 'GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
gyro_correct_loops = 0
straight_correct_loops = 0
gyro_correct_straight = 0
# change this to whatever speed what you want. V
left_wheel_speed = 100
right_wheel_speed = 100
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100),
0.01) #wheel alignment
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# change the value to how far you want the robot to go. V
#Pulling out of Launch area. V
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.5)
#Gyro Turn toowards the red circle. V
while GY.value() < 80:
left_wheel_speed = 100
right_wheel_speed = -100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#Driving forward towards the red circle. V
while MB.position > -1270: #was -1280, tessa is changing it to 1270 to stay in circle better
if GY.value() == 90:
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 8
gyro_correct_loops = 0
gyro_correct_straight = 0
straight_correct_loops = 0
else:
if GY.value() < 90:
correct_rate = abs(
GY.value()
) # This captures the gyro value at the beginning of the statement
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs(
GY.value()
) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
gyro_correct_loops = gyro_correct_loops + 1
if GY.value() == 0 and gyro_correct_straight == 0:
while straight_correct_loops < gyro_correct_loops + 1:
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1
gyro_correct_loops = 0
straight_correct_loops = 0
else:
correct_rate = abs(
GY.value()) # Same idea as the other if statement
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
gyro_correct_loops = gyro_correct_loops + 1
if abs(GY.value()) >= correct_rate:
gyro_adjust = gyro_adjust + 1
if GY.value(
) == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line
while straight_correct_loops < gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again
gyro_correct_loops = 0
straight_correct_loops = 0
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#Pulling away from the Red circle and driving into home. V
tank_drive.on_for_rotations(SpeedPercent(65), SpeedPercent(65), 5)
MB.stop(stop_action="coast")
MC.stop(stop_action="coast")
Launchrun()
def __init__(self):
self.mvFollowLine = MVFollowLine()
self.steeringDrive = MoveSteering(OUTPUT_B, OUTPUT_C)
self.moveTank = MoveTank(OUTPUT_B, OUTPUT_C)
self.mediumMotor = MediumMotor(OUTPUT_D)
self.mvInfrared = MVInfraredSensor()
