Skip to content
Snippets Groups Projects
Select Git revision
  • multicube-i2c
  • main default protected
  • imu-fusion
  • maker-faire
  • bugfix/flash-drive-strength
  • bugfix/slower-flash
  • refactor/portable-ev3-core
  • hbridge-experiments
  • jv-obhajoba
  • feature/gc-collectible-objects
  • jv-robosoutez
  • py-rt-test
12 results
Blame Permalink
ev3_gyro.py 3.02 KiB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78 






'''
Example program for the EV3 Gyro sensor
Measure and show angle and angular speed on display.
'''
import utime

from lib.robot_consts import Button, Sensor
from ev3_sensor_ll import SensorNotReadyError, SensorMismatchError
from .display_exception import display_incorrect_sensor, display_sensor_not_connected

mode_str = (("Angle"), ("Angle and speed"), ("Tilt angle"), ("Tilt speed"))
N_MODES = len(mode_str)

def ev3_gyro_run(robot, chosen_sensor_port):
    robot.init_sensor(sensor_type=Sensor.EV3_GYRO, port=chosen_sensor_port)
    
    robot.display.fill(0)
    robot.display.centered_text(f"EV3 Gyro S{chosen_sensor_port+1}", 0, 1)
    robot.display.centered_text("Connecting...", 16, 1)
    robot.display.show()

    try:
        robot.sensors.gyro[chosen_sensor_port].angle()
    except SensorNotReadyError as e:
        print(f"EV3 Gyro S{chosen_sensor_port+1} sensor not connected")
        display_sensor_not_connected(robot)
        return
    except SensorMismatchError as e:
        print(f"EV3 Gyro S{chosen_sensor_port+1} incorrect sensor connected")
        display_incorrect_sensor(robot)
        return

    while True:
        robot.display.fill(0)
        robot.display.centered_text(f"EV3 Gyro S{chosen_sensor_port+1}", 0, 1)
        robot.display.text(mode_str[mode], 0, 10, 1)
        try:
            if mode == 0:
                angle = robot.sensors.gyro[chosen_sensor_port].angle()
                robot.display.text(f"Angle: {angle}°", 0, 16, 1)
            elif mode == 1:
                angle, speed = robot.sensors.gyro[chosen_sensor_port].angle_and_speed()
                robot.display.text(f"Angle: {angle}°", 0, 16, 1)
                robot.display.text(f"Speed: {speed}", 0, 24, 1)
            elif mode == 2:
                angle = robot.sensors.gyro[chosen_sensor_port].tilt_angle()
                robot.display.text(f"Angle: {angle}°", 0, 16, 1)
            elif mode == 3:
                speed = robot.sensors.gyro[chosen_sensor_port].tilt_speed()
                robot.display.text(f"Speed: {speed}°", 0, 16, 1)
        except:
            print(f"EV3 Gyro S{chosen_sensor_port+1} sensor not connected")
            display_sensor_not_connected(robot)
            return
        robot.display.text('<', 0, 54, 1)
        robot.display.show()
        
        pressed = robot.buttons.pressed()
        # Exit program if left cube button is pressed
        if pressed[Button.LEFT]:
            break
        if not debounce:
            if pressed[Button.DOWN]:
                robot.sensors.gyro[chosen_sensor_port].reset_angle(0)
                mode += 1
                mode %= N_MODES
                debounce = True
                print("Mode:", mode_str[mode])
            elif pressed[Button.UP]:
                robot.sensors.gyro[chosen_sensor_port].reset_angle(0)
                mode -= 1
                mode %= N_MODES
                debounce = True
                print("Mode:", mode_str[mode])
            
        if not pressed[Button.DOWN] and not pressed[Button.UP]:
            debounce = False
        utime.sleep(0.1)