95 lines
3.0 KiB
Python
95 lines
3.0 KiB
Python
import Adafruit_PCA9685
|
|
import yaml
|
|
import time
|
|
import os
|
|
|
|
config_filename = '../config/exomy.yaml'
|
|
WHEELS = ('fl', 'fr', 'cl', 'cr', 'rl', 'rr')
|
|
|
|
|
|
def get_driving_pins():
|
|
with open(config_filename, 'r') as file:
|
|
param_dict = yaml.load(file)
|
|
|
|
return [param_dict['pin_drive_' + wheel] for wheel in WHEELS]
|
|
|
|
def get_drive_pwm_neutral_values():
|
|
with open(config_filename, 'r') as file:
|
|
param_dict = yaml.load(file)
|
|
|
|
values = {}
|
|
for wheel in WHEELS:
|
|
key = 'drive_pwm_neutral_' + wheel
|
|
if key not in param_dict:
|
|
print('The parameter ' + key + ' could not be found in the exomy.yaml \n')
|
|
print('It was set to the default value: 300\n')
|
|
values[wheel] = 300
|
|
else:
|
|
values[wheel] = param_dict[key]
|
|
return values
|
|
|
|
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 values are 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_dict = get_drive_pwm_neutral_values()
|
|
pin_list = get_driving_pins()
|
|
|
|
for index, pin in enumerate(pin_list):
|
|
pwm.set_pwm(pin, 0, value_dict[WHEELS[index]])
|
|
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)
|