-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathchallenge2-bonus.py
169 lines (148 loc) · 3.86 KB
/
challenge2-bonus.py
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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
#-*- coding:UTF-8 -*-
import RPi.GPIO as GPIO
import time
#Definition of motor pin
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13
TURN90DEGREES=0.33
TURNAROUND=0.66
#Set the GPIO port to BCM encoding mode
GPIO.setmode(GPIO.BCM)
#Ignore warning information
GPIO.setwarnings(False)
#Motor pin initialization operation
def motor_init():
global pwm_ENA
global pwm_ENB
global delaytime
GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
#Set the PWM pin and frequency is 2000hz
pwm_ENA = GPIO.PWM(ENA, 2000)
pwm_ENB = GPIO.PWM(ENB, 2000)
pwm_ENA.start(0)
pwm_ENB.start(0)
#advance
def run(delaytime):
GPIO.output(IN1, GPIO.HIGH)
GPIO.output(IN2, GPIO.LOW)
GPIO.output(IN3, GPIO.HIGH)
GPIO.output(IN4, GPIO.LOW)
pwm_ENA.ChangeDutyCycle(30)
pwm_ENB.ChangeDutyCycle(30)
time.sleep(delaytime)
#back
def back(delaytime):
GPIO.output(IN1, GPIO.LOW)
GPIO.output(IN2, GPIO.HIGH)
GPIO.output(IN3, GPIO.LOW)
GPIO.output(IN4, GPIO.HIGH)
pwm_ENA.ChangeDutyCycle(30)
pwm_ENB.ChangeDutyCycle(30)
time.sleep(delaytime)
#turn left
def left(delaytime):
GPIO.output(IN1, GPIO.LOW)
GPIO.output(IN2, GPIO.LOW)
GPIO.output(IN3, GPIO.HIGH)
GPIO.output(IN4, GPIO.LOW)
pwm_ENA.ChangeDutyCycle(50)
pwm_ENB.ChangeDutyCycle(50)
time.sleep(delaytime)
#turn right
def right(delaytime):
GPIO.output(IN1, GPIO.HIGH)
GPIO.output(IN2, GPIO.LOW)
GPIO.output(IN3, GPIO.LOW)
GPIO.output(IN4, GPIO.LOW)
pwm_ENA.ChangeDutyCycle(50)
pwm_ENB.ChangeDutyCycle(50)
time.sleep(delaytime)
#turn left in place
def spin_left(delaytime):
GPIO.output(IN1, GPIO.LOW)
GPIO.output(IN2, GPIO.HIGH)
GPIO.output(IN3, GPIO.HIGH)
GPIO.output(IN4, GPIO.LOW)
pwm_ENA.ChangeDutyCycle(50)
pwm_ENB.ChangeDutyCycle(50)
time.sleep(delaytime)
#turn right in place
def spin_right(delaytime):
GPIO.output(IN1, GPIO.HIGH)
GPIO.output(IN2, GPIO.LOW)
GPIO.output(IN3, GPIO.LOW)
GPIO.output(IN4, GPIO.HIGH)
pwm_ENA.ChangeDutyCycle(50)
pwm_ENB.ChangeDutyCycle(50)
time.sleep(delaytime)
#brake
def brake(delaytime):
GPIO.output(IN1, GPIO.LOW)
GPIO.output(IN2, GPIO.LOW)
GPIO.output(IN3, GPIO.LOW)
GPIO.output(IN4, GPIO.LOW)
pwm_ENA.ChangeDutyCycle(50)
pwm_ENB.ChangeDutyCycle(50)
time.sleep(delaytime)
#Delay 1s
time.sleep(1)
#The try/except statement is used to detect errors in the try block.
#the except statement catches the exception information and processes it.
#The robot car advance 1s，back 1s，turn left 2s，turn right 2s，turn left in place 3s
#turn right in place 3s，stop 1s。
try:
motor_init()
# T
run(1.2)
spin_left(TURN90DEGREES)
run(0.4)
brake(0.1)
back(0.4)
brake(0.1)
# E
back(0.6)
run(0.6)
spin_left(TURN90DEGREES)
run(0.6)
spin_left(TURN90DEGREES)
run(0.3)
back(0.3)
spin_right(TURN90DEGREES)
run(0.6)
spin_left(TURN90DEGREES)
run(0.6)
brake(0.1)
# C
run(0.6)
spin_left(TURNAROUND)
GPIO.output(IN1, GPIO.HIGH)
GPIO.output(IN2, GPIO.LOW)
GPIO.output(IN3, GPIO.HIGH)
GPIO.output(IN4, GPIO.LOW)
pwm_ENA.ChangeDutyCycle(30)
pwm_ENB.ChangeDutyCycle(10)
time.sleep(5)
brake(0.1)
# H
spin_right(TURN90DEGREES)
run(1.2)
back(0.6)
spin_left(TURN90DEGREES)
run(0.6)
spin_left(TURN90DEGREES)
back(0.6)
run(1.2)
except KeyboardInterrupt:
pass
pwm_ENA.stop()
pwm_ENB.stop()
GPIO.cleanup()