221 lines
6.9 KiB
Python
221 lines
6.9 KiB
Python
import Adafruit_PCA9685
|
|
import time
|
|
from shutil import copyfile
|
|
import os
|
|
|
|
DRIVE_MOTOR, STEER_MOTOR = [0, 1]
|
|
|
|
pos_names = {
|
|
1: 'fl',
|
|
2: 'fr',
|
|
3: 'cl',
|
|
4: 'cr',
|
|
5: 'rl',
|
|
6: 'rr',
|
|
}
|
|
|
|
pin_dict = {
|
|
|
|
}
|
|
|
|
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 = 2.4 # ms
|
|
on_time_2 = 1.5 # ms
|
|
|
|
# Duty cycle is the percentage of a cycle the signal is on
|
|
duty_cycle_1 = on_time_1/cycle
|
|
duty_cycle_2 = on_time_2/cycle
|
|
|
|
# The PCA 9685 board requests a 12 bit number for the duty_cycle
|
|
value_1 = 200 #int(duty_cycle_1*4096.0)
|
|
value_2 = 400#int(duty_cycle_2*4096.0)
|
|
|
|
class Motor():
|
|
def __init__(self, pin):
|
|
|
|
self.pin_name = 'pin_'
|
|
self.pin_number = pin
|
|
|
|
def wiggle_motor(self):
|
|
|
|
# Set the motor to the second value
|
|
pwm.set_pwm(self.pin_number, 0, value_2)
|
|
# Wait for 1 seconds
|
|
time.sleep(1.0)
|
|
# Set the motor to the first value
|
|
pwm.set_pwm(self.pin_number, 0, value_1)
|
|
# Wait for 1 seconds
|
|
time.sleep(1.0)
|
|
# Set the motor to neutral
|
|
pwm.set_pwm(self.pin_number, 0, 307)
|
|
# Wait for half seconds
|
|
time.sleep(0.5)
|
|
# Stop the motor
|
|
pwm.set_pwm(self.pin_number, 0, 0)
|
|
|
|
def stop_motor(self):
|
|
# Turn the motor off
|
|
pwm.set_pwm(self.pin_number, 0, 0)
|
|
|
|
|
|
def print_exomy_layout():
|
|
print(
|
|
'''
|
|
1 fl-||-fr 2
|
|
||
|
|
3 cl-||-cr 4
|
|
5 rl====rr 6
|
|
'''
|
|
)
|
|
|
|
|
|
def update_config_file():
|
|
file_name = '../config/exomy.yaml'
|
|
template_file_name = file_name+'.template'
|
|
|
|
if not os.path.exists(file_name):
|
|
copyfile(template_file_name, file_name)
|
|
print("exomy.yaml.template was copied to exomy.yaml")
|
|
|
|
output = ''
|
|
with open(file_name, 'rt') as file:
|
|
for line in file:
|
|
for key, value in pin_dict.items():
|
|
if(key in line):
|
|
line = line.replace(line.split(': ', 1)[
|
|
1], str(value) + '\n')
|
|
|
|
break
|
|
output += line
|
|
|
|
with open(file_name, 'w') as file:
|
|
file.write(output)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
print(
|
|
'''
|
|
$$$$$$$$\ $$\ $$\
|
|
$$ _____| $$$\ $$$ |
|
|
$$ | $$\ $$\ $$$$$$\ $$$$\ $$$$ |$$\ $$\
|
|
$$$$$\ \$$\ $$ |$$ __$$\ $$\$$\$$ $$ |$$ | $$ |
|
|
$$ __| \$$$$ / $$ / $$ |$$ \$$$ $$ |$$ | $$ |
|
|
$$ | $$ $$< $$ | $$ |$$ |\$ /$$ |$$ | $$ |
|
|
$$$$$$$$\ $$ /\$$\ \$$$$$$ |$$ | \_/ $$ |\$$$$$$$ |
|
|
\________|\__/ \__| \______/ \__| \__| \____$$ |
|
|
$$\ $$ |
|
|
\$$$$$$ |
|
|
\______/
|
|
'''
|
|
)
|
|
print(
|
|
'''
|
|
###############
|
|
Motor Configuration
|
|
|
|
This scripts leads you through the configuration of the motors.
|
|
First we have to find out, to which pin of the PWM board a motor is connected.
|
|
Look closely which motor moves and type in the answer.
|
|
|
|
Ensure to run the script until the end, otherwise your changes will not be saved!
|
|
This script can always be stopped with ctrl+c and restarted.
|
|
All other controls will be explained in the process.
|
|
###############
|
|
'''
|
|
)
|
|
|
|
for pin_number in range(16):
|
|
motor = Motor(pin_number)
|
|
motor.stop_motor()
|
|
|
|
for pin_number in range(16):
|
|
motor = Motor(pin_number)
|
|
motor.wiggle_motor()
|
|
type_selection = ''
|
|
while(1):
|
|
print("Pin #{}".format(pin_number))
|
|
print(
|
|
'Was it a steering or driving motor that moved, or should I repeat the movement? ')
|
|
type_selection = raw_input('(d)rive (s)teer (r)epeat - (n)one (f)inish_configuration\n')
|
|
if(type_selection == 'd'):
|
|
motor.pin_name += 'drive_'
|
|
print('Good job\n')
|
|
break
|
|
elif(type_selection == 's'):
|
|
motor.pin_name += 'steer_'
|
|
print('Good job\n')
|
|
break
|
|
elif(type_selection == 'r'):
|
|
print('Look closely\n')
|
|
motor.wiggle_motor()
|
|
elif(type_selection == 'n'):
|
|
print('Skipping pin')
|
|
break
|
|
elif(type_selection == 'f'):
|
|
print('Finishing calibration at pin {}.'.format(pin_number))
|
|
break
|
|
else:
|
|
print('Input must be d, s, r, n or f\n')
|
|
|
|
if (type_selection == 'd' or type_selection == 's'):
|
|
while(1):
|
|
print_exomy_layout()
|
|
pos_selection = raw_input(
|
|
'Type the position of the motor that moved.[1-6] or (r)epeat\n')
|
|
if(pos_selection == 'r'):
|
|
print('Look closely\n')
|
|
else:
|
|
try:
|
|
pos = int(pos_selection)
|
|
if(pos >= 1 and pos <= 6):
|
|
motor.pin_name += pos_names[pos]
|
|
break
|
|
else:
|
|
print('The input was not a number between 1 and 6\n')
|
|
except ValueError:
|
|
print('The input was not a number between 1 and 6\n')
|
|
|
|
pin_dict[motor.pin_name] = motor.pin_number
|
|
print('Motor set!\n')
|
|
print('########################################################\n')
|
|
elif (type_selection == 'f'):
|
|
break
|
|
|
|
|
|
print('Now we will step through all the motors and check whether they have been assigned correctly.\n')
|
|
print('Press ctrl+c if something is wrong and start the script again. \n')
|
|
|
|
for pin_name in pin_dict:
|
|
print('moving {}'.format(pin_name))
|
|
print_exomy_layout()
|
|
|
|
pin = pin_dict[pin_name]
|
|
motor = Motor(pin)
|
|
motor.wiggle_motor()
|
|
raw_input('Press button to continue')
|
|
|
|
print("You assigned {}/12 motors.".format(len(pin_dict.keys())))
|
|
|
|
print('Write to config file.\n')
|
|
update_config_file()
|
|
print(
|
|
'''
|
|
$$$$$$$$\ $$\ $$\ $$\ $$\
|
|
$$ _____|\__| \__| $$ | $$ |
|
|
$$ | $$\ $$$$$$$\ $$\ $$$$$$$\ $$$$$$$\ $$$$$$\ $$$$$$$ |
|
|
$$$$$\ $$ |$$ __$$\ $$ |$$ _____|$$ __$$\ $$ __$$\ $$ __$$ |
|
|
$$ __| $$ |$$ | $$ |$$ |\$$$$$$\ $$ | $$ |$$$$$$$$ |$$ / $$ |
|
|
$$ | $$ |$$ | $$ |$$ | \____$$\ $$ | $$ |$$ ____|$$ | $$ |
|
|
$$ | $$ |$$ | $$ |$$ |$$$$$$$ |$$ | $$ |\$$$$$$$\ \$$$$$$$ |
|
|
\__| \__|\__| \__|\__|\_______/ \__| \__| \_______| \_______|
|
|
|
|
''')
|
|
|