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)