forked from idg10/4tronix-rover-simulator
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdriveRover.py
155 lines (131 loc) · 3.82 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
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
# 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 time
# choose between real rover and simulator
import roversimulator as rover
#import rover
# Servo numbers
servo_FL = 9
servo_RL = 11
servo_FR = 15
servo_RR = 13
servo_MA = 0
# this turns out not to work on Windows.
# #======================================================================
# # Reading single character by forcing stdin to raw mode
# import sys
# import tty
# import termios
# 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
# only works on Windows...
import msvcrt
def readchar():
key = msvcrt.getch()
val = 0
for v in key:
val = v + (val << 8)
return chr(val)
def readkey(getchar_fn=None):
getchar = getchar_fn or readchar
c1 = getchar()
if ord(c1) == 0xe0:
# On Windows, we get this with arrow keys
# and it's followed by a number indicating
# direction
d = getchar()
if d == 'K': return chr(19) # Left
if d == 'M': return chr(18) # Right
if d == 'H': return chr(16) # Up
if d == 'P': return chr(17) # Left
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()