Aufgeräumt
This commit is contained in:
@@ -0,0 +1,220 @@
|
||||
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(
|
||||
'''
|
||||
$$$$$$$$\ $$\ $$\ $$\ $$\
|
||||
$$ _____|\__| \__| $$ | $$ |
|
||||
$$ | $$\ $$$$$$$\ $$\ $$$$$$$\ $$$$$$$\ $$$$$$\ $$$$$$$ |
|
||||
$$$$$\ $$ |$$ __$$\ $$ |$$ _____|$$ __$$\ $$ __$$\ $$ __$$ |
|
||||
$$ __| $$ |$$ | $$ |$$ |\$$$$$$\ $$ | $$ |$$$$$$$$ |$$ / $$ |
|
||||
$$ | $$ |$$ | $$ |$$ | \____$$\ $$ | $$ |$$ ____|$$ | $$ |
|
||||
$$ | $$ |$$ | $$ |$$ |$$$$$$$ |$$ | $$ |\$$$$$$$\ \$$$$$$$ |
|
||||
\__| \__|\__| \__|\__|\_______/ \__| \__| \_______| \_______|
|
||||
|
||||
''')
|
||||
|
||||
Reference in New Issue
Block a user