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 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)
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)
#!/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)
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 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')
print(len(sensorValues), maxVal, minVal, midVal, meanVal)
#print(sensorValues)
sensorValues = []
medium_motorA.on_for_degrees(speed = 10, degrees = 110)
sample1()
sleep(2)
#moving the arm back down
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)
wheels.on_arc_right(SpeedPercent(-40), 180, 470)
wheels.on_arc_left(SpeedPercent(-40), 180, 240)
on(speed=-65)
while not touch.is_pressed:
pass
wheels.off(brake=False)
# go forward until nearest object is within 25 cm
on()
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:
#colSCrossDetect = ColorSensor(INPUT_2)
#stopButton = TouchSensor(INPUT_3)
colSFollower = ColorSensor(INPUT_4)
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(35, 90, 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')
print(len(sensorValues), maxVal, minVal, midVal, meanVal)
print("The data::")
for i in range(len(sensorValues)):
# Etsitään valkoinen viiva
while not robotSeesWhite():
tank_drive.run_forever(speed_sp=-150)
sound.speak('White')
# Kuljetaan viivaa pitkin
while True:
# Ajetaan 200mm suoraan, jos ollaan keltaisen päällä
# Odotetaan 10s ja käännytään 170 astetta vasempaan
if robotSeesYellow():
mdiff.on_for_distance(SpeedRPM(-60), 200)
time.sleep(4)
mdiff.turn_left(SpeedRPM(40), 340)
mdiff.on_for_distance(SpeedRPM(-60), 160)
# Ajetaan suoraan, jos ollaan viivan päällä
elif robotSeesWhite():
tank_drive.run_forever(speed_sp=-200)
else:
whiteColorFound = False
# Etsitään käännöstä oikealle max 90 astetta
for i in range(20):
turnRight()
if robotSeesWhite():
whiteColorFound = True
break
if whiteColorFound:
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
mdiff = MoveDifferential(OUTPUT_D, OUTPUT_C, EV3EducationSetTire, 105)
gy = GyroSensor(INPUT_1)
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))
#!/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=70) 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), 135, 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) medium_motorA.on_for_degrees(speed=10, degrees=-110) sleep(5) mdiff.on_arc_left(SpeedRPM(50), 1200, 400)