152 lines
6.2 KiB
Python
152 lines
6.2 KiB
Python
"""Simulates speed testing."""
|
|
import matplotlib.pyplot as plt
|
|
from time import sleep
|
|
from math import ceil
|
|
|
|
|
|
dt = 0.050 # seconds
|
|
thread_pitch = 0.5906
|
|
|
|
run_speed = 240.0 # Hz
|
|
rampdown_speed = 20.0 # Hz
|
|
|
|
accel_time = 2.0 # seconds
|
|
decel_time = 2.0 # seconds
|
|
|
|
stroke_length = 60 # inches
|
|
upper_offset = 2 # inches
|
|
lower_offset = 2 # inches
|
|
|
|
motor_full_speed_hz = 120 # Hz
|
|
motor_full_speed_rpm = 1200 # RPM
|
|
|
|
|
|
def sim(run_freq, approach_freq, stroke_limit):
|
|
"""Run the main simulation loop."""
|
|
strokes = 0
|
|
|
|
time_to_rampdown = ((run_freq - rampdown_speed) / motor_full_speed_hz) * decel_time
|
|
print("Rampdown Time: {} s.".format(time_to_rampdown))
|
|
distance_to_rampdown = 0.75 * (run_freq - rampdown_speed) * (motor_full_speed_rpm / motor_full_speed_hz) * (1.0 / 60.0) * time_to_rampdown * 0.5906
|
|
print("Rampdown Distance: {} in.".format(distance_to_rampdown))
|
|
|
|
time_to_turnaround = (rampdown_speed / motor_full_speed_hz) * decel_time
|
|
print("Turn Time: {} s.".format(time_to_turnaround))
|
|
distance_to_turnaround = 0.75 * rampdown_speed * (motor_full_speed_rpm / motor_full_speed_hz) * (1.0 / 60.0) * time_to_turnaround * 0.5906
|
|
print("Turn Distance: {} in.".format(distance_to_turnaround))
|
|
|
|
upper_rampdown_target = stroke_length - (upper_offset + distance_to_rampdown)
|
|
upper_turnaround_target = stroke_length - (upper_offset + distance_to_turnaround)
|
|
upper_offset_target = stroke_length - upper_offset
|
|
lower_rampdown_target = lower_offset + distance_to_rampdown
|
|
lower_turnaround_target = lower_offset + distance_to_turnaround
|
|
|
|
print("--")
|
|
print("Targets")
|
|
print("Lower Offset: {} in.".format(lower_offset))
|
|
print("Upper Rampdown Target: {} in.".format(upper_rampdown_target))
|
|
print("Turnaround Target: {} in.".format(upper_turnaround_target))
|
|
print("Upper Offset: {} in.".format(upper_offset_target))
|
|
print("Lower Rampdown Target: {} in.".format(lower_rampdown_target))
|
|
print("Lower Turnaround Target: {} in.".format(lower_turnaround_target))
|
|
print("--")
|
|
|
|
position = lower_offset
|
|
time = 0.0
|
|
|
|
direction = 1
|
|
starting_speed = 0.0 # Hz
|
|
speed = starting_speed
|
|
last_speed = speed
|
|
|
|
speed_array = []
|
|
position_array = []
|
|
time_array = []
|
|
stroke_part = -1
|
|
|
|
while strokes != stroke_limit:
|
|
last_speed = speed
|
|
if direction == 1:
|
|
if position < upper_rampdown_target:
|
|
# below offset and rampdown distance, ramp up to setpoint, then run at setpoint
|
|
if stroke_part != 0:
|
|
stroke_part = 0
|
|
print("{} - {}".format(stroke_part, position))
|
|
if speed < run_freq:
|
|
speed += (motor_full_speed_hz / accel_time) * dt
|
|
if speed > run_freq:
|
|
speed = run_freq
|
|
elif position >= upper_rampdown_target and position < upper_turnaround_target:
|
|
# between rampdown distance and turnaround distance, ramp to rampdown speed, then run at rampdown speed
|
|
if stroke_part != 1:
|
|
stroke_part = 1
|
|
print("{} - {}".format(stroke_part, position))
|
|
if speed > rampdown_speed:
|
|
speed += -1.0 * (motor_full_speed_hz / decel_time) * dt
|
|
elif position >= upper_turnaround_target:
|
|
# above turnaround distance, ramp to 0 and change direction
|
|
if stroke_part != 2:
|
|
stroke_part = 2
|
|
print("{} - {}".format(stroke_part, position))
|
|
if speed > 0:
|
|
speed += -1.0 * (motor_full_speed_hz / decel_time) * dt
|
|
if speed <= 0:
|
|
speed = 0
|
|
direction = -1
|
|
|
|
elif direction == -1:
|
|
if position > lower_rampdown_target:
|
|
# above offset and rampdown distance, ramp up to setpoint (negative), then run at setpoint
|
|
if stroke_part != 3:
|
|
stroke_part = 3
|
|
print("{} - {}".format(stroke_part, position))
|
|
if speed > (-1.0 * run_freq):
|
|
speed += -1.0 * (motor_full_speed_hz / accel_time) * dt
|
|
if speed < (-1.0 * run_freq):
|
|
speed = -1.0 * run_freq
|
|
elif position > lower_turnaround_target and position <= lower_rampdown_target:
|
|
# between turnaround distance and rampdown distance, ramp ro rampdown speed, then run at rampdown speed
|
|
if stroke_part != 4:
|
|
stroke_part = 4
|
|
print("{} - {}".format(stroke_part, position))
|
|
if speed < (-1.0 * rampdown_speed):
|
|
speed += 1.0 * (motor_full_speed_hz / decel_time) * dt
|
|
elif position <= lower_turnaround_target:
|
|
# below turnaround distance, ramp to 0 and change direction
|
|
if stroke_part != 5:
|
|
stroke_part = 5
|
|
print("{} - {}".format(stroke_part, position))
|
|
if speed < 0:
|
|
speed += 1.0 * (motor_full_speed_hz / decel_time) * dt
|
|
if speed >= 0:
|
|
speed = 0
|
|
direction = 1
|
|
strokes += 1
|
|
|
|
delta_x = (speed + last_speed) / 2.0 * (motor_full_speed_rpm / motor_full_speed_hz) * (dt / 60.0) * thread_pitch
|
|
# delta_x = speed * (motor_full_speed_rpm / motor_full_speed_hz) * (dt / 60.0) * thread_pitch
|
|
position += delta_x
|
|
time += dt
|
|
speed_array.append(speed * (motor_full_speed_rpm / motor_full_speed_hz))
|
|
position_array.append(position)
|
|
time_array.append(time)
|
|
print("Time: {} sec., Position: {} in., Speed: {}".format(time, position, speed * (motor_full_speed_rpm / motor_full_speed_hz)))
|
|
# sleep(dt)
|
|
|
|
SPM = 60.0 / (time / strokes)
|
|
max_position = max(position_array)
|
|
min_position = min(position_array)
|
|
print("Max: {}, Min: {}".format(max_position, min_position))
|
|
print("SPM: {}".format(SPM))
|
|
fig, ax1 = plt.subplots()
|
|
ax1.plot(time_array, speed_array, "b-")
|
|
ax1.set_ylabel("RPM", color="b")
|
|
ax2 = ax1.twinx()
|
|
ax2.plot(time_array, position_array, "r-")
|
|
ax2.set_ylabel("in.", color="r")
|
|
fig.tight_layout()
|
|
plt.show()
|
|
|
|
|
|
sim(run_speed, rampdown_speed, 1)
|