def randomWalk(tank: MoveDifferential, touchSensor: TouchSensor):
btn = Button()
while not btn.any():
if touchSensor.is_pressed:
tank.off()
else:
tank.on(left_speed = 45, right_speed = 45)
def initmotors():
lw = Motor(OUTPUT_C)
rw = Motor(OUTPUT_B)
# for x in (lw,rw):
# x.polarity = 'inversed'
# x.ramp_up_sp = 2000
# x.ramp_down_sp = 2000
# lw.polarity = 'inversed'
# rw.polarity = 'inversed'
lw.ramp_up_sp = 2000
rw.ramp_up_sp = 2000
lw.ramp_down_sp = 1000
rw.ramp_down_sp = 1000
global mdiff
global mtank
mdiff = MoveDifferential(OUTPUT_C, OUTPUT_B, MCTire, 129.3)
mtank = MoveTank(OUTPUT_C, OUTPUT_B)
mtank.gyro = GyroSensor()
mdiff.set_polarity('inversed')
def __init__(self, left_motor_port, right_motor_port, rot_motor_port,
wheel_class, wheel_distance_mm,
desc=None, motor_class=LargeMotor):
MoveDifferential.__init__(self, left_motor_port, right_motor_port, wheel_class, wheel_distance_mm)
"""
LegoBot Class inherits all usefull stuff for differential drive
and adds sound, LEDs, IRSensor which is rotated by Medium Motor
"""
self.leds = Leds()
self.sound = Sound()
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
# Startup sequence
self.sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
# create IR sensors
self.ir_sensor = InfraredSensor()
self.sensor_rotation_point = Pose( 0.05, 0.0, np.radians(0))
self.sensor_rotation_radius = 0.04
self.sensor_thread_run = False
self.sensor_thread_id = None
# temporary storage for ir readings and poses until half rotation is fully made
self.ir_sensors = None
# initialize motion
self.ang_velocity = (0.0,0.0)
self.rot_motor = MediumMotor(rot_motor_port)
self.rotate_thread_run = False
self.rotate_thread_id = None
self.rotation_degrees = 180
# information about robot for controller or supervisor
self.info = Struct()
self.info.wheels = Struct()
self.info.wheels.radius = self.wheel.radius_mm /1000
self.info.wheels.base_length = wheel_distance_mm /1000
self.info.wheels.max_velocity = 2*pi*170/60 # 170 RPM
self.info.wheels.min_velocity = 2*pi*30/60 # 30 RPM
self.info.pose = None
self.info.ir_sensors = Struct()
self.info.ir_sensors.poses = None
self.info.ir_sensors.readings = None
self.info.ir_sensors.rmax = 0.7
self.info.ir_sensors.rmin = 0.04
# starting odometry thread
self.odometry_start(0,0,0)
# start measuring distance with IR Sensor in another thread while rotating
self.sensor_update_start(self.rot_motor)
# start rotating of medium motor
self.rotate_and_update_sensors_start()
def move_dif(self, speed=15, units=10, backwards=False):
speed = -speed if backwards else speed
self.dif = MoveDifferential(Left_Motor_Port, Right_Motor_Port,
EV3EducationSetTire,
Wheel_Well_diameter * 10)
self.dif.on_for_distance(SpeedPercent(speed), units * 10)
sleep(0.3)
self.dif.off(brake=False)
def test_touch_and_us_sensor_forward(self):
test_args = ["program", "-t", "config_small"]
with patch.object(sys, 'argv', test_args):
sim = Process(target=__main__.main, daemon=True)
sim.start()
cs.client_socket = None
sleep(5)
ts1 = TouchSensor(INPUT_1)
usf = UltrasonicSensor(INPUT_3)
usf.mode = "US-DIST-CM"
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire, 15 * STUD_MM)
tank_drive.turn_right(30, 90)
self.assertEqual(ts1.is_pressed, 0)
sleep(0.2)
self.assertAlmostEqual(usf.value(), 756, delta=10)
val = usf.value()
print(val)
tank_drive.on_for_distance(50, 450)
self.assertAlmostEqual(val - 450, usf.value(), delta=15)
self.assertEqual(False, ts1.is_pressed)
sleep(3)
tank_drive.on_for_rotations(20, 0, 0.3)
self.assertEqual(True, ts1.is_pressed)
cs.client_socket.client.close()
sim.terminate()
class Wheels(MoveSteering):
def __init__(self, left_motor_port, right_motor_port, desc=None, motor_class=LargeMotor):
super().__init__(left_motor_port, right_motor_port, desc, motor_class)
self.diff = MoveDifferential(left_motor_port, right_motor_port, EV3EducationSetTire, 110, desc, motor_class)
def get_rotations(self):
return (self.diff.left_motor.rotations + self.diff.right_motor.rotations) / 2.0
def distance_remaining(self, total_distance, initial_rotations):
return total_distance - ((self.get_rotations() - initial_rotations) * self.diff.circumference_mm)
@staticmethod
def __speed(speed):
return speed if isinstance(speed, SpeedValue) else SpeedPercent(speed)
def on_for_rotations(self, steering, speed, rotations, brake=True, block=True):
super().on_for_rotations(steering, self.__speed(speed), rotations, brake, block)
def on_for_degrees(self, steering, speed, degrees, brake=True, block=True):
super().on_for_degrees(steering, self.__speed(speed), degrees, brake, block)
def on_for_seconds(self, steering, speed, seconds, brake=True, block=True):
super().on_for_seconds(steering, self.__speed(speed), seconds, brake, block)
def on(self, steering, speed):
super().on(steering, self.__speed(speed))
def get_speed_steering(self, steering, speed):
return super().get_speed_steering(steering, self.__speed(speed))
def on_for_distance(self, speed, distance_mm, brake=True, block=True):
self.diff.on_for_distance(self.__speed(speed), distance_mm, brake, block)
def on_arc_right(self, speed, radius_mm, distance_mm, brake=True, block=True):
self.diff.on_arc_right(self.__speed(speed), radius_mm, distance_mm, brake, block)
def on_arc_left(self, speed, radius_mm, distance_mm, brake=True, block=True):
self.diff.on_arc_left(self.__speed(speed), radius_mm, distance_mm, brake, block)
def turn_right(self, speed, degrees, brake=True, block=True):
self.diff.turn_right(self.__speed(speed), degrees, brake, block)
def turn_left(self, speed, degrees, brake=True, block=True):
self.diff.turn_left(self.__speed(speed), degrees, brake, block)
def turn(self, speed, degrees, brake=True, block=True):
(self.turn_left if degrees < 0 else self.turn_right)(speed, degrees, brake, block)
def get_tank_drive():
"""
Loads the setup of the motors of a Robot. Contains the motor outputs, the type of tire and
the distance between the two wheels of the robot.
:return: A tank_drive setup with the two motors, tire type and distance between tires.
"""
return MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire, 15 * STUD_MM)
STUD_MM = 8
INCH_MM = 25.4
ONE_FOOT_CICLE_RADIUS_MM = (12 * INCH_MM) / 2
ONE_FOOT_CICLE_CIRCUMFERENCE_MM = 2 * pi * ONE_FOOT_CICLE_RADIUS_MM
# Testing with RileyRover
# http://www.damienkee.com/home/2013/8/2/rileyrover-ev3-classroom-robot-design.html
#
# The centers of the wheels are 16 studs apart but this is not the
# "effective" wheel seperation. Test drives of circles with
# a diameter of 1-foot shows that the effective wheel seperation is
# closer to 16.3 studs. ndward has a writeup that goes into effective
# wheel seperation.
# https://sites.google.com/site/ev3basic/ev3-basic-programming/going-further/writerbot-v1/drawing-arcs
mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16.3 * STUD_MM)
# This goes crazy on brickpi3, does it do the same on ev3?
#mdiff.on_for_distance(SpeedRPM(-40), 720, brake=False)
#mdiff.on_for_distance(SpeedRPM(40), 720, brake=False)
# Test arc left/right turns
#mdiff.on_arc_right(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM / 4)
mdiff.on_arc_left(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM,
ONE_FOOT_CICLE_CIRCUMFERENCE_MM)
# Test turning in place
#mdiff.turn_right(SpeedRPM(40), 180)
#mdiff.turn_left(SpeedRPM(40), 180)
#!/usr/bin/env python3
from ev3dev.ev3 import *
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedRPM, MoveDifferential, MoveTank
from ev3dev2.wheel import EV3Tire
from time import sleep
leftMotor = LargeMotor('outB')
rightMotor = LargeMotor('outC')
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
# Arvoja pituuden mittaamista varten
STUD_MM = 8
mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM)
def runStraight(speed, distanceMM):
mdiff.on_for_distance(SpeedRPM(speed), distanceMM)
# Aja suoraan pitkä 240
runStraight(-100, 240)
## MUTKA LÄHDÖSTÄ
tank_drive.on_for_degrees(-100, 100, 450) #vasen käännös 450, suora 710
## SUORA
runStraight(-100, 710)
## EKA KULMA
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, MoveDifferential from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS from ev3dev2.wheel import EV3Tire from time import sleep from ev3dev2.motor import MediumMotor, OUTPUT_A medium_motorA = MediumMotor(OUTPUT_A) large_motorB = LargeMotor(OUTPUT_B) large_motorC = LargeMotor(OUTPUT_C) STUD_MM = 8 mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) mdiff.on_for_distance(speed = 50, distance_mm = 690) sleep(5) mdiff.turn_left(speed = 50, degrees = 80) mdiff.on_arc_right(SpeedRPM(60), 150, 65) mdiff.on_for_distance(speed = 50, distance_mm = -145) sleep(5) medium_motorA.on_for_degrees(speed = 10, degrees = -110) mdiff.on_arc_left(SpeedRPM(60), 150,67) sleep(10) mdiff.on_for_distance(10,50) #sleep while picking up the first green piece #medium_motorA.on_for_degrees(speed = 10, degrees = -110) sleep(5) #mdiff.turn_left(speed = 35, degrees = 10) sleep(2) medium_motorA.on_for_degrees(speed = 10, degrees = 110) sleep(2) mdiff.on_arc_right(SpeedRPM(60), 5200, 400) sleep(5) medium_motorA.on_for_degrees(speed = 10, degrees = -110)
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, MoveDifferential from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS from ev3dev2.wheel import EV3Tire from time import sleep from ev3dev2.motor import MediumMotor, OUTPUT_A medium_motorA = MediumMotor(OUTPUT_A) large_motorB = LargeMotor(OUTPUT_B) large_motorC = LargeMotor(OUTPUT_C) STUD_MM = 8 #created mdiff object mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) mdiff.on_for_distance(speed=50, distance_mm=-50) mdiff.turn_left(speed=50, degrees=60) mdiff.on_for_distance(speed=50, distance_mm=145) mdiff.turn_right(speed=50, degrees=60) mdiff.on_for_distance(speed=50, distance_mm=675) sleep(2) medium_motorA.on_for_degrees(speed=50, degrees=-110) sleep(2) medium_motorA.on_for_degrees(speed=50, degrees=110)
#!/usr/bin/env python3
from ev3dev.ev3 import *
from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedRPM, MoveDifferential, MoveTank
from ev3dev2.wheel import EV3Tire
from time import sleep
leftMotor = LargeMotor('outB')
rightMotor = LargeMotor('outC')
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
# Arvoja pituuden mittaamista varten
STUD_MM = 8
mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM)
colSens = ColorSensor()
colSens.mode = 'COL-REFLECT'
def isWhite():
return colSens.value() > 15
def runStraight(speed, distanceMM):
mdiff.on_for_distance(SpeedRPM(speed), distanceMM)
# Aloita etsimällä valkoinen viiva
while not isWhite():
tank_drive.run_forever(speed_sp=-100)
# Ohita viiva robotin suoristamista varten
def drive():
tank_drive.on(SpeedPercent(30), SpeedPercent(30))
def checkColor():
if cs.color != 6:
tank_drive.stop()
sound.speak('gg', play_type=1)
reverseRotations(1)
rotateDegrees(80)
drive()
def check():
while True:
checkColor()
cs = ColorSensor()
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire,
15 * STUD_MM)
sound = Sound()
us = UltrasonicSensor()
us.mode = 'US-DIST-CM'
tank_drive.turn_right(30, 90)
drive()
check()
class chassis:
# Sets up motors
def __init__(self):
self.right = LargeMotor(Right_Motor_Port)
self.left = LargeMotor(Left_Motor_Port)
self.chassis = MoveTank(Left_Motor_Port, Right_Motor_Port)
self.rot_conversion = lambda distance: distance / CONVERSION_TO_CM # This will convert from a distance to a
self.sp_conversion = lambda distance: 360 * self.rot_conversion(
distance)
# theoretical number of rotations
self.degree_conversion = lambda deg: self.rot_conversion(
(deg * pi * Wheel_Well_diameter
) / 360) # Will convert from degrees into a distance
# Send move comand but backwards, just for ease if I use it
def move_backwards(self, units=10, speed=15, backwards=True):
self.move(units, speed, backwards)
# Move forward, units being in cm
def move(self, units=10, speed=15, forwards=False):
# Speed forward if forwards is ture or go back
speed = speed if forwards else (-speed)
self.chassis.on_for_rotations(SpeedPercent(speed),
SpeedPercent(speed),
self.rot_conversion(units),
brake=True)
self.chassis.off(brake=False)
def move_rel(self, units=10, speed=15, forwards=False):
speed = speed if forwards else (-speed)
self.chassis.run_to_rel_pos()
# Move to a relative position forward
def turn_counter_clockwise(self,
degrees=180,
speed=10,
counter_clockwise=True):
self.turn_clockwise(degrees, speed, not counter_clockwise)
def turn_clockwise(self, degrees=180, speed=10, clockwise=True):
speeds = SpeedPercent(-speed)
speedb = SpeedPercent(speed)
self.chassis.on_for_rotations(speedb, speeds,
self.degree_conversion(degrees))
self.chassis.off(brake=False)
def move_dif(self, speed=15, units=10, backwards=False):
speed = -speed if backwards else speed
self.dif = MoveDifferential(Left_Motor_Port, Right_Motor_Port,
EV3EducationSetTire,
Wheel_Well_diameter * 10)
self.dif.on_for_distance(SpeedPercent(speed), units * 10)
sleep(0.3)
self.dif.off(brake=False)
def turn_angle(self, angle=180):
self.turn = MoveDifferential(Left_Motor_Port, Right_Motor_Port,
EV3EducationSetTire,
Wheel_Well_diameter * 10)
self.turn.turn_left(15, angle)
def position(self):
return [self.right.position, self.left.position]
def odo_start(self):
self.dif.odometry_start()
def odo_stop(self):
self.dif.odometry_stop()
from runner import Runner
from ev3dev2.unit import STUD_MM
from robot import Robot
from ev3dev2.sensor.lego import UltrasonicSensor, ColorSensor
from ev3dev2.wheel import EV3EducationSetTire
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, OUTPUT_D, MoveDifferential, Motor
from utils import SensorMap, BluetoothMaster
from celebrations import *
from mission import Mission
MAC_ADDRESS = '00:1a:7d:da:71:10' # Mac address via which the Robots connect with bluetooth
PORT = 4 # Port on which the Robots connect with bluetooth
sensor_map_master = {
'tank_drive':
MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire, 15 * STUD_MM),
'measurement_motor':
Motor(OUTPUT_B),
'cs_l':
ColorSensor('ev3-ports:in1'),
'cs_m':
ColorSensor('ev3-ports:in2'),
'cs_r':
ColorSensor('ev3-ports:in3'),
'us_b':
UltrasonicSensor('ev3-ports:in4')
}
def create_runner():
r = Robot(SensorMap(sensor_map_master),
from ev3dev2.power import PowerSupply
from time import sleep
import robot_util
import threading
import importlib
tts_module = importlib.import_module('tts.ev3')
STUD_MM = 8
# TODO: Add code here
speed = 20
sound = Sound()
tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
steering_drive = MoveSteering(OUTPUT_A, OUTPUT_B)
mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3EducationSetRim, 16 * STUD_MM)
btn = Button()
arm = MediumMotor()
lightswitch = LargeMotor(OUTPUT_D)
lightswitch.position = 0
arm.position = 0
supply = PowerSupply()
mdiff.odometry_start()
scriptname = "/hardware/ev3.py"
debug = True
stationary_mode = False
movements = []
# drive in a turn for 5 rotations of the outer motor
# the first two parameters can be unit classes or percentages.
from time import sleep, time from crossDetection import crossDetection from ev3dev2.motor import OUTPUT_A, OUTPUT_B, SpeedPercent, MoveDifferential, SpeedRPM from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3 from ev3dev2.sensor.lego import ColorSensor, TouchSensor from ev3dev2.wheel import EV3Tire from linefollower import LineFollower # rightMotor = LargeMotor(OUTPUT_A) # leftMotor = LargeMotor(OUTPUT_B) # rightMotor.command = rightMotor.COMMAND_RUN_DIRECT # leftMotor.command = leftMotor.COMMAND_RUN_DIRECT mdiff = MoveDifferential(OUTPUT_B, OUTPUT_A, EV3Tire, 100)
from ev3dev2.button import Button
from time import sleep
from ev3dev.ev3 import Sound
import sys, threading, math, pickle, copy, bluetooth
import grid
# ################################################################################################################# #
# MOTOR METHODS
speed = 20
wheelDiameter = 56
wheelBase = 115
rightMotor = LargeMotor(OUTPUT_D)
leftMotor = LargeMotor(OUTPUT_A)
moveTank = MoveTank(OUTPUT_A, OUTPUT_D)
moveDiff = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire, 115)
ultrasonicS = UltrasonicSensor()
ultrasonicM = MediumMotor(OUTPUT_B)
colorS = ColorSensor()
ultrasonicS.mode = 'US-DIST-CM'
colorS.mode = 'COL-COLOR'
def calculateRotationsForDistance(distance, wheelDiameter):
return distance/(math.pi*wheelDiameter)
def moveForwardMotor(motor, wheelDiameter, distance):
motor.on_for_rotations(speed, calculateRotationsForDistance(distance, wheelDiameter))
def moveForwardTank(distance):
#coding: utf-8
from ev3dev.ev3 import *
from threading import *
from ev3dev2.motor import OUTPUT_C, OUTPUT_D, MoveDifferential, SpeedRPM
from ev3dev2.wheel import EV3EducationSetTire
from ev3dev2.sensor import INPUT_1
from ev3dev2.sensor.lego import GyroSensor
import time, socket
import math, json
import colorsys
#------VARIÁVEIS DO PROGRAMA
mdiff = MoveDifferential(OUTPUT_D, OUTPUT_C, EV3EducationSetTire, 105)
#Motores
m1 = LargeMotor('outD') #Esquerdo
m2 = LargeMotor('outC') #Direito
m3 = MediumMotor('outB') #Motor mais alto
m4 = MediumMotor('outA') #Motor mais baixo
#Sensores
Sensor_Cor = [ColorSensor('in1'),
ColorSensor('in2')] #1 = Esquerdo, 2 = Direito
Sensor_Cor[0].mode = 'COL-COLOR'
Sensor_Cor[1].mode = 'COL-COLOR'
Sensor_Ultrassonico = [UltrassonicSensor('in3'),
UltrassonicSensor('in4')] #1 = Esquerdo, 2 = Direito
print('Initializing sensor calibration')
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, SpeedPercent, MoveDifferential, SpeedRPM
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3
from ev3dev2.sensor.lego import ColorSensor, TouchSensor
from ev3dev2.wheel import EV3Tire
from time import time
colSFollower = ColorSensor(INPUT_1)
colSCrossDetect = ColorSensor(INPUT_2)
stopButton = TouchSensor(INPUT_3)
print('Initializing finished')
while (True):
mDiff = MoveDifferential(OUTPUT_B, OUTPUT_A, EV3Tire, 90)
sensorValues = []
mDiff.turn_left(5, 360, block=False)
t = time()
while mDiff.is_running:
sensorValues.append(colSFollower.reflected_light_intensity)
mDiff.wait_until_not_moving()
print("Run time", time() - t)
maxVal = max(sensorValues)
minVal = min(sensorValues)
midVal = minVal + (maxVal - minVal) / 2
meanVal = sum(sensorValues) / len(sensorValues)
print('nrVal', 'maxVal', 'minVal', 'midVal', 'meanVal')
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveDifferential, SpeedRPM from ev3dev2.wheel import EV3Tire STUD_MM = 8 # test with a robot that: # - uses the standard wheels known as EV3Tire # - wheels are 16 studs apart mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 17 * STUD_MM) # Rotate 90 degrees clockwise mdiff.turn_right(SpeedRPM(40), 90 / 1.666667)
import time, os
import cv2
import numpy as np
# logging
logging.basicConfig(level=logging.DEBUG,
format="%(asctime)s %(levelname)5s: %(message)s")
log = logging.getLogger(__name__)
STUD_MM = 8
INCH_MM = 25.4
ONE_FOOT_CICLE_RADIUS_MM = (12 * INCH_MM) / 2
ONE_FOOT_CICLE_CIRCUMFERENCE_MM = 2 * pi * ONE_FOOT_CICLE_RADIUS_MM
mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, MyTire, 16 * STUD_MM)
# Image directory
# directory = r'/home/robot/ev3dev2Projects/noBotDevAndSims/sentdex/ORB_BFMatcher/refDataDayX100'
directory = r'/home/robot/ev3dev2Projects/noBotDevAndSims/sentdex/ORB_BFMatcher/refData'
# Change the current directory
# to specified directory
os.chdir(directory)
# initialize the camera
# cap = cv2.VideoCapture(0)
cap = cv2.VideoCapture(
-1) # work-around for error "can't open camera by index"
# allow the camera to warmup
#!/usr/bin/env python3 from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveDifferential, SpeedRPM from ev3dev2.wheel import EV3Tire STUD_MM = 8 # test with a robot that: # - usa ruedas EV3tire # - wheels are 16 studs apart mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 14 * STUD_MM) # Rotate 90 degrees clockwise mdiff.turn_right(SpeedRPM(40), 90) """ # Drive forward 500 mm mdiff.on_for_distance(SpeedRPM(40), 500) # Drive in arc to the right along an imaginary circle of radius 150 mm. # Drive for 700 mm around this imaginary circle. mdiff.on_arc_right(SpeedRPM(80), 150, 700) # Enable odometry mdiff.odometry_start() # Use odometry to drive to specific coordinates mdiff.on_to_coordinates(SpeedRPM(40), 300, 300) # Use odometry to go back to where we started mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) # Use odometry to rotate in place to 90 degrees
def test_color_and_us_sensor_downwards(self):
test_args = ["program", "-t", "config_large"]
with patch.object(sys, 'argv', test_args):
sim = Process(target=__main__.main, daemon=True)
sim.start()
cs.client_socket = None
sleep(5)
clm = ColorSensor(INPUT_2)
usb = UltrasonicSensor(INPUT_4)
usb.mode = "US-DIST-CM"
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire, 15 * STUD_MM)
self.assertEqual(clm.color, 1)
tank_drive.on_for_rotations(0, -55, 1)
tank_drive.on_for_rotations(10, 0, 0.2)
tank_drive.stop()
self.assertEqual(clm.color, 5)
tank_drive.turn_left(30, -40)
self.assertEqual(usb.value(), 20.0)
tank_drive.turn_left(30, 120)
self.assertEqual(usb.value(), 2550.0)
cs.client_socket.client.close()
sim.terminate()
def drive_motors(distance):
movediff = MoveDifferential(OUTPUT_A, OUTPUT_C, EV3EducationSetTire,
10 * 8)
movediff.on_for_distance(SpeedRPM(20), distance)
def main():
# Setup input ports
colorSensorStop = ev3.LightSensor('in1')
colorSensorLeft = ev3.ColorSensor('in2')
colorSensorRight = ev3.ColorSensor('in3')
assert colorSensorStop.connected, "LightSensorStop(ColorSensor) is not connected"
assert colorSensorLeft.connected, "LightSensorLeft(ColorSensor) is not conected"
assert colorSensorRight.connected, "LightSensorRight(ColorSensor) is not conected"
#colorSensorStop.MODE_REFLECT
#colorSensorLeft.raw
#colorSensorRight.raw
# Setup output ports
mDiff = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3Tire, 15 * STUD_MM)
mDiff.left_motor.polarity = "normal"
mDiff.right_motor.polarity = "normal"
# setup gracefully shutdown
def signal_handler(sig, frame):
print('Shutting down gracefully')
mDiff.stop()
exit(0)
signal.signal(signal.SIGINT, signal_handler)
print('Press Ctrl+C to exit')
# setup decoder and define chain of actions
#string_of_actions = "urrruulldddl"
#string_of_actions = "fffblfrffrfrffb" #"ffffrfrfrfblfflffrflfb"
string_of_actions = "llll.uddllu.r.r.r.r.rdr.u.uruulld.r.rlddllu.luulld.rur.d.dull.d.rd.r.r.rdr.u.uruurrd.lul.dulld.rddlllluur.dld.r.r.rdr.u.udlllldllu.r.r.r.r.rdr.u"
#string_of_actions = "lffffrffbtffrffrfrffffffbrflflfffbrflfflfflflfffbtflffrffrflffbrfflfflflffblfrffffbtflfflffblffbrflffffbrflflfffbrflfflfflflffblfrfrffbtfrffrfrffbrflfflfffrffffrffrfrffbrflflffffbrflflfffbtfrfffflfrffrfrffffffbrflflff"
#string_of_actions = "lfffrfflf"
#string_of_actions = "l"
decoder = Decoder(string_of_actions, DEFINED_ACTIONS)
current_action = decoder.get_next_action()
while current_action != NOT_DEFINED: # current action == -1 -> no more actions to execute
if (current_action == FORWARD):
move_forward(mDiff, colorSensorStop, colorSensorLeft,
colorSensorRight)
current_action = decoder.get_next_action()
#if(current_action == FORWARD_GYRO):
# move_forward_gyro(mDiff, colorSensorStop, colorSensorFollow, gyro)
# current_action = decoder.get_next_action()
# if(current_action == FORWARD):
# move_forward_dual(mDiff, colorSensorStop, colorSensorLeft, colorSensorRight)
# current_action = decoder.get_next_action()
elif (current_action == CONTINUE):
cont_forward(mDiff, colorSensorStop, colorSensorLeft,
colorSensorRight)
current_action = decoder.get_next_action()
elif (current_action == BACKWARD):
move_backward(mDiff, colorSensorStop)
current_action = decoder.get_next_action()
elif (current_action == ONE_EIGHTY):
turn_one_eighty(mDiff)
current_action = decoder.get_next_action()
elif (current_action == LEFT):
turn_left(mDiff)
current_action = decoder.get_next_action()
elif (current_action == RIGHT):
turn_right(mDiff)
current_action = decoder.get_next_action()
else:
raise TypeError(
"No switch case implemented for action/behaviour: {}".format(
current_action))
from ev3dev2.wheel import EV3EducationSetTire
from ev3_toys.logger import get_logger
# done = False
gs = GyroSensor(INPUT_4)
# reset the angle and rate values for the Gyro by changing the mode
gs.mode = GyroSensor.MODE_GYRO_CAL
sleep(1)
gs.mode = GyroSensor.MODE_GYRO_RATE
gs.mode = GyroSensor.MODE_GYRO_ANG
gs.mode = GyroSensor.MODE_GYRO_G_A
sleep(1)
motors = MoveDifferential(OUTPUT_C, OUTPUT_A, EV3EducationSetTire, 103)
def check_gyro(val: Value, logger, gyro: GyroSensor):
# write to a file for testing
while val.value == 0:
logger.info('(Angle, Rate) = {}\n'.format(gyro.angle_and_rate))
sleep(0.5)
value = Value('i', 0)
t = Process(target=check_gyro, args=(value, get_logger('arc.gyro'), gs))
t.start()
motors.on_arc_left(SpeedPercent(50), 200, 1256.64)
def turn_angle(self, angle=180):
self.turn = MoveDifferential(Left_Motor_Port, Right_Motor_Port,
EV3EducationSetTire,
Wheel_Well_diameter * 10)
self.turn.turn_left(15, angle)
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, MoveDifferential from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS from ev3dev2.wheel import EV3Tire from time import sleep from ev3dev2.motor import MediumMotor, OUTPUT_A medium_motorA = MediumMotor(OUTPUT_A) large_motorB = LargeMotor(OUTPUT_B) large_motorC = LargeMotor(OUTPUT_C) STUD_MM = 8 #created mdiff object mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) #mdiff.on_for_distance(speed = 50, distance_mm = -50) mdiff.on_for_distance(speed=50, distance_mm=150) mdiff.turn_left(speed=50, degrees=68) mdiff.on_for_distance(speed=50, distance_mm=335) sleep(2) medium_motorA.on_for_degrees(speed=10, degrees=-110) mdiff.on_for_distance(speed=10, distance_mm=80) sleep(2) #mdiff.on_for_distance(speed = 50, distance_mm = 50) medium_motorA.on_for_degrees(speed=10, degrees=110)
def main():
'''The main function of our program'''
# set the console just how we want it
reset_console()
set_cursor(OFF)
set_font('Lat15-Terminus24x12')
print('How are you?')
print("")
print("Hello Selina.")
print("Hello Ethan.")
STUD_MM = 8
tank = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16 * STUD_MM)
motorLift = MediumMotor(OUTPUT_D)
sound = Sound()
# sound.speak('How are you master!')
# sound.speak("I like my family")
# sound.speak("I like my sister and i like my brother.")
sound.beep()
eye = InfraredSensor(INPUT_1)
robot = Robot(tank, None, eye)
botton = Button()
while not botton.any():
distance = eye.distance(channel=1)
heading = eye.heading(channel=1)
print('distance: {}, heading: {}'.format(distance, heading))
motorLift.on_to_position(speed=40, position=-7200, block=True) #20 Rounds
if distance is None:
sound.speak("I am lost, there is no beacon!")
else:
if (distance < 14):
tank.off()
sound.speak("I am very close to the beacon!")
motorLift.on_to_position(speed=40, position=7200, block=True)
sound.speak("I had to get some more rubbish.")
sound.speak("Please wait while I lift up my fork.")
tank.turn_right(speed=20, degrees=random.randint(290, 340)) # random.randint(150, 210)
tank.on_for_seconds(left_speed=20, right_speed=20, seconds=20)
tank.turn_right(speed=20, degrees=330)
motorLift.on_to_position(speed=40, position=0, block=True)
elif distance >= 100:
sound.speak("I am too faraway from the beacon")
elif (distance < 99) and (-4 <= heading <= 4): # in right heading
sound.speak("Moving farward")
tank.on(left_speed=20, right_speed=20)
else:
if heading > 0:
tank.turn_left(speed=20, degrees=20)
else:
tank.turn_right(speed=20, degrees=20)
sound.speak("I am finding the beacon.")
time.sleep(0.1)
