217 lines
6.5 KiB
Python
217 lines
6.5 KiB
Python
|
# This file is part of the Micro Python project, http://micropython.org/
|
||
|
|
#
|
||
|
|
# The MIT License (MIT)
|
||
|
|
#
|
||
|
|
# Copyright (c) 2017 Glenn Ruben Bakke
|
||
|
|
#
|
||
|
|
# Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
|
|
# of this software and associated documentation files (the "Software"), to deal
|
||
|
|
# in the Software without restriction, including without limitation the rights
|
||
|
|
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
|
|
# copies of the Software, and to permit persons to whom the Software is
|
||
|
|
# furnished to do so, subject to the following conditions:
|
||
|
|
#
|
||
|
|
# The above copyright notice and this permission notice shall be included in
|
||
|
|
# all copies or substantial portions of the Software.
|
||
|
|
#
|
||
|
|
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
|
|
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
|
|
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
|
|
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
|
|
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
|
|
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||
|
|
# THE SOFTWARE
|
||
|
|
|
||
|
|
# MicroPython controller for PowerUp 3.0 paper airplane
|
||
|
|
# https://www.poweruptoys.com/products/powerup-v3
|
||
|
|
#
|
||
|
|
# Examples is written for nrf52832, pca10040 using s132 bluetooth stack.
|
||
|
|
#
|
||
|
|
# Joystick shield pin mapping:
|
||
|
|
# - analog stick x-direction - ADC0 - P0.02/"A02"
|
||
|
|
# - buttons P0.13 - P0.16 / "A13", "A14", "A15", "A16"
|
||
|
|
#
|
||
|
|
# Example usage:
|
||
|
|
#
|
||
|
|
# from powerup import PowerUp3
|
||
|
|
# p = PowerUp3()
|
||
|
|
|
||
|
|
import time
|
||
|
|
from machine import ADC
|
||
|
|
from machine import Pin
|
||
|
|
from ubluepy import Peripheral, Scanner, constants
|
||
|
|
|
||
|
|
def bytes_to_str(bytes):
|
||
|
|
string = ""
|
||
|
|
for b in bytes:
|
||
|
|
string += chr(b)
|
||
|
|
return string
|
||
|
|
|
||
|
|
def get_device_names(scan_entries):
|
||
|
|
dev_names = []
|
||
|
|
for e in scan_entries:
|
||
|
|
scan = e.getScanData()
|
||
|
|
if scan:
|
||
|
|
for s in scan:
|
||
|
|
if s[0] == constants.ad_types.AD_TYPE_COMPLETE_LOCAL_NAME:
|
||
|
|
dev_names.append((e, bytes_to_str(s[2])))
|
||
|
|
return dev_names
|
||
|
|
|
||
|
|
def find_device_by_name(name):
|
||
|
|
s = Scanner()
|
||
|
|
scan_res = s.scan(500)
|
||
|
|
|
||
|
|
device_names = get_device_names(scan_res)
|
||
|
|
# print(device_names)
|
||
|
|
for dev in device_names:
|
||
|
|
if name == dev[1]:
|
||
|
|
return dev[0]
|
||
|
|
|
||
|
|
class PowerUp3:
|
||
|
|
def __init__(self):
|
||
|
|
self.x_adc = ADC(1)
|
||
|
|
|
||
|
|
self.button_speed_up = Pin("A13", mode=Pin.IN, pull=Pin.PULL_UP)
|
||
|
|
self.button_speed_down = Pin("A15", mode=Pin.IN, pull=Pin.PULL_UP)
|
||
|
|
self.button_speed_full = Pin("A14", mode=Pin.IN, pull=Pin.PULL_UP)
|
||
|
|
self.button_speed_off = Pin("A16", mode=Pin.IN, pull=Pin.PULL_UP)
|
||
|
|
|
||
|
|
self.x_mid = 0
|
||
|
|
|
||
|
|
self.calibrate()
|
||
|
|
self.connect()
|
||
|
|
self.loop()
|
||
|
|
|
||
|
|
def read_stick_x(self):
|
||
|
|
return self.x_adc.value()
|
||
|
|
|
||
|
|
def button_speed_up(self):
|
||
|
|
return bool(self.button_speed_up.value())
|
||
|
|
|
||
|
|
def button_speed_down(self):
|
||
|
|
return bool(self.button_speed_down.value())
|
||
|
|
|
||
|
|
def button_speed_full(self):
|
||
|
|
return bool(self.button_speed_full.value())
|
||
|
|
|
||
|
|
def button_speed_off(self):
|
||
|
|
return bool(self.button_speed_off.value())
|
||
|
|
|
||
|
|
def calibrate(self):
|
||
|
|
self.x_mid = self.read_stick_x()
|
||
|
|
|
||
|
|
def __str__(self):
|
||
|
|
return "calibration x: %i, y: %i" % (self.x_mid)
|
||
|
|
|
||
|
|
def map_chars(self):
|
||
|
|
s = self.p.getServices()
|
||
|
|
|
||
|
|
service_batt = s[3]
|
||
|
|
service_control = s[4]
|
||
|
|
|
||
|
|
self.char_batt_lvl = service_batt.getCharacteristics()[0]
|
||
|
|
self.char_control_speed = service_control.getCharacteristics()[0]
|
||
|
|
self.char_control_angle = service_control.getCharacteristics()[2]
|
||
|
|
|
||
|
|
def battery_level(self):
|
||
|
|
return int(self.char_batt_lvl.read()[0])
|
||
|
|
|
||
|
|
def speed(self, new_speed=None):
|
||
|
|
if new_speed == None:
|
||
|
|
return int(self.char_control_speed.read()[0])
|
||
|
|
else:
|
||
|
|
self.char_control_speed.write(bytearray([new_speed]))
|
||
|
|
|
||
|
|
def angle(self, new_angle=None):
|
||
|
|
if new_angle == None:
|
||
|
|
return int(self.char_control_angle.read()[0])
|
||
|
|
else:
|
||
|
|
self.char_control_angle.write(bytearray([new_angle]))
|
||
|
|
|
||
|
|
def connect(self):
|
||
|
|
dev = None
|
||
|
|
|
||
|
|
# connect to the airplane
|
||
|
|
while not dev:
|
||
|
|
dev = find_device_by_name("TailorToys PowerUp")
|
||
|
|
if dev:
|
||
|
|
# print(dev.addr())
|
||
|
|
self.p = Peripheral()
|
||
|
|
self.p.connect(dev.addr())
|
||
|
|
|
||
|
|
# locate interesting characteristics
|
||
|
|
self.map_chars()
|
||
|
|
|
||
|
|
def rudder_center(self):
|
||
|
|
if self.old_angle != 0:
|
||
|
|
self.old_angle = 0
|
||
|
|
self.angle(0)
|
||
|
|
|
||
|
|
def rudder_left(self, angle):
|
||
|
|
steps = (angle // self.interval_size_left)
|
||
|
|
new_angle = 7 - steps
|
||
|
|
|
||
|
|
if self.old_angle != new_angle:
|
||
|
|
self.angle(new_angle)
|
||
|
|
self.old_angle = new_angle
|
||
|
|
|
||
|
|
def rudder_right(self, angle):
|
||
|
|
steps = (angle // self.interval_size_right)
|
||
|
|
new_angle = -steps
|
||
|
|
|
||
|
|
if self.old_angle != new_angle:
|
||
|
|
self.angle(new_angle)
|
||
|
|
self.old_angle = new_angle
|
||
|
|
|
||
|
|
def throttle(self, speed):
|
||
|
|
if (speed > 100):
|
||
|
|
speed = 100
|
||
|
|
elif (speed < 0):
|
||
|
|
speed = 0
|
||
|
|
|
||
|
|
if self.old_speed != speed:
|
||
|
|
self.speed(speed)
|
||
|
|
self.old_speed = speed
|
||
|
|
|
||
|
|
def loop(self):
|
||
|
|
adc_threshold = 10
|
||
|
|
right_threshold = self.x_mid + adc_threshold
|
||
|
|
left_threshold = self.x_mid - adc_threshold
|
||
|
|
|
||
|
|
self.interval_size_left = self.x_mid // 7
|
||
|
|
self.interval_size_right = (255 - self.x_mid) // 7
|
||
|
|
|
||
|
|
self.old_angle = 0
|
||
|
|
self.old_speed = 0
|
||
|
|
|
||
|
|
while True:
|
||
|
|
|
||
|
|
time.sleep_ms(100)
|
||
|
|
|
||
|
|
# read out new angle
|
||
|
|
new_angle = self.read_stick_x()
|
||
|
|
if (new_angle < 256):
|
||
|
|
if (new_angle > right_threshold):
|
||
|
|
self.rudder_right(new_angle - self.x_mid)
|
||
|
|
elif (new_angle < left_threshold):
|
||
|
|
self.rudder_left(new_angle)
|
||
|
|
else:
|
||
|
|
self.rudder_center()
|
||
|
|
|
||
|
|
# read out new speed
|
||
|
|
new_speed = self.old_speed
|
||
|
|
|
||
|
|
# TODO: bool return not working correctly, so test negated for now.
|
||
|
|
if not self.button_speed_up():
|
||
|
|
new_speed += 25
|
||
|
|
elif not self.button_speed_down():
|
||
|
|
new_speed -= 25
|
||
|
|
elif not self.button_speed_full():
|
||
|
|
new_speed = 100
|
||
|
|
elif not self.button_speed_off():
|
||
|
|
new_speed = 0
|
||
|
|
else:
|
||
|
|
pass
|
||
|
|
|
||
|
|
self.throttle(new_speed)
|