AutomatedObstacleAvoidingRobot/main.py at main · ProgPals/AutomatedObstacleAvoidingRobot · 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
from machine import Pin, PWM  # importing PIN and PWM
import time  # importing time
import utime

# Defining motor pins
In1 = Pin(0, Pin.OUT)
In2 = Pin(1, Pin.OUT)
In3 = Pin(2, Pin.OUT)
In4 = Pin(3, Pin.OUT)
# Defining enable pins and PWM object
# Defining  Trigger and Echo pins
trigger = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)

# Defining  Servo pin and PWM object
servoPin = Pin(0)
servo = PWM(servoPin)
duty_cycle = 0  # Defining and initializing duty cycle PWM

# Defining frequency for servo and enable pins
servo.freq(50)
EN_A.freq(1000)
EN_B.freq(1000)

# Setting maximum duty cycle for maximum speed
EN_A.duty_u16(65025)
EN_B.duty_u16(65025)

# Forward


def move_forward():
    In1.high()
    In2.low()
    In3.high()
    In4.low()

# Backward


def move_backward():
    In1.low()
    In2.high()
    In3.low()
    In4.high()

# Turn Right


def turn_right():
    In1.low()
    In2.low()
    In3.low()
    In4.high()

# Turn Left


def turn_left():
    In1.low()
    In2.high()
    In3.low()
    In4.low()

# Stop


def stop():
    In1.low()
    In2.low()
    In3.low()
    In4.low()

# Defining function to get distance from ultrasonic sensor


def get_distance():
    trigger.low()
    utime.sleep_us(2)
    trigger.high()
    utime.sleep_us(5)
    trigger.low()
    while echo.value() == 0:
        signaloff = utime.ticks_us()
    while echo.value() == 1:
        signalon = utime.ticks_us()
    timepassed = signalon - signaloff
    dist = (timepassed * 0.0343) / 2
    return dist

# Defining function to set servo angle


def setservo(angle):
    duty_cycle = int(angle*(7803-1950)/180) + 1950
    servo.duty_u16(duty_cycle)


setservo(90)

while True:
    distance = get_distance()  # Getting distance in cm

    # Defining direction based on conditions
    if distance < 15:
        stop()
        move_backward()
        time.sleep(1)
        stop()
        time.sleep(0.5)
        setservo(30)  # Servo angle to 30 degree
        time.sleep(1)
        right_distance = get_distance()
        # print(right_distance)
        time.sleep(0.5)
        setservo(150)  # Servo angle to 150 degree
        time.sleep(1)
        left_distance = get_distance()
        # print(left_distance)
        time.sleep(0.5)
        setservo(90)

        if right_distance > left_distance:
            turn_right()
            time.sleep(2)
            stop()
        else:
            turn_left()
            time.sleep(2)
            stop()
    else:
        move_forward()

    time.sleep(0.5)