from __future__ import division
import time
# Import the PCA9685 module.
import Adafruit_PCA9685
# Uncomment to enable debug output.
#import logging
#logging.basicConfig(level=logging.DEBUG)
# Initialise the PCA9685 using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685()
# Alternatively specify a different address and/or bus:
#pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)
# Configure min and max servo pulse lengths
servo_min = 150 # Min pulse length out of 4096
servo_max = 600 # Max pulse length out of 4096
# Helper function to make setting a servo pulse width simpler.
def set_servo_pulse(channel, pulse):
pulse_length = 1000000 # 1,000,000 us per second
pulse_length //= 60 # 60 Hz
print('{0}us per period'.format(pulse_length))
pulse_length //= 4096 # 12 bits of resolution
print('{0}us per bit'.format(pulse_length))
pulse *= 1000
pulse //= pulse_length
pwm.set_pwm(channel, 0, pulse)
# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)
import numpy as np
from hk import *
NN = 100
hk = HK(idim=12, odim=12)
print('Moving servo on channel 0, press Ctrl-C to quit...')
input = np.random.rand(12).reshape(12,1)
print(input)
for i in range(1000):
input = hk.step(input)
output = np.floor(np.abs(input.squeeze())* NN) - int(NN/2) + 375
print(output[0])
pwm.set_pwm(1, 0, int(output[0]))
pwm.set_pwm(2, 0, int(output[1]))
pwm.set_pwm(3, 0, int(output[2]))
pwm.set_pwm(4, 0, int(output[3]))
pwm.set_pwm(5, 0, int(output[4]))
pwm.set_pwm(6, 0, int(output[5]))
pwm.set_pwm(7, 0, int(output[6]))
pwm.set_pwm(8, 0, int(output[7]))
pwm.set_pwm(9, 0, int(output[8]))
pwm.set_pwm(10, 0, int(output[9]))
pwm.set_pwm(11, 0, int(output[10]))
pwm.set_pwm(12, 0, int(output[11]))
time.sleep(0.1)
print("Homeokinesis output: ", output)
pwm.set_pwm(1, 0, 375)
pwm.set_pwm(2, 0, 375)
pwm.set_pwm(3, 0, 375)
pwm.set_pwm(4, 0, 375)
pwm.set_pwm(5, 0, 375)
pwm.set_pwm(6, 0, 375)
pwm.set_pwm(7, 0, 375)
pwm.set_pwm(8, 0, 375)
pwm.set_pwm(9, 0, 375)
pwm.set_pwm(10, 0, 375)
pwm.set_pwm(11, 0, 375)
pwm.set_pwm(12, 0, 375)
Quarupted robot running no-input homeokinesis
最新推荐文章于 2023-02-16 10:19:39 发布