def walk_timed():
"""Makes the puppy walk a certain time in ms.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change walk_time to adjust the time the puppy walks in ms."""
#walk_time equals desired walk time in ms
walk_time = 6000
elapsed_time = StopWatch()
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
wait(500)
#puppy walks for specified time in ms
while elapsed_time.time() < walk_time:
left_leg_motor.run_target(-100, 25, wait=False)
right_leg_motor.run_target(-100, 25)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(100, 65, wait=False)
right_leg_motor.run_target(100, 65)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
wait(100)
elapsed_time.reset()
def follow_line():
# Return sensor values
black_area, white_area = calibration()
brick.display.text("Ready! Set! Go!")
wait(3000)
# Keeps track of time
timer = StopWatch()
# Loop to run for 20 seconds
while (timer.time() / 1000) < 20:
# The edge value is the mean of the black and white area readings
desired_value = (black_area + white_area) / 2
# Read sensor value
sensor_value = color_sensor.reflection()
# Magic constant
gain = 0.6
# Determine power for the motors
power = gain * (sensor_value - desired_value)
# One motor will speed up, the other will slow down
left_motor.dc(50 + power)
right_motor.dc(50 - power)
def wall_follow():
# Keeps track of time
timer = StopWatch()
# Loop to run the program for 20 seconds
while (timer.time() / 1000) < 20:
sensor_value = ultra_sensor.distance() / 10
# Within reasonable distance from the wall
if sensor_value < 22 and sensor_value > 18:
robot.drive_time(80, 0, 500)
# Too close to wall
elif sensor_value < 19:
# centimeters
diff = 20 - sensor_value
robot.drive_time(0, -90, 1000)
robot.drive_time(10 * diff, 0, 1000)
robot.drive_time(0, 90, 1000)
# Too far to wall
else:
# centimeters
diff = sensor_value - 20
robot.drive_time(0, 90, 1000)
robot.drive_time(10 * diff, 0, 1000)
robot.drive_time(0, -90, 1000)
wait(200)
def __init__(self):
# Initialize the EV3 brick.
self.ev3 = EV3Brick()
# Initialize the motors connected to the back legs.
self.left_leg_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE)
self.right_leg_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
# Initialize the motor connected to the head.
# Worm gear moves 1 tooth per rotation. It is interfaced to a 24-tooth
# gear. The 24-tooth gear is connected to parallel 12-tooth gears via
# an axle. The 12-tooth gears interface with 36-tooth gears.
self.head_motor = Motor(Port.C,
Direction.COUNTERCLOCKWISE,
gears=[[1, 24], [12, 36]])
# Initialize the Color Sensor.
self.color_sensor = ColorSensor(Port.S4)
# Initialize the touch sensor.
self.touch_sensor = TouchSensor(Port.S1)
# This attribute is used by properties.
self._eyes = None
# These attributes are used by the playful behavior.
self.playful_timer = StopWatch()
self.playful_bark_interval = None
def straight_line(angle, speed, duration):
timer = StopWatch()
while timer.time() <= duration:
screen.clear()
fix_amount = p_controller(angle, gyro)
drive_base.drive(speed, -fix_amount)
def calibrate(self):
rightHigh = 40
rightLow = 70
leftHigh = 40
leftLow = 70
timer = StopWatch()
self.moveSteering(0, 125)
while timer.time() < 5000:
if self.rightSensor.light() > rightHigh:
rightHigh = self.rightSensor.light()
if self.rightSensor.light() < rightLow:
rightLow = self.rightSensor.light()
if self.leftSensor.light() > leftHigh:
leftHigh = self.leftSensor.light()
if self.leftSensor.light() < leftLow:
leftLow = self.leftSensor.light()
self.stop()
# write results to file
with open('sensorpoints.py', 'w') as myFile:
myFile.write('leftLow = ')
myFile.write(str(leftLow))
myFile.write('\nrightLow = ')
myFile.write(str(rightLow))
myFile.write('\nleftHigh = ')
myFile.write(str(leftHigh))
myFile.write('\nrightHigh = ')
myFile.write(str(rightHigh))
def run(self, angle: int):
"""
Start turing in place for a specified angle.
----------
angle : int – angle to turn (clockwise - positive).
"""
# Setup starting values
stop_watch = StopWatch()
self.pid.setpoint = angle
self.gyro_sensor.reset_angle(0)
last_angle = angle + 100
last_rotation = 0
while not abs(self.pid.setpoint -
last_angle) < TurnToAngleBehavior.ACCURACY_ANGLE:
angle = self.gyro_sensor.angle()
rotation = self.pid(angle)
if last_rotation != rotation:
self.bot.drive(0, rotation)
last_angle = angle
last_rotation = rotation
print(stop_watch.time(), ' -- angle:', angle, ', rotation: ',
rotation, ', error: ', (angle - self.pid.setpoint))
def __init__(self): # micro controller ev3 = EV3Brick() ev3.speaker.beep() # sensors # low value = registers black tape # high value = aluminum self.sensorYellow = ColorSensor(Port.S1) self.sensorRed = ColorSensor(Port.S3) self.sensorBlue = ColorSensor(Port.S2) # motor left_motor = Motor(Port.A, gears=[40, 24]) right_motor = Motor(Port.B, gears=[40, 24]) axle_track = 205 wheel_diameter = 35 self.motors = DriveBase(left_motor, right_motor, wheel_diameter, axle_track) # constants # intersection detection of side sensors self.thresholdBlueSensor = 30 # value for making turns self.thresholdSideSensors = 15 # timer self.watch = StopWatch() self.timer = 0
def main():
coords = [0, 0]
left.reset_angle(0)
right.reset_angle(0)
Kp, Ki, Kd, i, last_error, target = 20, 0, 0, 0, 0, 5
conveyor_belt = Motor(Port.A)
directions_made = []
stopwatch = StopWatch()
stopwatch.resume()
driving_forward = False
while True:
if stopwatch.time() >= 100000: # OUR LAST HOPE FOR POINTS!!!!!
robot.stop()
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
break
if lightSensor.refelctivity() <= 500:
backward(1000)
if cSensor.color() == Color.BLUE:
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
conveyor_belt.run_time(-100, 1000, Stop.BRAKE, False)
foundvictim()
if LeftSensor.distance() >= 40:
wait(500)
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
leftturn()
directions_made.append("left turn")
robot.drive_time(-1000, 0, 1000)
directions_made.append(("forward", 1000))
elif ForwardSensor.distance() <= 300:
robot.stop(Stop.BRAKE)
if driving_forward:
directions_made.append(
("forward", stopwatch.time() - start_time))
driving_forward = False
rightturn()
directions_made.append("right turn")
else:
robot.drive(-1000, 0)
driving_forward, start_time = True, stopwatch.time()
for direction in directions_made:
if direction == "left turn":
robot.stop()
rightturn()
elif direction == "right turn":
robot.stop()
leftturn()
else:
robot.drive_time(-1000, 0, direction[1])
def drive_time(self, dist=100, time=1, ang=0):
stopwatch = StopWatch()
stopwatch.reset()
while True:
self.drive.drive(dist, ang)
if stopwatch.time() > time:
self.drive.drive.stop()
break
def follow_line_right(speed, duration):
timer = StopWatch()
while timer.time() <= duration:
deviation = right_color_sensor.reflection() - setpoint
turn_rate = K_P * deviation
screen.print(right_color_sensor.reflection())
drive_base.drive(speed, turn_rate)
wait(10)
def andarTempo(self, velocEsquerda, velocDireita, tempo):
cronometro = StopWatch() # Definimos um cronometro
cronometro.reset() # Resetamos o tempo marcado no cronometro
while cronometro.time() < tempo:
self.motorDireito.run(velocEsquerda)
self.motorEsquerdo.run(velocDireita)
self.motorDireito.stop()
self.motorEsquerdo.stop()
def get_double_press(
self
): # used to indicate a user has finished their time with CHORE bot
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.touch_left.pressed() and self.touch_right.pressed():
return True
if stopwatch.time() > 3000:
self.ev3.light.off()
return False
class Pid:
def __init__(self, kp, ki, kd):
"""Class that can be used to do PID calculations.
:param kp: Proportional multiplier for the error
:type kp: Number
:param ki: Multiplier for the intergral of the error
:type ki: Number
:param kd: Multiplier for the derivative of the error
:type kd: Number
"""
self.clock = StopWatch()
self.kp = kp
self.ki = ki
self.kd = kd
self.last_error = 0
self.total_error = 0
self.last_time = 0
def reset(self):
"""Reset parameters before start of PID loop
"""
self.clock.reset()
self.last_time = self.clock.time()
self.last_error = 0
self.total_error = 0
def compute_steering(self, error):
"""Computes and returns the corrections.
:param error: Difference between target and actual angles
:type error: Number
:return: Returns correction value
:rtype: Number
"""
current_time = self.clock.time()
elapsed_time = current_time - self.last_time
self.last_time = current_time
error_change = 0
if elapsed_time > 0:
error_change = (error - self.last_error) / elapsed_time
self.total_error = self.last_error * elapsed_time + self.total_error
self.last_error = error
steering = error * self.kp
steering += error_change * self.kd
steering += self.total_error * self.ki / 100
return steering
def get_feedback(self):
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.touch_left.pressed():
self.ev3.light.off()
return -1
elif self.touch_right.pressed():
self.ev3.light.off()
return 1
if stopwatch.time() > 10000:
self.ev3.light.off()
return 0
def __init__(self, robot, ev3, left_motor, right_motor, medium_motor,
color_sensor_1, color_sensor_2):
#self, DriveBase, Hub
self.driveBase = robot
self.hub = ev3
self.left_motor = left_motor
self.right_motor = right_motor
self.medium_motor = medium_motor
self.left_motor.reset_angle(0)
self.left_motor.reset_angle(0)
self.stopWatch = StopWatch()
self.color_sensor_1 = color_sensor_1
self.color_sensor_2 = color_sensor_2
def __init__(self, kp, ki, kd):
"""Class that can be used to do PID calculations.
:param kp: Proportional multiplier for the error
:type kp: Number
:param ki: Multiplier for the intergral of the error
:type ki: Number
:param kd: Multiplier for the derivative of the error
:type kd: Number
"""
self.clock = StopWatch()
self.kp = kp
self.ki = ki
self.kd = kd
self.last_error = 0
self.total_error = 0
self.last_time = 0
def __init__(self, address):
try:
self.sock = get_bluetooth_rfcomm_socket(address, 1)
except OSError as e:
print("Turn on Bluetooth on the EV3 and on SPIKE.")
raise e
self._values = None
start_new_thread(self.reader, ())
watch = StopWatch()
while watch.time() < 2000:
if self.values() is not None:
return
wait(100)
raise IOError("No data received")
def __init__(self,dimensions,motors,GPS_number,Gyro,Ultra,Yanse,PID_parameters,time_step):
# define system
self.ev3 = ev3 = EV3Brick()
# define dimensions
self.a = dimensions['a']
self.b = dimensions['b']
self.r = dimensions['r']
# define motors
self.left_up_motor = motors['left_up_motor']
self.right_up_motor = motors['right_up_motor']
self.left_down_motor = motors['left_down_motor']
self.right_down_motor = motors['right_down_motor']
# define GPS
self.GPS_number = GPS_number
# define Sensors
self.Gyro = Gyro
self.Gyro.reset_angle(0)
self.Ultra = Ultra
self.Yanse = Yanse
# define PID parameters
self.Kp_x = PID_parameters['x']['Kp']
self.Ki_x = PID_parameters['x']['Ki']
self.Kd_x = PID_parameters['x']['Kd']
self.Kp_y = PID_parameters['y']['Kp']
self.Ki_y = PID_parameters['y']['Ki']
self.Kd_y = PID_parameters['y']['Kd']
self.Kp_theta = PID_parameters['theta']['Kp']
self.Ki_theta = PID_parameters['theta']['Ki']
self.Kd_theta = PID_parameters['theta']['Kd']
# define watch
self.watch = StopWatch()
# define time steo
self.time_step = time_step
# initialize position
self.position_x = 0
self.position_y = 0
def turn(self, angle, speed, time=5):
# Startup
steering = 100
kTurn = 0.01
offset = 20
timer = StopWatch()
# Loop
while (abs(self.gyroSensor.angle() - angle) > 0) & (timer.time() <
time * 1000):
error = self.gyroSensor.angle() - angle
#if error > 0 :
# steering = 100
#else:
# steering = -100
self.moveSteering(steering,
speed * error * kTurn + copysign(offset, error))
# Exit
self.stop(Stop.HOLD)
print("turning to: ", angle, " gyro: ", self.gyroSensor.angle())
def __init__(self):
"""Class that represents the robot
"""
try:
self.state = "Port 1: Right Color"
self.right_color = ColorSensor(Port.S1)
self.state = "Port 2: Center Color"
self.center_color = ColorSensor(Port.S2)
self.state = "Port 3: Left Color"
self.left_color = ColorSensor(Port.S3)
self.state = "Port 4: Gyro"
self.gyro = GyroSensor(Port.S4, Direction.COUNTERCLOCKWISE)
self.state = "Port A: Left Motor"
self.left_motor = Motor(Port.A)
self.state = "Port B: Right Motor"
self.right_motor = Motor(Port.B)
self.state = "Port C: Linear Gear"
self.linear_attachment_motor = Motor(Port.C)
self.state = "Port D: Block Dropper"
self.dropper_attachment_motor = Motor(Port.D)
self.wheel_diameter = 55
self.axle_track = 123
self.drive_base = DriveBase(self.left_motor, self.right_motor,
self.wheel_diameter, self.axle_track)
self.state = "OK"
self.clock = StopWatch()
self.dance_clock = 0
self.sign = 1
except:
brick.screen.clear()
big_font = Font(size=18)
brick.screen.set_font(big_font)
brick.screen.draw_text(0, 20, "Error!")
brick.screen.draw_text(0, 40, self.state)
def run(self, until):
stop_watch = StopWatch()
last_angle = 0
while not until():
reflection = self.color_sensor.reflection()
angle = self.pid(reflection)
if last_angle != angle:
self.bot.drive(50, angle)
last_angle = angle
def act_playful():
"""Makes the puppy act playful."""
playful_timer = StopWatch()
playful_bark_interval = None
ev3.screen.show_image(ImageFile.NEUTRAL)
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
playful_bark_interval = 0
if playful_timer.time() > playful_bark_interval:
ev3.speaker.play_file(SoundFile.DOG_BARK_2)
playful_timer.reset()
playful_bark_interval = randint(4, 8) * 1000
def proportional_derivative(calibrate_val):
# initialize local variables
watch = StopWatch()
prev_time = 0 # time when the previous value was taken
curr_time = 0 # time when the current value was taken
prev_left = 0 # previous left sensor value
prev_right = 0 # previous right sensor value
curr_left = 0 # current left sensor value
curr_right = 0 # current right sensor value
run = True
speed = 0 # values determined from testing to be the best four our robot
desired_r = 200 # desired right sensor value
desired_l = 200 # desired left sensor value
k_p = 2 # proportional constant
k_d = 2 # derivative constant
while run and (count < 1000):
curr_time = watch.time()
if curr_time == 0:
prev_time = curr_time
curr_left = left_sensor.read(
) + calibrate_val # left sensor was determined through testing to need a static offeset to match the right sensor's value
curr_right = right_sensor.read()
if prev_left == 0:
prev_left = curr_left
if prev_right == 0:
prev_right = curr_right
left_speed = prop_deriv_control(L0, L1, time0, time1, desired_l, k_p,
k_d, speed)
right_speed = prop_deriv_control(R0, R1, time0, time1, desired_r, k_p,
k_d, speed)
prev_time = curr_time
prev_left = curr_left
prev_right = curr_right
left_motor.run(left_speed)
right_motor.run(right_speed)
if Button.CENTER in brick.buttons():
# ends run loop
run = False
# ends main loop
return True
def follow_line_distance_left(speed, distance):
timer = StopWatch()
drive_base.reset()
while abs(drive_base.distance()) <= distance:
deviation = left_color_sensor.reflection() - setpoint
turn_rate = K_P * deviation
screen.print(left_color_sensor.reflection())
drive_base.drive(speed, turn_rate)
wait(10)
def get_color(self, time=1000):
stopwatch = StopWatch()
stopwatch.reset()
self.ev3.light.on(Color.ORANGE)
while True:
if self.color_sensor.color() == Color.BLUE:
self.ev3.light.off()
return Color.BLUE
if self.color_sensor.color() == Color.GREEN:
self.ev3.light.off()
return Color.GREEN
if self.color_sensor.color() == Color.YELLOW:
self.ev3.light.off()
return Color.YELLOW
if self.color_sensor.color() == Color.RED:
self.ev3.light.off()
return Color.RED
if stopwatch.time() > time:
self.ev3.light.off()
return None
def scan_rock(self, time):
# Stops the robot and moves the scan arm.
self.stop()
self.left_front_wheel.run(100)
# During the given duration, scan for rocks.
watch = StopWatch()
while watch.time() < time:
# If a rock color is detected, display it and make a sound.
if self.sherloc.color() in self.ROCK_COLORS:
self.hub.display.image(Icon.CIRCLE)
self.hub.speaker.beep()
else:
self.hub.display.off()
wait(10)
# Turn the arm motor and restore the heartbeat animation again.
self.hub.display.animate(self.HEART_BEAT, 30)
self.left_front_wheel.stop()
class RPMSensor():
lightSensor = ColorSensor(Port.S1)
calibrateButton = Button.CENTER
sample = [0, 100]
mid = 50
mode = 'measuring'
rpm = 0
stopWatch = StopWatch()
def __init__(self):
m = Thread(target=self.measure)
m.start()
c = ButtonListener(self.calibrateButton, self.calibrate)
d = Thread(target=self.display)
d.start()
def display(self):
while True:
brick.display.clear()
if self.mode == 'measuring':
brick.display.text('{0:.2f} RPM'.format(self.rpm), (60, 50))
elif self.mode == 'calibrating':
brick.display.text('CALIBRATING...', (60, 50))
wait(500)
def calibrate(self):
self.mode = 'calibrating'
self.sample = []
while len(self.sample) < 100:
v = self.lightSensor.reflection()
self.sample.append(v)
maximum = max(self.sample)
self.mid = maximum - (maximum * 0.1)
print(self.mid)
self.mode = 'measuring'
def measure(self):
while True:
if self.mode != 'measuring':
continue
v = self.lightSensor.reflection()
if v > self.mid:
interval = self.stopWatch.time() # how long it took for 1 rotation
interval = 1 if interval == 0 else interval
rotationPerSec = 1 / interval
self.rpm = rotationPerSec * 60
self.stopWatch.pause()
else:
self.stopWatch.reset()
self.stopWatch.resume()
def wait_until_bumped(self, wait_timer=500):
"""Wait until the TouchSensor is bumped
:param wait_timer: Time to wait (milliseconds) to consider a press and release a bump,
defaults to 500
:type wait_timer: int, float, optional
"""
if not isinstance(wait_timer, (int, float)):
return
my_watch = StopWatch()
while True:
self.wait_until_pressed()
my_watch.reset()
my_watch.resume()
self.wait_until_released()
my_watch.pause()
if my_watch.time() > wait_timer:
continue
return
def tester():
powerL = 50
powerR = 48
# Time it takes for your robot to move forward in a straight line for
# approximately 15 cm when your left and right motors are set to
# powerL and powerR respectively.
time15cm = 0.887
# 15% faster
powerR15 = powerR * 1.15
# resets the rotation angle of both motors
left_motor.reset_angle(0)
right_motor.reset_angle(0)
timer = StopWatch()
# starts the left and right motors moving using the dc command at powers
# powerL and powerR15
while (timer.time() / 1000) < time15cm:
left_motor.dc(powerL)
right_motor.dc(powerR15)
# waits time15cm seconds
wait(time15cm)
# stops both motors moving
left_motor.dc(0)
right_motor.dc(0)
wait(1000)
# gets the rotation angle of both motors
left_rot_angle_after = left_motor.angle()
right_rot_angle_after = right_motor.angle()
computePoseChange(left_rot_angle_after, right_rot_angle_after)
