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 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 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)
#!/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
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)
#!/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)
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()
def sample2():
medium_motorA.on_for_degrees(speed = 10, degrees = -110)
sleep(5)
#go to sample 2 position
mdiff.on_arc_left(SpeedRPM(50), 1200, 400)
sleep(5)
#moving the arm up with second sample and next commands are going to cache site
medium_motorA.on_for_degrees(speed = 10, degrees = 110)
sample2()
sleep(2)
mdiff.turn_left(speed = 50, degrees = 38)
sleep(2)
mdiff.on_for_distance(speed = 50, distance_mm = 800)
sleep(2)
mdiff.turn_right(speed = 50, degrees = 65)
sleep(2)
mdiff.on_for_distance(speed =50, distance_mm = 750)
sleep(2)
mdiff.turn_right(speed = 50, degrees = 55)
sleep(2)
mdiff.on_for_distance(speed = 50, distance_mm = 260)
#moving the arm down near the cache site
medium_motorA.on_for_degrees(speed = 10, degrees = -110)
sleep(2)
mdiff.on_for_distance(speed = 50, distance_mm = -50)
sleep(2)
#moving the arm up after disengaging with both samples
medium_motorA.on_for_degrees(speed = 10, degrees = 110)
sleep(2)
mdiff.turn_left(speed = 50, degrees = 50)
wait_until_distance(35)
on(speed=15)
wait_until_distance(20)
# hard turn to get off wall
wheels.turn_left(SpeedPercent(15), 45)
# arc to the left to orient towards push object
wheels.on_arc_left(SpeedPercent(30), 300, 230, brake=False)
# move forward 430 mm
forward_distance()
# reverse at an arc
wheels.on_arc_right(SpeedPercent(-50), 200, 225, brake=False)
# reverse for 145 mm
wheels.on_for_distance(SpeedPercent(-30), 155)
# reverse at an arc, orient touch sensor towards wall
wheels.on_arc_left(SpeedPercent(-50), 175, 115, brake=False)
# move back at 50% speed until touch sensor is pressed
on(speed=-50)
while not touch.is_pressed:
pass
# rotate right 90 degrees
wheels.turn_right(SpeedPercent(35), 90)
# move forward 110 mm
forward_distance(110)
# return to the base, avoiding obstacles
return_to_base()
gy.mode = 'GYRO-ANG'
while True:
baseAngle = gy.value()
print("Inicio loop")
print("Angulo Base: ", baseAngle)
mdiff.turn_left(SpeedRPM(40), 90)
time.sleep(0.5)
angle = abs(gy.value() - baseAngle)
diffAng = angle - 90
print("Angulo Calculado: ", angle)
print("Diferenca: ", diffAng)
if(diffAng < 0):
mdiff.turn_left(SpeedRPM(40), abs(diffAng))
else:
mdiff.turn_right(SpeedRPM(40), abs(diffAng))
time.sleep(2)
# Anda Reto
# Gira
#!/usr/bin/env python3
from ev3dev2.motor import OUTPUT_A, OUTPUT_D, SpeedPercent, MoveDifferential
from ev3dev2.unit import STUD_MM
from ev3dev2.wheel import EV3EducationSetTire
from ev3dev2.sound import Sound
def reverseRotations(rotations):
tank_drive.on_for_rotations(SpeedPercent(-35), SpeedPercent(-35), rotations)
def rotateDegrees(degrees):
tank_drive.turn_left(SpeedPercent(40), degrees)
def drive():
tank_drive.on(SpeedPercent(30), SpeedPercent(30))
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire, 15 * STUD_MM)
sound = Sound()
tank_drive.turn_right(30, 360)
drive()
medium_motorA.on_for_degrees(speed=10, degrees=110) sleep(2) #moving the arm back down medium_motorA.on_for_degrees(speed=10, degrees=-110) sleep(5) #arc with radius 1200 and distance 400 mdiff.on_arc_left(SpeedRPM(50), 1200, 400) sleep(5) #moving the arm up with second sample and next commands are going to cache site medium_motorA.on_for_degrees(speed=10, degrees=110) sleep(2) mdiff.turn_left(speed=50, degrees=38) sleep(2) mdiff.on_for_distance(speed=50, distance_mm=800) sleep(2) mdiff.turn_right(speed=50, degrees=75) sleep(2) mdiff.on_for_distance(speed=50, distance_mm=700) sleep(2) mdiff.turn_right(speed=65, degrees=45) sleep(2) #moving the arm down near the cache site medium_motorA.on_for_degrees(speed=10, degrees=-110) sleep(2) mdiff.on_for_distance(speed=50, distance_mm=-50) sleep(2) #moving the arm up after disengaging with both samples medium_motorA.on_for_degrees(speed=10, degrees=110) sleep(2) mdiff.turn_left(speed=50, degrees=65) mdiff.on_for_distance(speed=50, distance_mm=250)
String,
rcallback,
queue_size=5)
print("starting loop")
#while not rospy.is_shutdown():
# rate.sleep()
# pass
while not rospy.is_shutdown():
try:
if us.distance_centimeters > 10 and not turning:
tank_drive.on_for_distance(SpeedRPM(40), -20)
else:
send_request()
#print("Obstacle detected, requesting identification")
#while not received:
# pass
#received = False
if turning:
tank_drive.turn_right(SpeedRPM(60), 90)
turning = False
except rospy.ROSInterruptException:
pass
rate.sleep()
pass
#!/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=45) mdiff.on_for_distance(speed=50, distance_mm=500) mdiff.turn_left(speed=50, degrees=50) mdiff.on_for_distance(speed=50, distance_mm=600) mdiff.turn_right(speed=50, degrees=90) mdiff.on_for_distance(speed=50, distance_mm=850) mdiff.turn_right(speed=50, degrees=80) midff.on_for_distance(speed=50, distance_mm=600) sleep(2) medium_motorA.on_for_degrees(speed=8, degrees=-110) sleep(2) medium_motorA.on_for_degrees(speed=8, degrees=110)