ev3dev-python-robot/robot/__init__.py at master · rueedlinger/ev3dev-python-robot · GitHub

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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
# -*- coding: utf-8 -*-

from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import GyroSensor, ColorSensor, InfraredSensor, UltrasonicSensor
import logging

class Robot:

    def __init__(self):

        logging.debug("Setting up...")

        # motors
        right_motor = LargeMotor(OUTPUT_A)
        logging.info("motor right connected: %s" % str(right_motor))
        logging.info("motor right max speed: %s" % str(right_motor.max_speed))

        left_motor = LargeMotor(OUTPUT_B)
        logging.info("motor left connected: %s" % str(right_motor))
        logging.info("motor left max speed: %s" % str(left_motor.max_speed))

        right_motor.reset()
        left_motor.reset()

        self.motors = [left_motor, right_motor]

        self.right_motor = right_motor
        self.left_motor = left_motor
        self.current_speed = 0

        try:

            gyro_sensor = GyroSensor()
            logging.info("gyro sensor connected: %s" % str(gyro_sensor.num_values))

            gyro_sensor.mode = GyroSensor.MODE_GYRO_ANG
            self.gyro_sensor = gyro_sensor

        except Exception:
            logging.exception("message")

        try:
            color_sensor = ColorSensor()
            logging.info("color sensor connected: %s" % str(color_sensor))
            color_sensor.mode = 'COL-REFLECT'
            self.color_sensor = color_sensor

        except Exception:
            logging.exception("message")

        try:
            ultrasonic_sensor = UltrasonicSensor()
            logging.info("ultrasonic sensor connected: %s" % str(ultrasonic_sensor))
            ultrasonic_sensor.mode = 'US-DIST-CM'
            self.ultrasonic_sensor = ultrasonic_sensor

        except Exception:
            logging.exception("message")

        try:
            ir_sensor = InfraredSensor()
            logging.info("ir sensor connected: %s" % str(ir_sensor))
            ir_sensor.mode = 'IR-REMOTE'
            self.ir_sensor = ir_sensor

        except Exception:
            logging.exception("message")

    def forward(self, speed=None):

        if speed:
            self.set_speed(abs(speed))
        else:
            self.set_speed(abs(self.current_speed))

    def backward(self, speed=None):

        if speed:
            self.set_speed(abs(speed) * -1)
        else:
            self.set_speed(abs(self.current_speed) * -1)

    def brake(self):
        self.current_speed = 0
        for m in self.motors:
            m.stop()

    def turn(self, degree=90):


        if degree >= 0:
            self.right_motor.speed_sp = -100
            self.left_motor.speed_sp = 100
            self.right_motor.run_forever()
            self.left_motor.run_forever()

            angle = self.gyro_sensor.value() + degree

            while self.gyro_sensor.value() <= angle:
                pass

        else:
            self.right_motor.speed_sp = 100
            self.left_motor.speed_sp = -100
            self.right_motor.run_forever()
            self.left_motor.run_forever()

            angle = self.gyro_sensor.value() + degree

            while self.gyro_sensor.value() >= angle:
                pass


        self.set_speed(self.current_speed)


    def set_speed(self, speed):

        self.current_speed = speed

        for m in self.motors:
            m.speed_sp = speed
            m.run_forever()

        logging.debug('speed sp (right_motor): %s' % str(self.right_motor.speed_sp))
        logging.debug('speed sp (left_motor): %s' % str(self.left_motor.speed_sp))

        logging.debug('speed (right_motor): %s' % str(self.right_motor.speed))
        logging.debug('speed (left_motor): %s' % str(self.left_motor.speed))

        logging.debug('max speed (right_motor): %s' % str(self.right_motor.max_speed))
        logging.debug('max speed (left_motor): %s' % str(self.left_motor.max_speed))