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)
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()
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 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)
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 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_MoveDifferential(self):
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire,
15 * STUD_MM)
tank_drive.turn_right(20, 90 - 34)
self.assertEqual(tank_drive.is_running, False)
tank_drive.odometry_start()
tank_drive.on_to_coordinates(30, -50, 300)
self.assertEqual(tank_drive.is_running, False)
tank_drive.on_for_distance(30, 300)
self.assertEqual(tank_drive.is_running, False)
tank_drive.on_arc_right(30, 200, 600, block=True)
self.assertEqual(tank_drive.is_running, 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)
#!/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
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)
#!/usr/bin/env python3 from time import sleep from ev3dev2.sound import Sound from ev3dev2.motor import OUTPUT_A, OUTPUT_D, OUTPUT_B, OUTPUT_C, LargeMotor, MediumMotor, SpeedPercent, MoveTank, MoveDifferential, follow_for_ms from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sensor.lego import ColorSensor from ev3dev2.button import * from ev3dev2.wheel import EV3Tire from ev3dev2.sound import Sound #initiate all motors sensors etc. # this is common so that it is controlled in one place and everyone uses the same names for motors etc. STUD_MM = 8 mdiff = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3Tire, 16 * STUD_MM) tank = MoveTank(OUTPUT_A, OUTPUT_D) leftMotor = LargeMotor(OUTPUT_A) rightMotor = LargeMotor(OUTPUT_D) medMotor = MediumMotor(OUTPUT_C) medMotor2 = MediumMotor(OUTPUT_B) tank.cs = ColorSensor(INPUT_3) color1 = ColorSensor(INPUT_1) color2 = ColorSensor(INPUT_2) color3 = ColorSensor(INPUT_3) color4 = ColorSensor(INPUT_4) btn = Button() sound = Sound()
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveDifferential, SpeedRPM
from ev3dev2.wheel import EV3EducationSetRim
from ev3dev2.sound import Sound
from os import remove
from os.path import isfile
from time import sleep
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3EducationSetRim, 250)
sound = Sound()
def move(speed, mm):
print("Moving with {}% speed for {} millimeters".format(speed, mm))
_move_tank(speed, mm)
def rotate(speed, degrees):
print("Rotating {} degrees at {}% speed".format(degrees, speed))
_rotate_tank(speed, degrees)
def say(message):
print("Say: {}".format(message))
sound.speak(message)
def pause(seconds):
print("Pausing for {} seconds".format(seconds))
i = 0
while i < seconds:
if isfile('terminate'):
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)
obstacle_list = []
print("Startimg ...")
file_location = os.path.join(
"/home/robot/catkin_ws/src/lego_smart_env/scripts", "obstacles.txt")
fp = open(file_location, "r")
obstacle_name = fp.readline()
while obstacle_name:
obstacle_list.append(obstacle_name[:-1])
obstacle_name = fp.readline()
print("initializing ros node")
rospy.init_node('lego_node', anonymous=True)
print("ros node initialized")
us = UltrasonicSensor()
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16 * STUD_MM)
tank_drive.odometry_start()
rate = rospy.Rate(10)
pub = rospy.Publisher('/smart_environment/request', String, queue_size=10)
rospy.Subscriber('/smart_environment/response',
String,
rcallback,
queue_size=5)
print("starting loop")
#while not rospy.is_shutdown():
# rate.sleep()
# pass
while not rospy.is_shutdown():
try:
def drive_motors(distance):
movediff = MoveDifferential(OUTPUT_A, OUTPUT_C, EV3EducationSetTire,
10 * 8)
movediff.on_for_distance(SpeedRPM(20), distance)
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):
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)
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 actions import *
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), bluetooth=BluetoothMaster(MAC_ADDRESS, PORT))
mission_Mission = Mission([BorderAction(priority=2), ColorDetAction(colors=[ColorSensor.COLOR_RED], priority=0), DontDrownAction(lakes=[ColorSensor.COLOR_RED, ColorSensor.COLOR_YELLOW, ColorSensor.COLOR_BLUE], priority=1), DriveAction(priority=0)])
Runner(r, [mission_Mission]).run()
if __name__ == '__main__':
create_runner()
from ev3dev2.button import Button
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveDifferential, SpeedPercent
from ev3dev2.sensor import INPUT_3
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.wheel import EV3EducationSetTire
from classes import Claw
button = Button()
claw = Claw()
wheels = MoveDifferential(OUTPUT_B, OUTPUT_A, EV3EducationSetTire, 110)
touch = TouchSensor(INPUT_3)
def on(speed=50):
wheels.on(SpeedPercent(speed), SpeedPercent(speed))
def wait_until_distance(distance=25):
while claw.eyes.distance_centimeters > distance:
pass
def forward_distance(distance=430, threshold=15):
wheels.on_for_distance(SpeedPercent(30), distance - threshold)
wheels.on_for_distance(SpeedPercent(15), threshold)
def return_to_base():
wheels.on_arc_left(SpeedPercent(-40), 180, 250)
wheels.on_arc_right(SpeedPercent(-40), 180, 470)
#!/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
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 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),
#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
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
def move_for_distance(self, speed=50, distance_mm=100):
MoveDifferential(OUTPUT_A, OUTPUT_B, EV3EducationSetTire,
80).on_for_distance(speed, distance_mm, brake=False)
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))
#!/usr/bin/env python3
from ev3dev2.sensor import INPUT_2
from ev3dev2.sensor.lego import GyroSensor
from ev3dev2.motor import OUTPUT_A, OUTPUT_C, MoveDifferential, SpeedRPM
from ev3dev2.wheel import EV3Tire
from time import sleep
import sys
import math
import paho.mqtt.client as mqtt
STUD_MM = 8
k0 = 11.5
alpha = 90
rd = 5
mdiff = MoveDifferential(OUTPUT_A, OUTPUT_C, EV3Tire, k0 * STUD_MM)
gy = GyroSensor(INPUT_2)
sleep(0.5)
gy.calibrate
gy.reset
ip_address = "192.168.1.149" # IP del broker con mosquitto (raspberry o EV3)
def arco():
mdiff.on_arc_right(rd, 750,
4550) #120 cm 550 y 3350 - 64 300 y 2000 - 169+7
def on_connect(client, userdata, flags, rc):