-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathdriveRover.py
129 lines (110 loc) · 3.18 KB
/
driveRover.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
# Mars Rover Simple Drive Mode
# Similar to motortest.py but integrates servo steering
# Moves: Forward, Reverse, turn Right, turn Left, Stop
# Press Ctrl-C to stop
from __future__ import print_function
import rover, time
#======================================================================
# Reading single character by forcing stdin to raw mode
import sys
import tty
import termios
# Servo numbers
servo_FL = 9
servo_RL = 11
servo_FR = 15
servo_RR = 13
servo_MA = 0
def readchar():
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
if ch == '0x03':
raise KeyboardInterrupt
return ch
def readkey(getchar_fn=None):
getchar = getchar_fn or readchar
c1 = getchar()
if ord(c1) != 0x1b:
return c1
c2 = getchar()
if ord(c2) != 0x5b:
return c1
c3 = getchar()
return chr(0x10 + ord(c3) - 65) # 16=Up, 17=Down, 18=Right, 19=Left arrows
# End of single character reading
#======================================================================
def goForward():
rover.setServo(servo_FL, 0)
rover.setServo(servo_FR, 0)
rover.setServo(servo_RL, 0)
rover.setServo(servo_RR, 0)
rover.forward(speed)
def goReverse():
rover.setServo(servo_FL, 0)
rover.setServo(servo_FR, 0)
rover.setServo(servo_RL, 0)
rover.setServo(servo_RR, 0)
rover.reverse(speed)
def goLeft():
rover.setServo(servo_FL, -20)
rover.setServo(servo_FR, -20)
rover.setServo(servo_RL, 20)
rover.setServo(servo_RR, 20)
def goRight():
rover.setServo(servo_FL, 20)
rover.setServo(servo_FR, 20)
rover.setServo(servo_RL, -20)
rover.setServo(servo_RR, -20)
speed = 60
print ("Drive M.A.R.S. Rover around")
print ("Use arrow keys to steer")
print ("Use , or < to slow down")
print ("Use . or > to speed up")
print ("Press space bar to coast to stop")
print ("Press b to brake and stop quickly")
print ("Press Ctrl-C to end")
print
rover.init(0)
# main loop
try:
while True:
keyp = readkey()
if keyp == 'w' or ord(keyp) == 16:
goForward()
print ('Forward', speed)
elif keyp == 'z' or ord(keyp) == 17:
goReverse()
print ('Reverse', speed)
elif keyp == 's' or ord(keyp) == 18:
goRight()
print ('Go Right', speed)
elif keyp == 'a' or ord(keyp) == 19:
goLeft()
print ('Go Left', speed)
elif keyp == '.' or keyp == '>':
speed = min(100, speed+10)
print ('Speed+', speed)
elif keyp == ',' or keyp == '<':
speed = max (0, speed-10)
print ('Speed-', speed)
elif keyp == ' ':
rover.stop()
print ('Stop')
elif keyp == 'b':
rover.brake()
rover.setServo(servo_FL, 0)
rover.setServo(servo_FR, 0)
rover.setServo(servo_RL, 0)
rover.setServo(servo_RR, 0)
print ('Brake')
elif ord(keyp) == 3:
break
except KeyboardInterrupt:
pass
finally:
rover.cleanup()