-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathnav_driven.py
executable file
·151 lines (136 loc) · 4.25 KB
/
nav_driven.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
# This script is to control two motors with the navigation keys, using the L293D chip for the motor control.
# The enabling is done via PWM, use PIN #21/27 and PIN #18 from the GPIO as PWM pins.
# For IN1, IN2, IN3 and IN4 use the pins #4, #17, #23 and #24.
# YOU NEED TO RUN THE SCRIPT AS SUDO!
# The motors are controlled with the navigation keys and stopped with the space bar using the curses library for python.
# The motor connected to IN1 and IN2 should be the right motor, the other the left one. If the motor turns backwards when you
# press up, you need to change the cables.
# You can end the program by typing q.
import curses
import RPi.GPIO as io
# GPIO mode is set to BCM numbering
io.setmode(io.BCM)
# name the pins used
# right motor
in1_pin = 17 #bottom
in2_pin = 4 #top
# left motor
in3_pin = 23 #bottom
in4_pin = 24 #top
# right
pwm_right_pin = 27 # this number depends on your version of the Pi. If it's Model A it should be 21
# left
pwm_left_pin = 18
# set the pins as output pins
io.setup(in1_pin, io.OUT)
io.setup(in2_pin, io.OUT)
io.setup(in3_pin, io.OUT)
io.setup(in4_pin, io.OUT)
io.setup(pwm_right_pin, io.OUT)
io.setup(pwm_left_pin, io.OUT)
# set and start the PWM pins
p_right = io.PWM(pwm_right_pin, 50)
p_left = io.PWM(pwm_left_pin, 50)
p_right.start(11)
p_left.start(11)
# methods for turning the motors forwards and backwards for both motors seperately
def forwards_right():
io.output(in1_pin, True)
io.output(in2_pin, False)
def backwards_right():
io.output(in1_pin, False)
io.output(in2_pin, True)
def forwards_left():
io.output(in3_pin, True)
io.output(in4_pin, False)
def backwards_left():
io.output(in3_pin, False)
io.output(in4_pin,True)
def stop():
io.output(in1_pin, False)
io.output(in2_pin, False)
io.output(in3_pin, False)
io.output(in4_pin, False)
def move_on_keys(stdscr):
key='s'
speed_right = 50
speed_left = 50
# main loop, waiting for input and calling the methods above to steer
while True:
# Wait for input
cmd = stdscr.getch()
# stop if input is q
if cmd == ord('q'):
break
elif cmd == curses.KEY_LEFT:
# the if makes the programm only respond to the first time the key is pressed. It doesn't change until another key is pressed.
if key == 'l':
continue
else:
key='l'
# print out what it's doing (just to control if it's working)
stdscr.addstr('left')
forwards_right()
backwards_left()
speed_right = 50
speed_left = 50
elif cmd == curses.KEY_RIGHT:
# the if makes the programm only respond to the first time the key is pressed. It doesn't change until another key is pressed.
if key == 'r':
continue
else:
key='r'
# print out what it's doing (just to control if it's working)
stdscr.addstr('right')
forwards_left()
backwards_right()
speed_right = 50
speed_left = 50
elif cmd == curses.KEY_UP:
# If it's already going forward, it just accelerates
if key == 'u':
speed_right += 10
speed_left += 10
stdscr.addstr(str(speed_left))
else:
key='u'
# print out what it's doing (just to control if it's working)
stdscr.addstr('up')
forwards_left()
forwards_right()
speed_right = 50
speed_left = 50
elif cmd == curses.KEY_DOWN:
# the if makes the programm only respond to the first time the key is pressed. It doesn't change until another key is pressed.
if key == 'd':
continue
else:
key='d'
# print out what it's doing (just to control if it's working)
stdscr.addstr('down')
backwards_left()
backwards_right()
speed_right = 50
speed_left = 50
elif cmd == ord(' '):
# the if makes the programm only respond to the first time the key is pressed. It doesn't change until another key is pressed.
if key == 's':
continue
else:
key='s'
# print out what it's doing (just to control if it's working)
stdscr.addstr('stop')
stop()
# set speed
p_right.ChangeDutyCycle(min(speed_right,100))
p_left.ChangeDutyCycle(min(speed_left,100))
# This initialises curses and starts the method above. The wrapper() function takes care about handling exceptions and quitting
# curses properly
curses.wrapper(move_on_keys)
# Just to make sure it left the loop
print "Engines stopped"
# stop the PWM
p_right.stop()
p_left.stop()
# clean up the GPIO
io.cleanup()