-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.py
171 lines (143 loc) · 5.45 KB
/
main.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
170
171
#!/usr/bin/env python
# coding: Latin-1
# Load library functions we want
import time
import sys
import pygame
from robot import AmyBot, CamJamBot, FourTronix
from joystick import Joystick
from argparse import ArgumentParser
import logging
from time import sleep
from rainbow import Rainbow
import grove_oled
from sonar import Sonar
L1_BUTTON = 6 # L1 rainbow
L2_BUTTON = 8 # R1 remote
R1_BUTTON = 7 # L2 maze
R2_BUTTON = 9 # R2 straight
INTERVAL = 0.0
LOGGER = logging.getLogger(__name__)
class Controller(Rainbow, Sonar):
mode = R1_BUTTON
def __init__(self, amybot=True, cambot=False, fourtronix=False, straight=False):
Sonar.__init__(self)
# Rainbow.__init__(self)
print ("Hello!")
self.last_text = "Bleurgh!"
self.joystick = Joystick()
# if elsing this because the init methods of both classes do stuff with hardware, so best to only intiailise deliberately
if amybot:
self.bot = AmyBot()
LOGGER.info('Enable AmyBot')
elif cambot:
self.bot = CamJamBot()
LOGGER.info('Enable CamJamBot')
elif fourtronix:
self.bot = FourTronix()
LOGGER.info('Enable FourTronixBot')
else:
print("Unknown Robot Type")
sys.exit(0)
self.straight_line_start = False
if straight:
self.mode = R2_BUTTON
self.straight_line_start = True
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr
interval = 0.0
self.modes = {
# L1_BUTTON: self.rainbow,
R1_BUTTON: self.remote,
L2_BUTTON: self.maze,
R2_BUTTON: self.straight}
super().__init__()
def run(self):
grove_oled.oled_init()
grove_oled.oled_clearDisplay()
grove_oled.oled_setNormalDisplay()
grove_oled.oled_setVerticalMode()
self.show ("Amybot!")
try:
LOGGER.info('Press CTRL+C to quit')
running = True
# Loop indefinitely
while running:
# Get the latest events from the system
had_event = False
events = pygame.event.get()
# Handle each event individually
for event in events:
if event.type == pygame.QUIT:
# User exit
running = False
break
elif event.type == pygame.JOYBUTTONDOWN:
# A button on the joystick just got pushed down
self.mode = event.button
if event.button == R2_BUTTON:
self.straight_line_start = not self.straight_line_start
try:
self.modes[self.mode]()
except KeyError:
LOGGER.warning("No mode set for key %i", self.mode)
self.mode = R1_BUTTON
time.sleep(INTERVAL)
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
self.bot.move(0, 0)
self.show('Motors off')
def remote(self):
self.show("Remote mode")
left_drive, right_drive = self.joystick.get_reading()
self.bot.move(left_drive, right_drive)
def maze(self):
self.show("Maze mode")
def adjust_power(self, power, gap):
new_power = (power - (gap / 100))
if new_power > 1.0:
new_power = 1.0
if new_power < -1.0:
new_power = -1.0
return new_power
def straight(self):
self.show("Line mode")
left_power = 1.0
right_power = 1.0
while self.straight_line_start:
try:
dist = self.get_distance()
if (dist < 80):
left_power = self.adjust_power(left_power, 80 - dist)
elif (dist > 100):
right_power = self.adjust_power(right_power, 100 - dist)
else:
left_power = 1.0
right_power = 1.0
sleep(0.1)
print(left_power, right_power)
self.bot.move(left_power, right_power)
except TypeError:
print("Type Error")
except IOError:
print("IO Error")
self.bot.stop()
def show(self, text):
LOGGER.debug(text)
if text != self.last_text:
#grove_oled.oled_clearDisplay()
grove_oled.oled_setTextXY(0,0)
grove_oled.oled_putString(text.center(12))
self.last_text = text
if __name__ == '__main__':
logging.basicConfig(level=logging.DEBUG)
parser = ArgumentParser()
# either fortronix, amybot or camjambot can be passed in, but only one
group = parser.add_mutually_exclusive_group()
group.add_argument("--amybot", help="Use the kit amy has (whatever Jim provided)", action="store_true")
group.add_argument("--camjambot", help="Use the camjam edubot kit", action="store_true")
group.add_argument("--fourtronix", help="Use the 4tronix controller", action="store_true")
parser.add_argument("--straight", help="go straight into straight line mode on start", action="store_true")
args = parser.parse_args()
controller = Controller(args.amybot, args.camjambot, args.fourtronix, args.straight)
controller.run()