97 lines
2.9 KiB
Python
97 lines
2.9 KiB
Python
import Adafruit_PCA9685
|
|
import yaml
|
|
import time
|
|
import os
|
|
|
|
config_filename = '../config/exomy.yaml'
|
|
|
|
|
|
def get_driving_pins():
|
|
pin_list = []
|
|
with open(config_filename, 'r') as file:
|
|
param_dict = yaml.load(file)
|
|
|
|
for key, value in param_dict.items():
|
|
if('pin_drive_' in str(key)):
|
|
pin_list.append(value)
|
|
return pin_list
|
|
|
|
def get_drive_pwm_neutral():
|
|
|
|
with open(config_filename, 'r') as file:
|
|
param_dict = yaml.load(file)
|
|
|
|
for key, value in param_dict.items():
|
|
if('drive_pwm_neutral' in str(key)):
|
|
return value
|
|
|
|
default_value = 300
|
|
print('The parameter drive_pwm_neutral could not be found in the exomy.yaml \n')
|
|
print('It was set to the default value: '+ default_value + '\n')
|
|
return default_value
|
|
|
|
if __name__ == "__main__":
|
|
print(
|
|
'''
|
|
$$$$$$$$\ $$\ $$\
|
|
$$ _____| $$$\ $$$ |
|
|
$$ | $$\ $$\ $$$$$$\ $$$$\ $$$$ |$$\ $$\
|
|
$$$$$\ \$$\ $$ |$$ __$$\ $$\$$\$$ $$ |$$ | $$ |
|
|
$$ __| \$$$$ / $$ / $$ |$$ \$$$ $$ |$$ | $$ |
|
|
$$ | $$ $$< $$ | $$ |$$ |\$ /$$ |$$ | $$ |
|
|
$$$$$$$$\ $$ /\$$\ \$$$$$$ |$$ | \_/ $$ |\$$$$$$$ |
|
|
\________|\__/ \__| \______/ \__| \__| \____$$ |
|
|
$$\ $$ |
|
|
\$$$$$$ |
|
|
\______/
|
|
'''
|
|
)
|
|
print(
|
|
'''
|
|
This script helps you to set the neutral values of PWM of the driving motors correctly.
|
|
It will send the intended signal for "not moving" to all the motors.
|
|
On each motor you have to turn the correction screw until the motor really stands still.
|
|
'''
|
|
)
|
|
|
|
if not os.path.exists(config_filename):
|
|
print("exomy.yaml does not exist. Finish config_motor_pins.py to generate it.")
|
|
exit()
|
|
|
|
|
|
pwm = Adafruit_PCA9685.PCA9685()
|
|
|
|
'''
|
|
The drive_pwm_neutral value is determined from the exomy.yaml file.
|
|
But it can be also calculated from the values of the PWM board and motors,
|
|
like shown in the following calculation:
|
|
|
|
# For most motors a pwm frequency of 50Hz is normal
|
|
pwm_frequency = 50.0 # Hz
|
|
pwm.set_pwm_freq(pwm_frequency)
|
|
|
|
# The cycle is the inverted frequency converted to milliseconds
|
|
cycle = 1.0/pwm_frequency * 1000.0 # 20 ms
|
|
|
|
# The time the pwm signal is set to on during the duty cycle
|
|
on_time = 1.5 # ms
|
|
|
|
# Duty cycle is the percentage of a cycle the signal is on
|
|
duty_cycle = on_time/cycle # 0.075
|
|
|
|
# The PCA 9685 board requests a 12 bit number for the duty_cycle
|
|
value = int(duty_cycle*4096.0) # 307
|
|
'''
|
|
|
|
value = get_drive_pwm_neutral()
|
|
pin_list = get_driving_pins()
|
|
|
|
for pin in pin_list:
|
|
pwm.set_pwm(pin, 0, value)
|
|
time.sleep(0.1)
|
|
|
|
raw_input('Press any button if you are done to complete configuration\n')
|
|
|
|
for pin in pin_list:
|
|
pwm.set_pwm(pin, 0, 0)
|