#!/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()