ev3dev-lang-python-demo/robots/ROBODOZ3R/robodoz3r.py at 9b6b3219eb5628ec6e0d04fe8cdb6c7f1587b75c · ev3dev/ev3dev-lang-python-demo · 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
137
138
139
140
141
142
143
144
145
#!/usr/bin/env python3


from ev3dev.ev3 import (
    Motor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C,
    TouchSensor, RemoteControl, INPUT_1, INPUT_4,
    Screen, Sound
)

from time import sleep, time

from rc_tank_util import RemoteControlledTank


class RoboDoz3r(RemoteControlledTank):
    def __init__(
            self,
            left_motor_port: str = OUTPUT_C, right_motor_port: str = OUTPUT_B,
            shovel_motor_port: str = OUTPUT_A,
            touch_sensor_port: str = INPUT_1,
            ir_sensor_port: str = INPUT_4,
            tank_drive_ir_beacon_channel: int = 1,
            shovel_control_ir_beacon_channel: int = 4):
        super().__init__(
            left_motor_port=left_motor_port, right_motor_port=right_motor_port,
            polarity=Motor.POLARITY_INVERSED,
            ir_sensor_port=ir_sensor_port,
            ir_beacon_channel=tank_drive_ir_beacon_channel)

        self.shovel_motor = MediumMotor(address=shovel_motor_port)

        self.touch_sensor = TouchSensor(address=touch_sensor_port)

        self.shovel_control_remote_control = \
            RemoteControl(
                sensor=self.ir_sensor,
                channel=shovel_control_ir_beacon_channel)

        self.screen = Screen()
        self.speaker = Sound()

    def raise_or_lower_shovel_by_ir_beacon(self):
        """
        If the channel 4 is selected on the IR remote
        then you can control raising and lowering the shovel on the RoboDoz3r.

        Raise the shovel by pressing either Up button,
        or lower the shovel by pressing either Down button.
        """
        # raise the shovel
        if self.shovel_control_remote_control.red_up or \
                self.shovel_control_remote_control.blue_up:
            self.shovel_motor.run_forever(speed_sp=100)

        # lower the shovel
        elif self.shovel_control_remote_control.red_down or \
                self.shovel_control_remote_control.blue_down:
            self.shovel_motor.run_forever(speed_sp=-100)

        else:
            self.shovel_motor.stop(stop_action=Motor.STOP_ACTION_HOLD)

    def main(self, driving_speed: float = 1000):
        """
        This is the main control program for the RoboDoz3r
        """
        self.screen.draw.text(
            xy=(2, 2),
            text='ROBODOZ3R',
            fill=None,
            font=None,
            anchor=None,
            spacing=4,
            align='left',
            direction=None,
            features=None,
            language=None,
            stroke_width=0,
            stroke_fill=None)
        self.screen.update()

        self.speaker.play(wav_file='media/Motor start.wav').wait()

        # Engine Idle:
        # Let the engine sound idle for 2 seconds
        motor_idle_start_time = time()
        while time() - motor_idle_start_time <= 2:
            self.speaker.play(
                wav_file='media/Motor idle.wav').wait()

        while True:
            # Driving Mode:
            # Manual mode where the movement of the RoboDoz3r is controlled
            # by the IR remote
            while not self.touch_sensor.is_pressed:
                self.raise_or_lower_shovel_by_ir_beacon()
                self.drive_by_ir_beacon(speed=driving_speed)
                sleep(0.01)

            self.speaker.play(wav_file='media/Airbrake.wav').wait()

            # Auto Mode:
            # In autonomous mode the RoboDoz3r uses the IR sensor
            # in proximity mode to detect nearby obstacles in its path
            while not self.touch_sensor.is_pressed:
                if self.ir_sensor.proximity < 50:
                    self.left_motor.stop(stop_action=Motor.STOP_ACTION_HOLD)
                    self.right_motor.stop(stop_action=Motor.STOP_ACTION_HOLD)

                    sleep(1)

                    self.left_motor.run_timed(
                        speed_sp=-300,
                        time_sp=1000,
                        stop_action=Motor.STOP_ACTION_HOLD)
                    self.right_motor.run_timed(
                        speed_sp=-300,
                        time_sp=1000,
                        stop_action=Motor.STOP_ACTION_HOLD)
                    self.left_motor.wait_while(Motor.STATE_RUNNING)
                    self.right_motor.wait_while(Motor.STATE_RUNNING)

                    self.left_motor.run_timed(
                        speed_sp=500,
                        time_sp=1000,
                        stop_action=Motor.STOP_ACTION_HOLD)
                    self.right_motor.run_timed(
                        speed_sp=-500,
                        time_sp=1000,
                        stop_action=Motor.STOP_ACTION_HOLD)
                    self.left_motor.wait_while(Motor.STATE_RUNNING)
                    self.right_motor.wait_while(Motor.STATE_RUNNING)

                else:
                    self.left_motor.run_forever(speed_sp=500)
                    self.right_motor.run_forever(speed_sp=500)

                sleep(0.01)

            self.speaker.play(wav_file='media/Airbrake.wav').wait()


if __name__ == '__main__':
    ROBODOZ3R = RoboDoz3r()
    ROBODOZ3R.main()