152 lines
3.7 KiB
Python
152 lines
3.7 KiB
Python
import gc
|
|
from machine import Pin, PWM
|
|
from pimoroni_i2c import PimoroniI2C
|
|
from plasma import WS2812
|
|
from motor import Motor
|
|
from servo import Servo
|
|
from encoder import Encoder, MMME_CPR
|
|
|
|
# IO Pin Constants
|
|
GP0 = 0
|
|
GP1 = 1
|
|
GP2 = 2
|
|
|
|
A0 = 26
|
|
A1 = 27
|
|
A2 = 28
|
|
|
|
GPIOS = (GP0, GP1, GP2, A0, A1, A2)
|
|
ADCS = (A0, A1, A2)
|
|
|
|
|
|
# Index Constants
|
|
MOTOR_A = 0
|
|
MOTOR_B = 1
|
|
|
|
SERVO_1 = 0
|
|
SERVO_2 = 1
|
|
SERVO_3 = 2
|
|
SERVO_4 = 3
|
|
SERVO_5 = 4
|
|
SERVO_6 = 5
|
|
|
|
LED_GP0 = 0
|
|
LED_GP1 = 1
|
|
LED_GP2 = 2
|
|
LED_A0 = 3
|
|
LED_A1 = 4
|
|
LED_A2 = 5
|
|
LED_SERVO_1 = 6
|
|
LED_SERVO_2 = 7
|
|
LED_SERVO_3 = 8
|
|
LED_SERVO_4 = 9
|
|
LED_SERVO_5 = 10
|
|
LED_SERVO_6 = 11
|
|
|
|
|
|
# Count Constants
|
|
NUM_GPIOS = 6
|
|
NUM_ADCS = 3
|
|
NUM_MOTORS = 2
|
|
NUM_SERVOS = 6
|
|
NUM_LEDS = 12
|
|
|
|
|
|
class Inventor2040W():
|
|
AMP_EN_PIN = 3
|
|
I2C_SDA_PIN = 4
|
|
I2C_SCL_PIN = 5
|
|
MOTOR_A_PINS = (6, 7)
|
|
MOTOR_B_PINS = (8, 9)
|
|
ENCODER_A_PINS = (19, 18)
|
|
ENCODER_B_PINS = (17, 16)
|
|
|
|
SERVO_1_PIN = 10
|
|
SERVO_2_PIN = 11
|
|
SERVO_3_PIN = 12
|
|
SERVO_4_PIN = 13
|
|
SERVO_5_PIN = 14
|
|
SERVO_6_PIN = 15
|
|
|
|
LED_DATA_PIN = 20
|
|
PWM_AUDIO_PIN = 21
|
|
USER_SW_PIN = 22
|
|
|
|
AMP_CORRECTION = 4
|
|
DEFAULT_VOLUME = 0.2
|
|
|
|
def __init__(self, motor_gear_ratio=50, init_motors=True, init_servos=True):
|
|
# Free up hardware resources
|
|
gc.collect()
|
|
|
|
# Set up the motors and encoders, if the user wants them
|
|
self.motors = None
|
|
if init_motors:
|
|
cpr = MMME_CPR * motor_gear_ratio
|
|
self.motors = [Motor(self.MOTOR_A_PINS), Motor(self.MOTOR_B_PINS)]
|
|
# Set the encoders to use PIO 0 and State Machines 0 and 1
|
|
self.encoders = [Encoder(0, 0, self.ENCODER_A_PINS, counts_per_rev=cpr, count_microsteps=True),
|
|
Encoder(0, 1, self.ENCODER_B_PINS, counts_per_rev=cpr, count_microsteps=True)]
|
|
|
|
# Set up the servos, if the user wants them
|
|
self.servos = None
|
|
if init_servos:
|
|
self.servos = [Servo(i) for i in range(self.SERVO_1_PIN, self.SERVO_6_PIN + 1)]
|
|
|
|
# Set up the i2c for Qw/st and Breakout Garden
|
|
self.i2c = PimoroniI2C(self.I2C_SDA_PIN, self.I2C_SCL_PIN, 100000)
|
|
|
|
# Set up the amp enable
|
|
self.__amp_en = Pin(self.AMP_EN_PIN, Pin.OUT)
|
|
self.__amp_en.off()
|
|
|
|
self.audio_pwm = PWM(Pin(self.PWM_AUDIO_PIN))
|
|
self.__volume = self.DEFAULT_VOLUME
|
|
|
|
# Set up the user switch
|
|
self.__switch = Pin(self.USER_SW_PIN, Pin.IN, Pin.PULL_DOWN)
|
|
|
|
# Set up the WS2812 LEDs, using PIO 0 and State Machine 2
|
|
self.leds = WS2812(NUM_LEDS, 0, 2, self.LED_DATA_PIN)
|
|
self.leds.start()
|
|
|
|
def switch_pressed(self):
|
|
return self.__switch.value()
|
|
|
|
def play_tone(self, frequency):
|
|
try:
|
|
self.audio_pwm.freq(frequency)
|
|
except ValueError:
|
|
self.play_silence()
|
|
raise ValueError("frequency of range. Expected greater than 0")
|
|
|
|
corrected_volume = (self.__volume ** 4) # Correct for RC Filter curve
|
|
self.audio_pwm.duty_u16(int(32768 * corrected_volume))
|
|
self.unmute_audio()
|
|
|
|
def play_silence(self):
|
|
self.audio_pwm.freq(44100)
|
|
|
|
corrected_volume = (self.__volume ** 4) # Correct for RC Filter curve
|
|
self.audio_pwm.duty_u16(int(32768 * corrected_volume))
|
|
self.unmute_audio()
|
|
|
|
def stop_playing(self):
|
|
self.audio_pwm.duty_u16(0)
|
|
self.mute_audio()
|
|
|
|
def volume(self, volume=None):
|
|
if volume is None:
|
|
return self.__volume
|
|
|
|
if volume < 0.01 or volume > 1.0:
|
|
raise ValueError("volume out of range. Expected 0.0 to 1.0")
|
|
|
|
self.__volume = volume
|
|
|
|
def mute_audio(self):
|
|
self.__amp_en.off()
|
|
|
|
def unmute_audio(self):
|
|
self.__amp_en.on()
|