41 lines
1.1 KiB
Python
41 lines
1.1 KiB
Python
import time
|
|
import math
|
|
from pimoroni_i2c import PimoroniI2C
|
|
from breakout_ioexpander import BreakoutIOExpander
|
|
|
|
PINS_BREAKOUT_GARDEN = {"sda": 4, "scl": 5}
|
|
PINS_PICO_EXPLORER = {"sda": 20, "scl": 21}
|
|
|
|
ioe_servo_pin = 1
|
|
|
|
# Settings to produce a 50Hz output from the 24MHz clock.
|
|
# 24,000,000 Hz / 8 = 3,000,000 Hz
|
|
# 3,000,000 Hz / 60,000 Period = 50 Hz
|
|
divider = 8
|
|
period = 60000
|
|
cycle_time = 5.0
|
|
servo_range = 1000.0 # Between 1000 and 2000us (1-2ms)
|
|
|
|
i2c = PimoroniI2C(**PINS_BREAKOUT_GARDEN)
|
|
ioe = BreakoutIOExpander(i2c, address=0x18)
|
|
|
|
ioe.set_pwm_period(period)
|
|
ioe.set_pwm_control(divider)
|
|
|
|
ioe.set_mode(ioe_servo_pin, BreakoutIOExpander.PIN_PWM)
|
|
|
|
t = 0
|
|
|
|
while True:
|
|
s = math.sin((t * math.pi * 2.0) / cycle_time) / 2.0
|
|
servo_us = 1500.0 + (s * servo_range)
|
|
|
|
duty_per_microsecond = period / (20 * 1000) # Default is 3 LSB per microsecond
|
|
|
|
duty_cycle = round(servo_us * duty_per_microsecond)
|
|
print("Cycle Time: ", t % cycle_time, ", Pulse: ", servo_us, ", Duty Cycle: ", duty_cycle, sep="")
|
|
ioe.output(ioe_servo_pin, duty_cycle)
|
|
|
|
time.sleep(0.02)
|
|
t += 0.02
|