def main(amybot=True, camjambot=False):
joystick = Joystick()
# if elsing this because the init methods of both classes do stuff with hardware, so best to only intiailise deliberately
if amybot:
bot = AmyBot()
else:
bot = CamJamBot()
# 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
# Power settings
voltage_in = 6.0 # Total battery voltage to the PicoBorg Reverse
voltage_out = 5.0 #* 0.95 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power
# Setup the power limits
if voltage_out > voltage_in:
max_power = 1.0
else:
max_power = voltage_out / float(voltage_in)
# Setup pygame and wait for the joystick to become available
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
had_event = True
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved
had_event = True
if had_event:
# Read axis positions (-1 to +1) + determine how much to move by
left_drive, right_drive = joystick.get_reading()
bot.move(left_drive, right_drive)
time.sleep(interval)
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
#LeftMotor.speed(0)
#RightMotor.speed(0)
LOGGER.debug('Motors off')
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
