def main():
############################################# SETUP PARAMETERS ################################################
pull_duration = 0.07 # First part of the cycle - over this amount of time the motor pulls the cable with full Troque
release_duration = 0.15 # next part of the cycle - a gradual release starting at max torque ending at standby_torque
duty_cycle = 0.8 #duty cycle of the test
max_torque = 3 #maximum torque exerted by the motor
standbay_torque = 0.05 #between pulls the motor still exerts some torque - to keep the cable tight
###################################################################################################################
c = Controller(controller_ID=1) #create controller object
c.command_stop() #comand_stop cleares anny errors/faults in the controller
max_measured_velocity = 0 #this variable stores the max measured rotational velocity of the motor and is used for safety and can be observed as an erformance indicator
counter = 0 #if you had to interrupt testing and need to start again but not count from 0 - input the number starting number you want here.
time_zero = time.time()
previous_phase = 0 #variable used to detect switching from one cycle to the next
while True:
phase = (time.time() - time_zero) % (
duty_cycle
) #what is happening during the cycle depends on the phase value. Phase is in seconds
#below - the behaviour of the system is dependent on the value of phase - in different value ranges different behaviours are triggered
if phase < pull_duration:
#initiall full torque pull
measurements = c.set_position(
position=0,
max_torque=4,
ff_torque=max_torque,
kp_scale=0,
kd_scale=0,
get_data=True,
print_data=False
) #measurements are collected to extract velocity - which if too high stops the system (safety)
elif phase >= pull_duration and phase <= pull_duration + release_duration:
#gradual release
release_phase = (
(pull_duration + release_duration - phase) / release_duration
) #value normalized from 1 to 0.
torque = standbay_torque + (
max_torque - standbay_torque
) * release_phase #torque ges from max_torque to standby_torque
measurements = c.set_position(
position=0,
max_torque=4,
ff_torque=torque,
kp_scale=0,
kd_scale=0,
get_data=True,
print_data=False
) #measurements are collected to extract velocity - which if too high stops the system (safety)
else:
measurements = c.set_position(
position=0,
max_torque=4,
ff_torque=standbay_torque,
kp_scale=0,
kd_scale=0.1,
get_data=True,
print_data=False
) #measurements are collected to extract velocity - which if too high stops the system (safety)
if max_measured_velocity < abs(
measurements[MoteusReg.MOTEUS_REG_VELOCITY]): #for diagnostics
max_measured_velocity = abs(
measurements[MoteusReg.MOTEUS_REG_VELOCITY])
print("Max velocity that occured so far: " +
str(abs(measurements[MoteusReg.MOTEUS_REG_VELOCITY])))
if max_measured_velocity > 25: break
if (previous_phase > phase): #code for counting phase cycles
counter = counter + 1
print("Temperature = " +
str(measurements[MoteusReg.MOTEUS_REG_TEMP_C]) +
" Counter = " + str(counter))
previous_phase = phase
c.set_position(
position=0,
velocity=0,
max_torque=1,
ff_torque=0,
kp_scale=0,
kd_scale=1,
get_data=True,
print_data=False
) #if there is a break in the loop above - the velocity limit is exceeded - this command stops the rotr
def main():
############################################### SETUP PARAMETERS ############################################
rod_length = 500 # rod length from center of motor to the center of the bearing in meters
max_torque = 4 # range of commanded torques will be from zero to this value
step_count = 20 # how many measurements you want to do
ramp_up_time = 0.2 # no to create a hard hit torque will ramp up to the max value over this amount of time
hold_time = 1 # if no user input is provided motor will turn off after this time to prevent overheating
safety_max_velocity = 1 #if rotational velocity gets higher than this the machine stops commanding torque
direction_flip = True
###################################################################################################################
# don't know how to do it yet but we need functionality to redo a measurement
print("Welcome to Motor Cracker")
print(
"Please set all the settable parameters in code. You will be asked to perform a series of measurements. \n"
"To start a measurement you will click Enter. Than the motor will start exerting force on the scale, it will \n"
"quickly ramp it up (over ramp_up_time) instead of instantly turning it on to avoid a sudden hit. \n"
"It will hold torque for some time (hold_time) and than release. You need to read out the value shown by the \n"
"scale during the time when the motor holds (in grams) and input it into this program, and click enter to go \n"
"to next measurement. If you already read the value and want the motor to releace before hold_time passes - click Space\n"
)
# create multi-dim array by providing shape
results = numpy.empty(shape=(step_count + 1, 2), dtype='object')
results[0, 0] = 0
results[0, 1] = 0
if direction_flip: direction = -1
else: direction = 1
c = Controller(controller_ID=1)
for i in range(1, step_count + 1):
c.command_stop(
) #comand_stop cleares anny errors/faults in the controller
input("Start looking at the scale and click enter")
#wait for user to start the test
time.sleep(0.5) #to give user time to switch attention
torque_reached_in_this_cycle = (
max_torque / step_count
) * i #torque that will be commanded in this measurement step
#code to ramp up torque from 0 to torque_reached_in_this_cycle over ramp_up_time
time_before_ramp_start = time.time()
while time.time() <= time_before_ramp_start + ramp_up_time:
commanded_torque = ((time.time() - time_before_ramp_start) /
ramp_up_time) * torque_reached_in_this_cycle
measurement = c.set_torque(torque=direction * commanded_torque,
get_data=True,
print_data=False)
if abs(measurement[
MoteusReg.MOTEUS_REG_VELOCITY]) > safety_max_velocity:
c.command_stop(
) #this will disable the motor fi it reaches a velocity over safety_max_velocity
break
#code to hold the torque_reached_in_this_cycle over the hold_time
time_before_measurement = time.time()
while time.time() <= time_before_measurement + hold_time:
commanded_torque = torque_reached_in_this_cycle
measurement = c.set_torque(torque=direction * commanded_torque,
get_data=True,
print_data=False)
if abs(measurement[
MoteusReg.MOTEUS_REG_VELOCITY]) > safety_max_velocity:
c.command_stop(
) # this will disable the motor fi it reaches a velocity over safety_max_velocity
break
#if "user clicks space" : break
scale_redout = input(
"commanded " + str(int(torque_reached_in_this_cycle * 100) / 100) +
" Nm. \nPlease input the redout from the kitchen scale (in grams!) : "
)
real_torque = float(
scale_redout
) * rod_length / 100000 #calculation and unit conversion
# code to put those two values it in a new line in a CSV file
results[i, 0] = torque_reached_in_this_cycle
results[i, 1] = real_torque
time.sleep(
torque_reached_in_this_cycle * 2
) # wait between measurements to let the motor cool down to minimise the importance of thermal effects - this is not what we are measuring in this test
print(results)