def __init__(self): try: self.btn = ev3.Button() self.shut_down = False except: print('on computer')
#!/usr/bin/env python3 import sys import signal from time import sleep import ev3dev.ev3 as ev3 # misc stuff brick_but = ev3.Button() # sensors sensor_col = ev3.ColorSensor() sensor_ultrasonic = ev3.UltrasonicSensor() sensor_touch = ev3.TouchSensor() # motors motor_left = ev3.LargeMotor('outD') motor_right = ev3.LargeMotor('outA') devices = [sensor_col, sensor_ultrasonic, sensor_touch, motor_left, motor_right] # global vars that can be adjusted after performing calibration min_ref = 10 max_ref = 87 mid_ref = (10 + 87) / 2 kp = 0.9 power = -80 def stop_motors():
def __init__(self): """ Creates the one and only brick button object. """ self._buttons = ev3.Button()
power_right = power - course else: if course < -100: power_left = 0 power_right = power else: power_right = power power_left = power + course return (int(power_left), int(power_right)) Tp = 30 motR = ev3.LargeMotor('outA') motL = ev3.LargeMotor('outD') colorSensor = ev3.ColorSensor() btn = ev3.Button() colorSensor.mode = 'COL-REFLECT' colorSensor.connected runForward() # 0.25 0.15 works okeish Kp = float(0.25) # Proportional gain. Start value 1 Kd = 0.15 # Derivative gain. Start value 0 Ki = float( 0.02 ) # Integral gain. Start value 0 # REMEMBER we are using Kd*100 so this i Kp = float(0.25) # Proportional gain. Start value 1 Kd = 0.15 # Derivative gain. Start value 0 Ki = float(0.02) # Integral gain. Start value 0 really offset = 55 # Initialize the variables integral = 0.0 # the place where we will store our integral
#!/usr/bin/env python3 import ev3dev.ev3 as ev3 from time import sleep import paho.mqtt.client as mqtt from threading import Thread from multiprocessing import Process # Go in a straight line # Left motor will be MASTER --> mL btn = ev3.Button() # will use any button to stop script client = mqtt.Client() client.connect("vitez.si", 1883, 60) # Init motors mR = ev3.LargeMotor('outA') mL = ev3.LargeMotor('outD') maxR = mR.max_speed maxL = mL.max_speed targetSpeed = 450 # Init light sensor cl = ev3.ColorSensor() cl.mode = 'COL-REFLECT' blackValue = 10 #Init gyro gy = ev3.GyroSensor() # Gyro can be reset with changing between modes :) (WTF!) gy.mode = 'GYRO-ANG' gy.mode = 'GYRO-RATE' gy.mode = 'GYRO-ANG' # Init PID parameters --> tuned with Ziegler–Nichols method kP = 3.6
def __init__(self): self.btn = ev3.Button() self.shut_down = False
def wait_button(): buttons = ev3.Button() while 'enter' not in buttons.buttons_pressed: time.sleep(0.1)
def main(): print("--------------------------------------------") print("IR Remote") print(" - Use IR remote channel 1 to drive around") print(" - Use IR remote channel 2 to for the arm") print(" - Press the Back button on EV3 to exit") print("--------------------------------------------") ev3.Sound.speak("I R Remote") ev3.Leds.all_off() # Turn the leds off robot = robo.Snatch3r() dc = DataContainer() # DONE: 4. Add the necessary IR handler callbacks as per the instructions # above. # Remote control channel 1 is for driving the crawler tracks around (none of these functions exist yet below). # Remote control channel 2 is for moving the arm up and down (all of these functions already exist below). # For our standard shutdown button. btn = ev3.Button() btn.on_backspace = lambda state: handle_shutdown(state, dc) left_motor = ev3.LargeMotor(ev3.OUTPUT_B) right_motor = ev3.LargeMotor(ev3.OUTPUT_C) assert left_motor.connected assert right_motor.connected rc1 = ev3.RemoteControl(channel=1) rc1.on_red_up = lambda button_state: robot.red_up(button_state) rc1.on_red_down = lambda button_state: robot.red_down(button_state) rc1.on_blue_up = lambda button_state: robot.blue_up(button_state) rc1.on_blue_down = lambda button_state: robot.blue_down(button_state) rc2 = ev3.RemoteControl(channel=2) rc2.on_red_up = lambda button_state: handle_arm_up_button(button_state, robot) rc2.on_red_down = lambda button_state: handle_arm_down_button(button_state, robot) rc2.on_blue_up = lambda button_state: handle_calibrate_button(button_state, robot) btn.on_backspace = lambda button_state: handle_shutdown(button_state, dc, robot) robot.arm_calibration() # Start with an arm calibration in this program. while dc.running: # DONE: 5. Process the RemoteControl objects. rc1.process() rc2.process() btn.process() time.sleep(0.01) # DONE: 2. Have everyone talk about this problem together then pick one # member to modify libs/robot_controller.py # as necessary to implement the method below as per the instructions in the opening doc string. Once the code has # been tested and shown to work, then have that person commit their work. All other team members need to do a # VCS --> Update project... # Once the library is implemented any team member should be able to run his code as stated in todo3. robot.shutdown()
def main(): print("--------------------------------------------") print("IR Remote") print(" - Use IR remote channel 1 to drive around") print(" - Use IR remote channel 2 to for the arm") print(" - Press the Back button on EV3 to exit") print("--------------------------------------------") ev3.Sound.speak("I R Remote") ev3.Leds.all_off() # Turn the leds off robot = robo.Snatch3r() dc = DataContainer() # DONE: 4. Add the necessary IR handler callbacks as per the instructions above. # Remote control channel 1 is for driving the crawler tracks around (none of these functions exist yet below). # Remote control channel 2 is for moving the arm up and down (all of these functions already exist below). rc1 = ev3.RemoteControl(channel=1) rc2 = ev3.RemoteControl(channel=2) rc1.on_red_up = lambda state: handle_drive_left_forward(state, robot) rc1.on_red_down = lambda state: handle_drive_left_backward(state, robot) rc1.on_blue_up = lambda state: handle_drive_right_forward(state, robot) rc1.on_blue_down = lambda state: handle_drive_right_backward(state, robot) rc2.on_red_up = lambda state: robot.arm_up() rc2.on_red_down = lambda state: robot.arm_down() rc2.on_blue_up = lambda state: robot.arm_calibration() """ -- Pressing red up calls robot.arm_up(). -- Pressing red down calls robot.arm_down(). -- Pressing blue up calls robot.arm_calibration(). """ # For our standard shutdown button. btn = ev3.Button() btn.on_backspace = lambda state: handle_shutdown(state, dc) robot.arm_calibration() # Start with an arm calibration in this program. while dc.running: # DONE: 5. Process the RemoteControl objects. rc1.process() rc2.process() btn.process() time.sleep(0.01) # DONE: 2. Have everyone talk about this problem together then pick one member to modify libs/robot_controller.py # as necessary to implement the method below as per the instructions in the opening doc string. Once the code has # been tested and shown to work, then have that person commit their work. All other team members need to do a # VCS --> Update project... # Once the library is implemented any team member should be able to run his code as stated in todo3. robot.shutdown() # ---------------------------------------------------------------------- # Event handlers # Some event handlers have been written for you (ones for the arm). # Movement event handlers have not been provided. # ---------------------------------------------------------------------- # TODO: 6. Implement the IR handler callbacks handlers. # TODO: 7. When your program is complete, call over a TA or instructor to sign your checkoff sheet and do a code review. # # Observations you should make, IR buttons are a fun way to control the robot. """IR remote channel 1 to drive the crawler tracks around:
def main(): print("--------------------------------------------") print("IR Remote") print(" - Use IR remote channel 1 to drive around") print(" - Use IR remote channel 2 to for the arm") print(" - Press the Back button on EV3 to exit") print("--------------------------------------------") ev3.Sound.speak("I R Remote") arm = ev3.MediumMotor(ev3.OUTPUT_A) assert arm.connected touch_sensor = ev3.TouchSensor() assert touch_sensor.connected left_motor = ev3.LargeMotor(ev3.OUTPUT_B) assert left_motor.connected right_motor = ev3.LargeMotor(ev3.OUTPUT_C) assert right_motor.connected ev3.Leds.all_off() # Turn the leds off robot = robo.Snatch3r() dc = DataContainer() # Done: 4. Add the necessary IR handler callbacks as per the instructions above. # Remote control channel 1 is for driving the crawler tracks around (none of these functions exist yet below). # Remote control channel 2 is for moving the arm up and down (all of these functions already exist below). # For our standard shutdown button. btn = ev3.Button() btn.on_backspace = lambda state: handle_shutdown(state, dc) robot.arm_calibration() # Start with an arm calibration in this program. def up_stuff(button_state, motor): if button_state: ev3.Sound.speak("I R Remote") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN) motor.run_forever(speed_sp=600) else: ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) motor.stop(stop_action='brake') def down_stuff(button_state, motor): if button_state: ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.RED) motor.run_forever(speed_sp=-600) else: ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) motor.stop(stop_action='brake') def up_stuff2(button_state, motor): if button_state: ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.GREEN) motor.run_forever(speed_sp=600) else: ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) motor.stop(stop_action='brake') def down_stuff2(button_state, motor): if button_state: ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.RED) motor.run_forever(speed_sp=-600) else: ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) motor.stop(stop_action='brake') ir1 = ev3.RemoteControl(channel=1) ir1.on_red_up = lambda state: up_stuff(state, left_motor) ir1.on_red_down = lambda state: down_stuff(state, left_motor) ir1.on_blue_up = lambda state: up_stuff2(state, right_motor) ir1.on_blue_down = lambda state: down_stuff2(state, right_motor) ir2 = ev3.RemoteControl(channel=2) ir2.on_red_up = lambda state: handle_arm_up_button(state, robot) ir2.on_red_down = lambda state: handle_arm_down_button(state, robot) ir2.on_blue_up = lambda state: handle_calibrate_button(state, robot) btn.on_backspace = lambda state: handle_shutdown(state, dc) while dc.running: # TODO: 5. Process the RemoteControl objects. ir1.process() ir2.process() btn.process() time.sleep(0.01) # Done: 2. Have everyone talk about this problem together then pick one member to modify libs/robot_controller.py # as necessary to implement the method below as per the instructions in the opening doc string. Once the code has # been tested and shown to work, then have that person commit their work. All other team members need to do a # VCS --> Update project... # Once the library is implemented any team member should be able to run his code as stated in todo3. robot.shutdown()
def main(): print("--------------------------------------------") print("Running Snatch3r IR hardware test program") print(" - Use IR remote channel 1 to drive around") print(" - Use IR remote channel 2 to for the arm") print(" - Press backspace button on EV3 to exit") print("--------------------------------------------") ev3.Leds.all_off() # Turn the leds off # Connect two large motors on output ports B and C and medium motor on A left_motor = ev3.LargeMotor(ev3.OUTPUT_B) right_motor = ev3.LargeMotor(ev3.OUTPUT_C) arm_motor = ev3.MediumMotor(ev3.OUTPUT_A) # Check that the motors are actually connected assert left_motor.connected assert right_motor.connected assert arm_motor.connected # Only using 1 sensor in this program touch_sensor = ev3.TouchSensor() # address=in1 assert touch_sensor # Remote control channel 1 is for driving the crawler tracks around. rc1 = ev3.RemoteControl(channel=1) assert rc1.connected rc1.on_red_up = lambda state: handle_ir_move_button( state, left_motor, ev3.Leds.LEFT, 1) rc1.on_red_down = lambda state: handle_ir_move_button( state, left_motor, ev3.Leds.LEFT, -1) rc1.on_blue_up = lambda state: handle_ir_move_button( state, right_motor, ev3.Leds.RIGHT, 1) rc1.on_blue_down = lambda state: handle_ir_move_button( state, right_motor, ev3.Leds.RIGHT, -1) # Remote control channel 2 is for moving the arm up and down. rc2 = ev3.RemoteControl(channel=2) assert rc2.connected rc2.on_red_up = lambda state: handle_arm_up_button(state, arm_motor, touch_sensor) rc2.on_red_down = lambda state: handle_arm_down_button(state, arm_motor) rc2.on_blue_up = lambda state: handle_calibrate_button( state, arm_motor, touch_sensor) # Allows testing of the arm without the IR remote btn = ev3.Button() btn.on_up = lambda state: handle_arm_up_button(state, arm_motor, touch_sensor) btn.on_down = lambda state: handle_arm_down_button(state, arm_motor) btn.on_left = lambda state: handle_calibrate_button( state, arm_motor, touch_sensor) btn.on_right = lambda state: handle_calibrate_button( state, arm_motor, touch_sensor) btn.on_backspace = lambda state: handle_shutdown(state) ev3.Sound.speak("Ready") arm_calibration(arm_motor, touch_sensor) while True: rc1.process() rc2.process() btn.process() time.sleep(0.01)
def __init__(self): self.speed = 100 self.btn = ev3.Button() self.motor_left = ev3.LargeMotor('outA') self.motor_right = ev3.LargeMotor('outD')
def main(): print("--------------------------------------------") print("IR Remote") print(" - Use IR remote channel 1 to drive around") print(" - Use IR remote channel 2 to for the arm") print(" - Use IR remote channel 3 to draw shapes") print(" - Use IR remote channel 4 to dance!") print(" - Press the Back button on EV3 to exit") print("--------------------------------------------") ev3.Sound.speak("I R Remote") ev3.Leds.all_off() # Turn the leds off robot = robo.Snatch3r() # DONE: 4. Add the necessary IR handler callbacks as per the instructions above. # Remote control channel 1 is for driving the crawler tracks around (none of these functions exist yet below). # Remote control channel 2 is for moving the arm up and down (all of these functions already exist below). rc1 = ev3.RemoteControl(channel=1) rc2 = ev3.RemoteControl(channel=2) rc3 = ev3.RemoteControl(channel=3) rc4 = ev3.RemoteControl(channel=4) # For our standard shutdown button. btn = ev3.Button() btn.on_backspace = lambda state: handle_shutdown(state, dc) robot.arm_calibration() # Start with an arm calibration in this program. rc1.on_red_up = lambda button_state: handle_move_left_forward( button_state, robot) rc1.on_red_down = lambda button_state: handle_move_left_back( button_state, robot) rc1.on_blue_up = lambda button_state: handle_move_right_forward( button_state, robot) rc1.on_blue_down = lambda button_state: handle_move_right_back( button_state, robot) rc2.on_red_up = lambda button_state: handle_arm_up_button( button_state, robot) rc2.on_red_down = lambda button_state: handle_arm_down_button( button_state, robot) rc2.on_blue_up = lambda button_state: handle_calibrate_button( button_state, robot) rc2.on_blue_down = lambda button_state: handle_shutdown( button_state, robot) rc3.on_red_up = lambda button_state: draw_triangle(button_state, robot) rc3.on_red_down = lambda button_state: draw_square(button_state, robot) rc3.on_blue_up = lambda button_state: draw_pentagon(button_state, robot) rc3.on_blue_down = lambda button_state: draw_hexagon(button_state, robot) rc4.on_red_up = lambda button_state: dance_1(button_state, robot) rc4.on_red_down = lambda button_state: dance_2(button_state, robot) rc4.on_blue_up = lambda button_state: dance_3(button_state, robot) rc4.on_blue_down = lambda button_state: dance_4(button_state, robot) dc = DataContainer() while dc.running: # DONE: 5. Process the RemoteControl objects. btn.process() rc1.process() rc2.process() rc3.process() rc4.process() time.sleep(0.01) print("Goodbye!") ev3.Sound.speak("Goodbye").wait()
#!/usr/local/bin/python3 import ev3dev.ev3 as ev3 # Define Ports motorL = ev3.LargeMotor('outD') motorR = ev3.LargeMotor('outA') colorSensL = ev3.ColorSensor('in4') colorSensR = ev3.ColorSensor('in1') extButton = ev3.Button('in2') # whiteCal on touch #-- whiteCalL = colorSensL.reflected_light_intensity #-- whiteCalR = colorSensR.reflected_light_intensity # Print whiteCal print ('wCalL: ' + whiteCalL + ' wCalR: ' + whiteCalR) # Set sensor mode to color colorSensL.mode='COL-COLOR' colorSensL.mode='COL-COLOR' # Start while True: # ------- Check color for both sensors
def seek_beacons(self, turn_speed, forward_speed): print("seeking...") target = 3 b1_pos = [0, 0] b2_pos = [0, 0] bc1 = 0 bc2 = 0 btn = ev3.Button() beacon_1 = ev3.BeaconSeeker(channel=1) # beacon_2 = ev3.BeaconSeeker(channel=2) robot = robo.Snatch3r() robot.arm_calibration() while bc1 < 1: if beacon_1.distance != -128 and bc1 < 1: b1_pos = [beacon_1.heading, beacon_1.distance] print("found 1") print(beacon_1.heading_and_distance) print(btn.backspace) # print(beacon_1.distance) bc1 += 1 target = 1 ev3.Sound.beep().wait() # if beacon_2.distance != -128 and bc2 < 1: # b2_pos = [beacon_2.heading, beacon_2.distance] # print("found 2") # print(beacon_2.heading_and_distance) # # print(beacon_1.distance) # bc2 += 1 # target = 2 # ev3.Sound.beep().wait() # global mqtt_client # mqtt_client.send_message("update_b2", [b2_pos[0], b2_pos[1]]) beacon = ev3.BeaconSeeker(channel=1) while btn.backspace is False and forward_speed > 0 and turn_speed > 0: current_heading = beacon.heading # use the beacon_seeker heading current_distance = beacon.distance if current_distance == -128: # If the IR Remote is not found just sit idle for this program until it is moved. print("IR Remote not found. Distance is -128") else: if math.fabs(current_heading) < 2: # Close enough of a heading to move forward print("On the right heading. Distance: ", current_distance) # You add more! if current_distance == 0: print('You found it') robot.stop() break if current_distance > 0: robot.forward(forward_speed, forward_speed) if current_distance <= 1: robot.stop() robot.drive_inches(3, forward_speed) robot.stop() ev3.Sound.speak('beacon found') time.sleep(3) break if math.fabs(current_heading) > 2 and math.fabs( current_heading) < 10: print("Adjusting heading: ", current_heading) if current_heading > 0: robot.left_move(turn_speed, turn_speed) if math.fabs(current_heading) < 2: robot.stop() if current_heading < 0: robot.right_move(turn_speed, turn_speed) if math.fabs(current_heading) < 2: robot.stop() if math.fabs(current_heading) > 10: robot.right_move(turn_speed, turn_speed) time.sleep(0.2) # The touch_sensor # was pressed to abort the attempt if this code runs. robot.stop() mqtt_client.send_message("update_b1", [b1_pos[0], b1_pos[1]]) return False
def main(ts, c_addr, s_port, a_port, log_enabled): """ Main function called from __main__ :param ts: Sampling period in milliseconds :param c_addr: IP address of the controller :param s_port: Port number to send sensor signals :param a_port: Port number to receive actuation signals :param log_enabled: Set to 'True' to activate logging sensor measurements & states into logfile :return: None """ buttons = ev3.Button() leds = ev3.Leds() leds.set_color(leds.LEFT, leds.RED) leds.set_color(leds.RIGHT, leds.RED) logging.info( "Lay down the robot. Press the up button to start calibration. Press the down button to exit." ) # Initialization of the sensors gyroSensorValueRaw, \ motorEncoderLeft, motorEncoderRight = init_sensors() logging.debug("Initialized sensors") # Initialization of the actuators motorDutyCycleFile_left, motorDutyCycleFile_right = init_actuators() logging.debug("Initialized actuators") # Wait for up or down button while not buttons.up and not buttons.down: time.sleep(0.01) # If the up button was pressed wait for release and start calibration while buttons.up: time.sleep(0.01) # Calibration gyroOffset = calibrate_gyro(gyroSensorValueRaw) logging.info("Calibration done. Start the Controller and lift me!") ev3.Sound.beep().wait() leds.set_color(leds.LEFT, leds.AMBER) leds.set_color(leds.RIGHT, leds.AMBER) # Initialization of the sensor sockets udp_socket_sensor = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) udp_socket_sensor.setblocking(0) udp_socket_sensor.bind(('', s_port)) logging.debug("Initialized sensor UDP socket") # Initialization of the actuator socket udp_socket_actuator = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) udp_socket_actuator.setblocking(1) udp_socket_actuator.settimeout(0.001) udp_socket_actuator.bind(('', a_port)) logging.debug("Initialized actuator UDP socket") # Inits: next_trigger_time = 0 # he "starts" the loop sending the first message k = 0 # Aux variables first_packet = True buttons = ev3.Button() seq_no_applied = 0 motor_voltage_applied_left = 0 motor_voltage_applied_right = 0 prediction_count = 0 receptionStarted = False no_of_sim_steps = 10 voltagePredictions = [0] * 2 * no_of_sim_steps tau_RTT = 0 avg_diff = p.SAMPLING_TIME diff_variance = 0 old_sens_timestamp = time.perf_counter() tsr_k = time.perf_counter() tsstx_k = time.perf_counter() tasrx_k = 0 taw_k = 0 # Robot logging timenow = datetime.datetime.now() if log_enabled: filename = "datalog" + str(timenow.year) + "-" + str( timenow.month) + "-" + str(timenow.day) + "_" + str( timenow.hour) + "_" + str(timenow.minute) + "_" + str( timenow.second) + ".csv" f = open(filename, "w") f.write("gyro,enc_l,enc_r, rtt, prediction_count\n") outdated_warning_printed = False while True: next_trigger_time = time.perf_counter() + ts k += 1 # Sensor readings tsr_km1 = tsr_k tsr_k = time.perf_counter() gyro = SensorRead(gyroSensorValueRaw) new_sens_timestamp = time.perf_counter() enc_l = SensorRead(motorEncoderLeft) enc_r = SensorRead(motorEncoderRight) diff_sens_timestamp = new_sens_timestamp - old_sens_timestamp old_sens_timestamp = new_sens_timestamp avg_diff = ((k - 1) / k) * avg_diff + 1 / k * diff_sens_timestamp diff_variance = ((k - 1) / k) * diff_variance + (1 / k) * ( diff_sens_timestamp - avg_diff) * (diff_sens_timestamp - avg_diff) # Sensor transmission try: tsstx_km1 = tsstx_k tsstx_k = time.perf_counter( ) # transmission of a time reply for the controller #TODO in the send_sensor_signal? send_sensor_signals(udp_socket_sensor, c_addr, s_port, tsr_km1, tsstx_km1, tasrx_k, taw_k, k, gyro, enc_l, enc_r, gyroOffset, motor_voltage_applied_left, motor_voltage_applied_right) if receptionStarted: logging.debug("Sensing %d sent at %f with actuation %f", k, tsstx_k, tasrx_k) # logging.debug("Sensing %d sent at %f" % (k, t0_req_tx_current)) except KeyboardInterrupt: udp_socket_sensor.close() return except socket.error: return # Take timestamp after packet was sent timestamp = time.perf_counter() # Actuation reception - Use the rest of the time to receive host packets, trying at least once. while True: rx_packet = None try: while True: rx_bytes = udp_socket_actuator.recv(packet.H2R_PACKET_SIZE) rx_packet = rx_bytes[:] except socket.timeout: if rx_packet is not None: if first_packet: logging.info("Control loop started.") leds.set_color(leds.LEFT, leds.GREEN) leds.set_color(leds.RIGHT, leds.GREEN) first_packet = False tasrx_k = time.perf_counter( ) # reception of the time request from the controller logging.debug("Actuation %d received at %f" % (k, tasrx_k)) receptionStarted = True data_from_host = struct.unpack(packet.H2R_PACKET_FORMAT, rx_packet) seq_no_received = data_from_host[ packet.H2R_PACKET_VARS.Seq_number] # If recieved packet is newer than the one last applied: if seq_no_received == k: # Measure round trip time and get appropriate index tau_RTT = time.perf_counter() - timestamp # Get voltages # Current: voltage_left = float(data_from_host[2]) / 1000000 voltage_right = float(data_from_host[3]) / 1000000 # Predictions: for i in range(no_of_sim_steps): voltagePredictions[2 * i] = float( data_from_host[2 * i + 2]) / 1000000 voltagePredictions[2 * i + 1] = float( data_from_host[2 * i + 3]) / 1000000 # Reset prediction counter prediction_count = 0 motor_voltage_applied_left = voltage_left motor_voltage_applied_right = voltage_right time_last_applied = time.perf_counter() seq_no_applied = seq_no_received while time.perf_counter() <= next_trigger_time - 0.003: pass if log_enabled: f.write("%f,%f,%f,%f,%f\n" % (gyro, enc_l, enc_r, tau_RTT, prediction_count)) SetDuty(motorDutyCycleFile_left, voltage_left) SetDuty(motorDutyCycleFile_right, voltage_right) taw_k = time.perf_counter() break else: pass # logging.debug('Actuator: Not received %d at %f' % (k, time.perf_counter())) except KeyboardInterrupt: udp_socket_actuator.close() logging.debug("Keyboard interrupt, exiting") return except socket.error: logging.debug("Socket error, exiting") traceback.print_exc() return # Killswitch: if up button pressed, turn off motors and exit if buttons.up: SetDuty(motorDutyCycleFile_left, 0) SetDuty(motorDutyCycleFile_right, 0) logging.debug("avg_diff: %f, var_diff: %f, sampling_time: %f", avg_diff, diff_variance, p.SAMPLING_TIME) logging.info("Control loop stopped.") leds.set_color(leds.LEFT, leds.RED) leds.set_color(leds.RIGHT, leds.RED) if log_enabled: f.close() exit() # If time's up, break reception loop if time.perf_counter() >= (next_trigger_time - 0.006): if receptionStarted: if prediction_count < 10: tasrx_k = time.perf_counter( ) # threoretical reception timestamp if prediction_count > 0: logging.debug("Prediction step %d, %d", prediction_count, k) voltage_left = voltagePredictions[2 * prediction_count] voltage_right = voltagePredictions[2 * prediction_count + 1] SetDuty(motorDutyCycleFile_left, voltage_left) SetDuty(motorDutyCycleFile_right, voltage_right) taw_k = time.perf_counter( ) # theoretical application timestamp motor_voltage_applied_left = voltage_left motor_voltage_applied_right = voltage_right prediction_count += 1 seq_no_applied += 1 outdated_warning_printed = False else: if outdated_warning_printed == False: logging.debug( "Warning: No more simulation steps available!") outdated_warning_printed = True else: pass # Robot logging if log_enabled: f.write( "%f,%f,%f,%f,%f\n" % (gyro, enc_l, enc_r, tau_RTT, prediction_count - 1)) break
if (self.rightM.is_running): rflag = 1 print("Right booster is on") pass if __name__ == '__main__': # set up robot object here if using it # testBehav = Test(rob) # pass robot object here if need be rob = SturdyRobot() testBehav = SturdyRobot(rob) # add code to stop robot motors rob.leftM.stop() rob.rightM.stop() buttons = ev3.Button() while (not buttons.any()): while True: if (rob.cs.color == 1): rob.leftM.stop() rob.rightM.stop() print("turning") rob.curve(-0.4, 0.4) time.sleep(.5) pass if ((rob.readDistance()) > 15): rob.wander() print(rob.cs.color) pass if rob.Wary() == 'found':
class Robot: """ -- TEMPLATE -- This class provides logic for moving the sensor and scrolling the bar code cards """ btn = ev3.Button() abc = [None] * 10 light_motor = ev3.LargeMotor('outD') scrool_motor = ev3.LargeMotor('outA') def debug_print(*args, **kwargs): print(*args, **kwargs, file=sys.stderr) def sensor_step(self): """ Moves the sensor one step to read the next bar code value """ # implementation #32.101 self.light_motor.run_to_rel_pos(position_sp=-33, speed_sp=60, stop_action='hold') self.light_motor.wait_while('running') def sensor_reset(self): """ Resets the sensor position """ self.light_motor.run_to_rel_pos(position_sp=355, speed_sp=100, stop_action='hold') self.light_motor.wait_while('running') sleep(0.4) def scroll_step(self): """ Moves the bar code card to the next line. # """ self.scrool_motor.run_to_rel_pos(position_sp=62, speed_sp=100, stop_action='hold') self.scrool_motor.wait_while('running') sleep(0.4) def read_value(self) -> int: """ Reads a single value, converts it and returns the binary expression :return: int """ # implementation ls = ev3.LightSensor('in3') ls.mode = 'REFLECT' light_value = ls.value() if (light_value > 540): light_value = 0 else: light_value = 1 return light_value def special(self): self.light_motor.run_to_rel_pos(position_sp=-3, speed_sp=80, stop_action='hold') self.light_motor.wait_while('running') sleep(0.1) def start_pos(self): self.light_motor.reset() def turn_to_first(self): self.light_motor.run_to_abs_pos(position_sp=0, speed_sp=80, stop_action='hold') def reading(self) -> int: """ Reads a single value, converts it and returns the binary expression :return: int """ # implementation ls = ev3.LightSensor('in3') ls.mode = 'REFLECT' light_value = ls.value() if (light_value > 520): light_value = 0 else: light_value = 1 return light_value
def control_ev3_movements(): """ Builds TKinter GUI. Sets up buttons on TKinter GUI. Is for driving robot in all directions, sending messages from EV3 to PC and displaying on the GUI. """ root = tkinter.Tk() root.title("MQTT Remote") main_frame = ttk.Frame(root, padding=20, relief='raised') main_frame.grid() mqtt_client = com.MqttClient() mqtt_client.connect_to_ev3() left_speed_label = ttk.Label(main_frame, text="Left Speed") left_speed_label.grid(row=0, column=0) left_speed_entry = tkinter.Scale(main_frame, from_=0, to=600, tickinterval=200) # left_speed_entry.insert(0, "600") left_speed_entry.grid(row=1, column=0) right_speed_label = ttk.Label(main_frame, text="Right Speed") right_speed_label.grid(row=0, column=2) right_speed_entry = tkinter.Scale(main_frame, from_=0, to=600, tickinterval=200) # right_speed_entry.insert(0, "600") right_speed_entry.grid(row=1, column=2) forward_button = ttk.Button(main_frame, text="Forward") forward_button.grid(row=10, column=1) # forward_button and '<Up>' key forward_button['command'] = lambda: move_forward( mqtt_client, left_speed_entry, right_speed_entry) root.bind( '<Up>', lambda event: move_forward(mqtt_client, left_speed_entry, right_speed_entry)) left_button = ttk.Button(main_frame, text="Left") left_button.grid(row=11, column=0) # left_button and '<Left>' key left_button['command'] = lambda: turn_left(mqtt_client, left_speed_entry, right_speed_entry) root.bind( '<Left>', lambda event: turn_left(mqtt_client, left_speed_entry, right_speed_entry)) stop_button = ttk.Button(main_frame, text="Stop") stop_button.grid(row=11, column=1) # stop_button and '<space>' key (note, does not need left_speed_entry, right_speed_entry) stop_button['command'] = lambda: stop(mqtt_client) root.bind('<space>', lambda event: stop(mqtt_client)) right_button = ttk.Button(main_frame, text="Right") right_button.grid(row=11, column=2) # right_button and '<Right>' key right_button['command'] = lambda: turn_right(mqtt_client, left_speed_entry, right_speed_entry) root.bind( '<Right>', lambda event: turn_right(mqtt_client, left_speed_entry, right_speed_entry)) back_button = ttk.Button(main_frame, text="Back") back_button.grid(row=12, column=1) # back_button and '<Down>' key back_button['command'] = lambda: move_back(mqtt_client, left_speed_entry, right_speed_entry) root.bind( '<Down>', lambda event: move_back(mqtt_client, left_speed_entry, right_speed_entry)) up_button = ttk.Button(main_frame, text="Up") up_button.grid(row=13, column=0) up_button['command'] = lambda: send_up(mqtt_client) root.bind('<u>', lambda event: send_up(mqtt_client)) down_button = ttk.Button(main_frame, text="Down") down_button.grid(row=14, column=0) down_button['command'] = lambda: send_down(mqtt_client) root.bind('<j>', lambda event: send_down(mqtt_client)) tools = ttk.Label(main_frame, text="Tools To Use:") tools.grid(row=15, column=1) move_button = ttk.Button(main_frame, text="Navigate Maze") move_button.grid(row=16, column=0) move_button['command'] = lambda: drive_to_color(mqtt_client) root.bind('<a>', lambda event: drive_to_color(mqtt_client)) color_button = ttk.Button(main_frame, text="Pick Up Prize") color_button.grid(row=16, column=2) color_button['command'] = lambda: pick_up_prize(mqtt_client) root.bind('<s>', lambda event: pick_up_prize(mqtt_client)) beacon_find = ttk.Button(main_frame, text="Find Prize") beacon_find.grid(row=16, column=1) beacon_find['command'] = lambda: find_prize(mqtt_client) root.bind('<d>', lambda event: find_prize(mqtt_client)) # Buttons for quit and exit q_button = ttk.Button(main_frame, text="Quit") q_button.grid(row=13, column=2) q_button['command'] = (lambda: quit_program(mqtt_client, False)) e_button = ttk.Button(main_frame, text="Exit") e_button.grid(row=14, column=2) e_button['command'] = (lambda: quit_program(mqtt_client, True)) root.mainloop() # code for exiting program when back button on EV3 is pressed. dc = DataContainer btn = ev3.Button() btn.on_backspace = lambda state: handle_shutdown(state, dc)
# Braitenberg Vehicle - Cowardly Behaviour # Designed to be run on legomindstorm ev3 running the ev3dev operating system # Uses Proximity sensors # Directly proportional relationship between sensor output and motor input # Wires are uncrossed import ev3dev.ev3 as ev3 import time stop_button = ev3.Button() right_sensor = ev3.InfraredSensor('in1') right_sensor.mode = 'IR-PROX' assert right_sensor.connected left_sensor = ev3.InfraredSensor('in2') left_sensor.mode = 'IR-PROX' assert left_sensor.connected left_motor = ev3.LargeMotor('outA') assert left_motor.connected right_motor = ev3.LargeMotor('outB') assert right_motor.connected while not stop_button.any(): left_sensor_value = 500 - left_sensor.value() * 5 right_sensor_value = 500 - right_sensor.value() * 5 left_motor.run_forever(speed_sp=left_sensor_value, speed_regulation_enabled='on') right_motor.run_forever(speed_sp=right_sensor_value,
import ev3dev.ev3 as ev3 import time def log(motor): print("speed" + str(motor.speed)) m = ev3.Motor("outA") m2 = ev3.Motor("outB") m3 = ev3.Motor("outD") infrared = ev3.InfraredSensor('in1') color = ev3.ColorSensor('in2') button = ev3.Button() infrared.auto_mode = False infrared.mode = infrared.MODE_IR_PROX print("Ready!") while True: print("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA AAAAAAA") if button.any(): break time.sleep(5) velocity = 100 m.run_direct(duty_cycle_sp=velocity) m2.run_direct(duty_cycle_sp=velocity) m3.run_direct(duty_cycle_sp=0) activeMotor = m
def main(): """print("--------------------------------------------") print(" Buttons and LEDs") print("--------------------------------------------") ev3.Sound.speak("Buttons and L E Dees").wait() # Opening LED dance (to show the LED syntax) # Red LEDs ev3.Sound.speak("Red") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.RED) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.RED) time.sleep(3) # Green LEDs ev3.Sound.speak("Green") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.GREEN) time.sleep(3) # Turn LEDs off ev3.Sound.speak("Off") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) # ev3.Leds.all_off() # Could also use this single command if turning both LEDs off. print('Press the Back button on the EV3 to exit this program.')""" ev3.Sound.speak('Begin Fondling the Buttons') btn = ev3.Button() # Construct the one and only EV3 Button object led_colors = [ev3.Leds.BLACK, # This list is useful for the down button in TO DO 4. ev3.Leds.GREEN, ev3.Leds.RED, # ev3.Leds.ORANGE, # Too close to another color in my opinion # ev3.Leds.YELLOW, # Too close to another color in my opinion ev3.Leds.AMBER] current_color_index = 1 while True: if btn.left: print("Left") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) elif btn.right: print("Right") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.RED) elif btn.up: print("Up") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) elif btn.down: ev3.Leds.set_color(ev3.Leds.LEFT, led_colors[current_color_index]) ev3.Leds.set_color(ev3.Leds.RIGHT, led_colors[current_color_index]) while btn.down: time.sleep(.01) if current_color_index < len(led_colors)-1: current_color_index += 1 else: current_color_index = 0 elif btn.backspace: break time.sleep(.1) ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.GREEN) ev3.Sound.speak("Goodbye").wait()
def main(): print("--------------------------------------------") print(" IR Events with the Screen") print("--------------------------------------------") print("Exit this program with the Back button.") # You will notice very few print commands in this module since this module uses the screen. # If you run Screen programs on the EV3 using Brickman, print commands go to that screen too which causes ugly # screen images. If you run Screen programs via SSH print commands work as expected. # Object that is storing references to images that can be passed into callbacks. dc = DataContainer() display_image(dc.lcd_screen, dc.eyes) # Display an image on the EV3 screen ev3.Sound.speak("I R events with the Screen").wait() # Done: 3. Create a remote control object for channel 1. Add lambda # callbacks for: # .on_red_up to call handle_red_up_1 (that exist already) with state and dc as parameters # .on_red_down to call handle_red_down_1 (that exist already) with state and dc as parameters # .on_blue_up to call handle_blue_up_1 (that exist already) with state and dc as parameters # .on_blue_down to call handle_blue_down_1 (that exist already) with state and dc as parameters rc1 = ev3.RemoteControl(channel=1) rc1.on_red_up = lambda state: handle_red_up_1(state,dc) rc1.on_red_down = lambda state: handle_red_down_1(state,dc) rc1.on_blue_up = lambda state: handle_blue_up_1(state,dc) rc1.on_blue_down = lambda state: handle_blue_down_1(state,dc) # Done: 5. Create remote control objects for channels 2, 3, and 4. Add # lambda callbacks for on_red_up to each one: # Channel 2's .on_red_up should call handle_red_up_2 (that exist already) with state and dc as parameters # Channel 3's .on_red_up should call handle_red_up_3 (that exist already) with state and dc as parameters # Channel 4's .on_red_up should call handle_red_up_4 (that exist already) with state and dc as parameters rc2 = ev3.RemoteControl(channel=2) rc2.on_red_up = lambda state: handle_red_up_2(state,dc) rc3 = ev3.RemoteControl(channel=3) rc3.on_red_up = lambda state: handle_red_up_3(state,dc) rc4 = ev3.RemoteControl(channel=4) rc4.on_red_up = lambda state: handle_red_up_4(state,dc) # Buttons on EV3 btn = ev3.Button() btn.on_backspace = lambda state: handle_shutdown(state, dc) while dc.running: # Done: 4. Call the .process() method on your channel 1 RemoteControl # object, then review and run your code. # Review the handle functions below to see how they draw to the screen. They are already finished. rc1.process() # Done: 6. Call the .process() method on your channel 2 - 4 # RemoteControl objects and demo your code. # Review the handle functions below to see how they draw to the screen. They are already finished. rc2.process() rc3.process() rc4.process() # TODO: 7. Call over a TA or instructor to sign your team's checkoff sheet and do a code review. # # Observations you should make, IR buttons work exactly like buttons on the EV3. # The screen is a bit annoying to work with due to the Brickman OS interference. # Note you could've run this program with Brickman too, but screen draws would last one 1 second each. btn.process() # Monitors for the Back button to exit the program if called. time.sleep(0.01) # When the program completes (the user hit the Back button), display a crying image and say goodbye. display_image(dc.lcd_screen, dc.teary_eyes) ev3.Sound.speak("Goodbye").wait() print("If you ran via SSH and typed 'sudo chvt 6' earlier, don't forget to type") print("'sudo chvt 1' to get Brickman back after you finish this program.")
#!/usr/bin/env python3 import ev3dev.ev3 as ev3 import time lcd = ev3.Screen() # The EV3 display rightMotor = ev3.LargeMotor('outB') # The motor connected to the right wheel leftMotor = ev3.LargeMotor('outC') # The motor connected to the left wheel button = ev3.Button() # Any button camera = ev3.Sensor(address=ev3.INPUT_1) # The camera assert camera.connected, "Error while connecting Pixy camera to port 2" lcd = ev3.Screen() ts = ev3.TouchSensor('in2'); assert ts.connected, "Connect a touch sensor to any port" us = ev3.UltrasonicSensor() us.mode='US-DIST-CM' units = us.units # reports 'cm' even though the sensor measures 'mm' CAMERA_WIDTH_PIXELS = 255 CAMERA_HEIGHT_PIXELS = 255 leftMotor = ev3.LargeMotor('outC') rightMotor = ev3.LargeMotor('outB') lcd = ev3.Screen() ts = ev3.TouchSensor('in2'); assert ts.connected, "Connect a touch sensor to any port" us = ev3.UltrasonicSensor() us.mode='US-DIST-CM'
def main(): print("--------------------------------------------") print(" Buttons and LEDs") print("--------------------------------------------") ev3.Sound.speak("Buttons and L E Dees").wait() # Opening LED dance (to show the LED syntax) # Red LEDs ev3.Sound.speak("Red") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.RED) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.RED) time.sleep(3) # Green LEDs ev3.Sound.speak("Green") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.GREEN) time.sleep(3) # Turn LEDs off ev3.Sound.speak("Off") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) # ev3.Leds.all_off() # Could also use this single command if turning both LEDs off. print('Press the Back button on the EV3 to exit this program.') # Buttons on EV3 (the real focus of this module) btn = ev3.Button() # Construct the one and only EV3 Button object led_colors = [ev3.Leds.BLACK, # This list is useful for the down button in TO DO 4. ev3.Leds.GREEN, ev3.Leds.RED, # ev3.Leds.ORANGE, # Too close to another color in my opinion # ev3.Leds.YELLOW, # Too close to another color in my opinion ev3.Leds.AMBER] current_color_index = 0 while True: # DONE: 3. Implement the left, right, and up buttons as follows: # When the up button is being pressed: # -- print the word "up" # -- turn off all LEDs # When the left button is being pressed: # -- print the word "left" # -- make the LEFT led GREEN # -- turn off the right LED # When the right button is being pressed: # -- print "right" # -- make the RIGHT led RED # -- turn off the left LED # You are required to use the Button instance variables for the up, left, and right (not event callbacks). # Notice that the word "up" (or "left" or "right" is printed continually while you hold the button) # Optional: You can also comment out the code above that does the 6 second red, green, off pattern. It was # there just to provide you with code examples for using the LEDs. It does not need to run anymore. # Just make sure not to comment out too much. ;) if btn.up: print("up") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) if btn.left: print("left") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.BLACK) if btn.right: print("right") ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.BLACK) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.GREEN) # DONE: 4. Implement the down button to change the color of both LEDs. # The first press to down should make both LEDs GREEN, the next press makes them RED, then AMBER, then off. # If the user presses the down button again, wrap around the list to GREEN and continue as before. # If the user holds down the button, figure out how to make the color change still only happen once. # Since you are only allowed to use states, not event callbacks, this last request is a pain, but it's doable # with a while loop that blocks code execution until the down instance variable is False. # Use a time.sleep(0.01) inside the while loop to do nothing but wait for the button to be released. colours = [ev3.Leds.GREEN,ev3.Leds.RED,ev3.Leds.AMBER,ev3.Leds.BLACK] if btn.down: print("down") ev3.Leds.set_color(ev3.Leds.LEFT, colours[current_color_index]) ev3.Leds.set_color(ev3.Leds.RIGHT, colours[current_color_index]) if current_color_index < len(colours)-1: current_color_index += 1 else: current_color_index = 0 while(btn.down): time.sleep(0.01) # TODO: 5. Formally test your work. When you think you have the problem complete run these tests: # Press Left - Green left LED is on (try holding the button down for a few seconds when you to the press) # Press Right - Right right LED is on # Press Up - Both LEDs are off # Press Down - Both LEDs are Green # Press Down - Both LEDs are Red # Press Down - Both LEDs are Amber (try holding the button down for a few seconds when you to the press) # Press Down - Both LEDs are off # Press Down - Both LEDs are Green # Press Down - Both LEDs are Red (the cycle repeats) # Press Back - Both LEDs turn Green, the robot says Goodbye and the program exits # TODO: 6. Call over a TA or instructor to sign your team's checkoff sheet and do a code review. # # Observation you should make, working with buttons as 'states' is functional but usually 'events' work better. # Also observe that we don't use the Enter button. Enter can cause issues since your program is running at the # same time as the Brickman operating system. Both are receiving the button events. That can be changed, but # it's too much trouble to do here. So instead we just don't use the Enter button. if btn.backspace: break time.sleep(0.01) # Best practice to have a short delay to avoid working too hard between loop iterations. # Best practice to leave the LEDs on after you finish a program so you don't put away the robot while still on. ev3.Leds.set_color(ev3.Leds.LEFT, ev3.Leds.GREEN) ev3.Leds.set_color(ev3.Leds.RIGHT, ev3.Leds.GREEN) ev3.Sound.speak("Goodbye").wait()
import time #from client import * m1=ev3.LargeMotor('outA') # front m2=ev3.LargeMotor('outB') # right m3=ev3.LargeMotor('outC') # back m4=ev3.LargeMotor('outD') # left ult1=ev3.UltrasonicSensor('in1') #Front-sensor ult2=ev3.UltrasonicSensor('in2') #Right-sensor ult3=ev3.UltrasonicSensor('in3') #Back-sensor ult4=ev3.UltrasonicSensor('in4') #Left-sensor # For button press: butt = ev3.Button() # Initialise motors and sensors to defaults - will change depending on orientation. m = [m1, m2, m3, m4] frontMotor = m[0] rightMotor = m[1] backMotor = m[2] leftMotor = m[3] ult = [ult1, ult2, ult3, ult4] frontSensor = ult[0] rightSensor = ult[1] backSensor = ult[2] leftSensor = ult[3]