diff --git a/calc_spm_ramp.py b/calc_spm_ramp.py new file mode 100644 index 0000000..319fc14 --- /dev/null +++ b/calc_spm_ramp.py @@ -0,0 +1,57 @@ +"""Calculate the time for each stroke and then the SPM.""" + +nameplate_hz = 120.0 +nameplate_rpm = 1200.0 + +approach_freq = 20 + +total_stroke_length = 60 +offset_top = 2 +offset_bottom = 2 + +accel_time = 2.0 +decel_time = 2.0 + +thread_pitch = 0.5906 + + +def time_for_freq_change(freq_delta, np_hz, ramp_time): + """Calculate the time required for a frequency change.""" + return (freq_delta / np_hz) * ramp_time + + +def dist_for_freq_change(freq_delta, ramp_time, np_hz, np_rpm, thread_pitch): + """Calculate the distance required for a frequency change.""" + time_required = time_for_freq_change(freq_delta, np_hz, ramp_time) + return 0.75 * freq_delta * (np_rpm / np_hz) * (1.0 / 60.0) * time_required * thread_pitch + + +def time_for_uniform_freq(freq, distance, np_hz, np_rpm, thread_pitch): + """Calculate the time required to move a distance at a frequency.""" + return distance / (freq * (np_rpm / np_hz) * (1.0 / 60.0) * thread_pitch) + + +def calculate_stroke_time(stroke_length, upper_offset, lower_offset, run_frequency, ramp_frequency, accel_time, decel_time): + """Calculate the time it takes to make a full stroke.""" + ramp_up_time = time_for_freq_change(run_frequency, nameplate_hz, accel_time) + ramp_up_dist = dist_for_freq_change(run_frequency, accel_time, nameplate_hz, nameplate_rpm, thread_pitch) + + ramp_down_time = time_for_freq_change(run_frequency - ramp_frequency, nameplate_hz, decel_time) + ramp_down_dist = dist_for_freq_change(run_frequency - ramp_frequency, decel_time, nameplate_hz, nameplate_rpm, thread_pitch) + + turn_time = time_for_freq_change(ramp_frequency, nameplate_hz, decel_time) + turn_dist = dist_for_freq_change(ramp_frequency, decel_time, nameplate_hz, nameplate_rpm, thread_pitch) + + run_dist = total_stroke_length - (offset_bottom + offset_top + ramp_up_dist + ramp_down_dist + turn_dist) + run_time = time_for_uniform_freq(run_frequency, run_dist, nameplate_hz, nameplate_rpm, thread_pitch) + + stroke_time = 2.0 * (ramp_up_time + run_time + ramp_down_time + turn_time) + return stroke_time + + +for ramp_i in [0.2, 0.4, 0.6, 0.8, 1.0, 1.2, 1.4, 1.6, 1.8, 2.0]: + print("-----") + print("Ramp Rate: {}".format(ramp_i)) + for hz_i in [20.0, 30.0, 40.0, 50.0, 60.0, 70.0, 80.0, 90.0, 100.0, 110.0, 120.0]: + spm = 60 / calculate_stroke_time(total_stroke_length, offset_top, offset_bottom, hz_i, approach_freq, ramp_i, ramp_i) + print("Freq: {} Hz., SPM: {}".format(hz_i, spm)) diff --git a/pid_testing.py b/pid_testing.py new file mode 100644 index 0000000..d6997d6 --- /dev/null +++ b/pid_testing.py @@ -0,0 +1,70 @@ +""" +Sample PID calculations. + +Tests the algorithm for updating output frequency based on the current SPM. +""" +import matplotlib.pyplot as plt + +# Nameplate +nameplate_hz = 120.0 +nameplate_rpm = 1200.0 + +# Setpoints +starting_hz = 20.0 +spm_target = 4.0 + +pid_ramp_limit_large = 1.0 +pid_ramp_limit_small = 0.2 + +pid_ramp_hz_large = 5.0 +pid_ramp_hz_small = 1.0 + +hz_setpoint = starting_hz + + +def stroke(last_stroke_spm): + """ + Calculate the new output frequency. + + Input Parameters: + last_stroke_spm: last strokes SPM + """ + global hz_setpoint, spm_target + + error = abs(spm_target - last_stroke_spm) + error_sign = 1 if (spm_target > last_stroke_spm) else -1 + + if error >= pid_ramp_limit_large: + print("Large change") + hz_setpoint += error_sign * pid_ramp_hz_large + elif error >= pid_ramp_limit_small: + print("Small change") + hz_setpoint += error_sign * pid_ramp_hz_small + + return hz_setpoint + + +def loop(): + """Loop and calculate.""" + new_spm = 1.0 + spm_arr = [] + rpm_arr = [] + + while new_spm != 0.0: + new_spm = float(raw_input("New SPM: ")) + hz_sp = stroke(new_spm) + new_rpm = (nameplate_rpm / nameplate_hz) * hz_sp + spm_arr.append(new_spm) + rpm_arr.append(new_rpm) + print("{} SPM, {} RPM".format(new_spm, new_rpm)) + fig, ax1 = plt.subplots() + ax1.plot(spm_arr, "b-") + ax1.set_ylabel("SPM", color="b") + ax2 = ax1.twinx() + ax2.plot(rpm_arr, "r-") + ax2.set_ylabel("RPM", color="r") + fig.tight_layout() + plt.show() + + +loop() diff --git a/speed_testing.py b/speed_testing.py index 3ec5e21..b34cd9c 100644 --- a/speed_testing.py +++ b/speed_testing.py @@ -1,97 +1,151 @@ -from time import sleep -from datetime import datetime +"""Simulates speed testing.""" import matplotlib.pyplot as plt +from time import sleep +from math import ceil -rpm = 200 -dt = 0.010 #seconds + +dt = 0.050 # seconds thread_pitch = 0.5906 -accel_time = 2 # seconds -decel_time = 2 # seconds +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 = 9 # inches -lower_offset = 15 # inches +upper_offset = 2 # inches +lower_offset = 2 # inches -motor_full_speed = 120 # Hz +motor_full_speed_hz = 120 # Hz +motor_full_speed_rpm = 1200 # RPM -def ramp_up(i): - speed = (rpm / ramp_time) * (i * dt) - return speed +def sim(run_freq, approach_freq, stroke_limit): + """Run the main simulation loop.""" + strokes = 0 -def ramp_distance_sim(rpm, ramp_time): + 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 - dist = 0.0 - while time < ramp_time: - speed = (rpm / ramp_time) * time - dist += speed * dt * (1.0 / 60.0) * thread_pitch - time += dt - return dist -def get_ramp_constants(full_speed, accel_time, decel_time): - up_m = full_speed / accel_time - up_b = 0 + direction = 1 + starting_speed = 0.0 # Hz + speed = starting_speed + last_speed = speed - down_m = -1.0 * full_speed / decel_time - down_b = full_speed - return (up_m, up_b, down_m, down_b) - -def strokes(num): - stroke_i = 0 - position = 0.0 - speed = 0.0 - time = 0.0 - (ramp_up_slope, ramp_up_intercept, ramp_down_slope, ramp_down_intercept) = get_ramp_constants(motor_full_speed, accel_time, decel_time) - - while stroke_i < num: - while position < (stroke_length - upper_offset): - if speed < - - stroke_i += 1 - - - -def calc_ramp_distance(rpm, ramp_time): - global thread_pitch - return 0.5 * (ramp_time / 60.0) * rpm * thread_pitch - -def sim(): speed_array = [] - distance_array = [] + position_array = [] + time_array = [] + stroke_part = -1 - distance_travelled = 0 - start = datetime.now() - accel_index = 0 - decel_index = 0 - speed = 0 + 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 - accel_distance = calc_ramp_distance(rpm, accel_time) - decel_distance = calc_ramp_distance(rpm, decel_time) - while distance_travelled < stroke_length: - if distance_travelled < accel_distance: - speed = (rpm / accel_time) * (accel_index * dt) - accel_index += 1 - elif distance_travelled < (stroke_length - decel_distance): - speed = rpm - else: - speed = -1 * (rpm / decel_time) * (decel_index * dt) + rpm - decel_index += 1 - delta_x = speed * (dt / 60.0) * thread_pitch - distance_travelled += delta_x - speed_array.append(speed) - distance_array.append(distance_travelled) - print(speed, distance_travelled) - sleep(dt) + 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 - end = datetime.now() + 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) - stroke_time = end - start - print("Took {} seconds".format(stroke_time)) - plt.plot(speed_array) - plt.plot(distance_array) + 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() -# calculate SPM using ramp time and RPM setpoint -sim() +sim(run_speed, rampdown_speed, 1)