148 lines
4.9 KiB
Python
148 lines
4.9 KiB
Python
import Adafruit_PCA9685
|
|
import yaml
|
|
import time
|
|
import os
|
|
|
|
config_filename = '../config/exomy.yaml'
|
|
|
|
|
|
def get_steering_motor_pins():
|
|
steering_motor_pins = {}
|
|
with open(config_filename, 'r') as file:
|
|
param_dict = yaml.load(file)
|
|
|
|
for param_key, param_value in param_dict.items():
|
|
if('pin_steer_' in str(param_key)):
|
|
steering_motor_pins[param_key] = param_value
|
|
return steering_motor_pins
|
|
|
|
def get_steering_pwm_neutral_values():
|
|
steering_pwm_neutral_values = {}
|
|
with open(config_filename, 'r') as file:
|
|
param_dict = yaml.load(file)
|
|
|
|
for param_key, param_value in param_dict.items():
|
|
if('steer_pwm_neutral_' in str(param_key)):
|
|
steering_pwm_neutral_values[param_key] = param_value
|
|
return steering_pwm_neutral_values
|
|
|
|
|
|
def get_position_name(name):
|
|
position_name = ''
|
|
if('_fl' in name):
|
|
position_name = 'Front Left'
|
|
elif('_fr' in name):
|
|
position_name = 'Front Right'
|
|
elif('_cl' in name):
|
|
position_name = 'Center Left'
|
|
elif('_cr' in name):
|
|
position_name = 'Center Right'
|
|
elif('_rl' in name):
|
|
position_name = 'Rear Left'
|
|
elif('_rr' in name):
|
|
position_name = 'Rear Right'
|
|
|
|
return position_name
|
|
|
|
|
|
def update_config_file(steering_pwm_neutral_dict):
|
|
output = ''
|
|
with open(config_filename, 'rt') as file:
|
|
for line in file:
|
|
for key, value in steering_pwm_neutral_dict.items():
|
|
if(key in line):
|
|
line = line.replace(line.split(': ', 1)[
|
|
1], str(value) + '\n')
|
|
|
|
break
|
|
output += line
|
|
|
|
with open(config_filename, 'w') as file:
|
|
file.write(output)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
print(
|
|
'''
|
|
$$$$$$$$\ $$\ $$\
|
|
$$ _____| $$$\ $$$ |
|
|
$$ | $$\ $$\ $$$$$$\ $$$$\ $$$$ |$$\ $$\
|
|
$$$$$\ \$$\ $$ |$$ __$$\ $$\$$\$$ $$ |$$ | $$ |
|
|
$$ __| \$$$$ / $$ / $$ |$$ \$$$ $$ |$$ | $$ |
|
|
$$ | $$ $$< $$ | $$ |$$ |\$ /$$ |$$ | $$ |
|
|
$$$$$$$$\ $$ /\$$\ \$$$$$$ |$$ | \_/ $$ |\$$$$$$$ |
|
|
\________|\__/ \__| \______/ \__| \__| \____$$ |
|
|
$$\ $$ |
|
|
\$$$$$$ |
|
|
\______/
|
|
'''
|
|
)
|
|
print(
|
|
'''
|
|
This script helps you to set the neutral pwm values for the steering motors.
|
|
You will iterate over all steering motors and set them to a neutral position.
|
|
The determined value is written to the config file.
|
|
|
|
Commands:
|
|
a - Decrease value for current pin
|
|
d - Increase value for current pin
|
|
q - Finish setting value for current pin
|
|
|
|
[Every of these commands must be confirmed with the enter key]
|
|
|
|
ctrl+c - Exit script
|
|
------------------------------------------------------------------------------
|
|
'''
|
|
)
|
|
|
|
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()
|
|
# 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 # 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
|
|
|
|
# The PCA 9685 board requests a 12 bit number for the duty_cycle
|
|
initial_value = int(duty_cycle*4096.0)
|
|
|
|
# Get all steering pins
|
|
steering_motor_pins = get_steering_motor_pins()
|
|
pwm_neutral_dict = get_steering_pwm_neutral_values()
|
|
# Iterating over all motors and fine tune the zero value
|
|
for pin_name, pin_value in steering_motor_pins.items():
|
|
pwm_neutral_name = pin_name.replace('pin_steer_', 'steer_pwm_neutral_')
|
|
pwm_neutral_value = pwm_neutral_dict[pwm_neutral_name]
|
|
|
|
print('Set ' + get_position_name(pin_name) + ' steering motor: \n')
|
|
while(1):
|
|
# Set motor
|
|
pwm.set_pwm(pin_value, 0, pwm_neutral_value)
|
|
time.sleep(0.1)
|
|
print('Current value: ' + str(pwm_neutral_value) + '\n')
|
|
input = raw_input(
|
|
'q-set / a-decrease pwm neutral value/ d-increase pwm neutral value\n')
|
|
if(input is 'q'):
|
|
print('PWM neutral value for ' + get_position_name(pin_name) +
|
|
' has been set.\n')
|
|
break
|
|
elif(input is 'a'):
|
|
print('Decreased pwm neutral value')
|
|
pwm_neutral_value-= 5
|
|
elif(input is 'd'):
|
|
print('Increased pwm neutral value')
|
|
pwm_neutral_value += 5
|
|
pwm_neutral_dict[pwm_neutral_name] = pwm_neutral_value
|
|
update_config_file(pwm_neutral_dict)
|
|
print("Finished configuration!!!")
|