# ============================================================================== # File Name: S3RTT.py # Author: feecat # Version: V1.0 # Description: Trapezoidal Velocity Profile Generator # Website: https://github.com/feecat/S7RTT # License: Apache License Version 2.0 # ============================================================================== # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # ============================================================================== # Description: # This module generates trajectory data using a Trapezoidal Velocity Profile # (T-Curve) rather than an S-Curve (Sigmoid/Jerk-limited profile). # # The T-Curve approach significantly reduces computational complexity and # CPU consumption, making it highly suitable for resource-constrained # embedded devices. # # Note: # This Python version is intended for algorithm verification and testing. # For the production-ready embedded implementation, please refer to the # C language header file 'S3RTT.h'. # ============================================================================== import tkinter as tk from tkinter import ttk import math import matplotlib.pyplot as plt from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg S3_EPS = 1e-5 class MotionState: def __init__(self, dt=0.0, p=0.0, v=0.0, a=0.0): self.dt = dt self.p = p self.v = v self.a = a class Path: def __init__(self): self.nodes = [] # List of MotionState def push(self, dt, p, v, a): if dt > S3_EPS: self.nodes.append(MotionState(dt, p, v, a)) def total_time(self): return sum(n.dt for n in self.nodes) def s3_sign(x): if x >= 0: return 1.0 return -1.0 def s3_plan(start_state, target_p, target_v, v_max, a_max): path = Path() # Extract parameters and convert to float vi, pi = float(start_state.v), float(start_state.p) pf, vf = float(target_p), float(target_v) acc, v_cap = abs(float(a_max)), abs(float(v_max)) # Clamp the target velocity within the allowable maximum velocity limits vf = max(-v_cap, min(v_cap, vf)) # If acceleration is negligible, return a constant velocity path immediately if acc < S3_EPS: path.push(0.0, pi, vi, 0.0) return path dist = pf - pi # Determine the direction of motion (1 for positive, -1 for negative) s = s3_sign(dist) if abs(dist) > S3_EPS else 1.0 # ========================================== # Phase 1: Kinematic Violation Handling # ========================================== # Check 1: Wrong Direction or Overshoot # Detect if the current velocity is moving away from the target or if inertia makes stopping impossible is_wrong_dir = (vi * s < -S3_EPS) is_overshoot = False # Only check for overshoot if we are moving towards the target if not is_wrong_dir and (vi * s > S3_EPS): # Case A: Moving towards target, but target velocity requires moving backwards (must stop and reverse) if vf * s < -S3_EPS: is_overshoot = True # Case B: Current kinetic energy is too high to slow down to 'vf' within distance 'dist' # Formula check: v_initial^2 > v_final^2 + 2 * a * distance elif vi**2 > vf**2 + 2.0 * acc * abs(dist) + S3_EPS: is_overshoot = True if is_wrong_dir or is_overshoot: # Strategy: Perform a full stop, then plan recursively from the stopped state. # Calculate the distance required to come to a complete stop d_stop = (vi**2) / (2.0 * acc) * s3_sign(vi) stop_pos = pi + d_stop # Add the braking segment to zero velocity path.push(abs(vi)/acc, pi, vi, -s3_sign(vi)*acc) # Recursively plan from the stop position to the original target path.nodes.extend(s3_plan(MotionState(0, stop_pos, 0, 0), pf, vf, v_max, a_max).nodes) return path # Check 2: Insufficient Run-up Distance # If we need to accelerate to a high 'vf' but don't have enough distance to reach it if abs(vf) > abs(vi) and (vf * s > 0): max_reachable_sq = vi**2 + 2.0 * acc * abs(dist) if vf**2 > max_reachable_sq + S3_EPS: # Strategy: Move backward first to gain runway (Run-up). # Calculate the turnaround point needed to generate enough acceleration distance p_turn = pf - s * ((vf**2)/(2.0*acc)) # Plan segment to the turnaround point path.nodes.extend(s3_plan(MotionState(0, pi, vi, 0), p_turn, 0, v_max, a_max).nodes) # Plan segment from turnaround point to the target path.nodes.extend(s3_plan(MotionState(0, p_turn, 0, 0), pf, vf, v_max, a_max).nodes) return path # ========================================== # Phase 2: Standard Profile Generation # ========================================== # Calculate the theoretical peak velocity squared for a triangular profile # Derived from: 2 * a * d = (v_peak^2 - v_i^2) + (v_peak^2 - v_f^2) vp_sq = (2.0 * acc * abs(dist) + vi**2 + vf**2) / 2.0 vp = math.sqrt(max(0.0, vp_sq)) # Apply velocity constraints (Transition from Triangular to Trapezoidal profile) v_peak = min(vp, v_cap) * s # 1. Acceleration Phase: Move from current velocity to peak velocity t1 = abs(v_peak - vi) / acc path.push(t1, pi, vi, s3_sign(v_peak - vi) * acc) # Update current position after acceleration p_curr = pi + (vi + v_peak) * t1 * 0.5 # 2. Cruising Phase: Constant velocity (exists only if profile is Trapezoidal) # Calculate distance required for the final deceleration phase t3 = abs(vf - v_peak) / acc d3 = (v_peak + vf) * t3 * 0.5 # Calculate remaining distance available for cruising d_cruise = (pf - p_curr) - d3 # If there is meaningful distance left, insert a cruise segment if d_cruise * s > S3_EPS: path.push(abs(d_cruise)/abs(v_peak), p_curr, v_peak, 0.0) p_curr += d_cruise # 3. Deceleration/Adjustment Phase: Move from peak velocity to target velocity path.push(t3, p_curr, v_peak, s3_sign(vf - v_peak) * acc) return path def s3_plan_velocity(start_state, target_v, v_max, a_max): path = Path() vi = start_state.v pi = start_state.p acc = abs(a_max) v_cap = abs(v_max) vf = max(-v_cap, min(v_cap, target_v)) if acc < S3_EPS or abs(vf - vi) < S3_EPS: path.push(0.0, pi, vi, 0.0) return path duration = abs(vf - vi) / acc sign = 1.0 if vf > vi else -1.0 path.push(duration, pi, vi, sign * acc) return path def s3_at_time(path, t): if not path.nodes: return MotionState() if t < 0: t = 0 elapsed = 0.0 for node in path.nodes: if t < elapsed + node.dt: dt = t - elapsed res = MotionState() res.a = node.a res.v = node.v + node.a * dt res.p = node.p + node.v * dt + 0.5 * node.a * dt * dt return res elapsed += node.dt # Extrapolate last last = path.nodes[-1] dt_seg = last.dt v_end = last.v + last.a * dt_seg p_end = last.p + last.v * dt_seg + 0.5 * last.a * dt_seg * dt_seg dt_ex = t - elapsed res = MotionState() res.a = 0.0 res.v = v_end res.p = p_end + v_end * dt_ex return res # ========================================== # UI & Plotting # ========================================== class App: def __init__(self, root): self.root = root self.root.title("S3RTT C-Port Simulation (Overshoot Logic)") self.root.geometry("1200x800") # Controls Frame control_frame = ttk.LabelFrame(root, text="Parameters") control_frame.pack(fill="x", padx=10, pady=5) self.entries = {} params = [ ("Start Pos", "0.0"), ("Start Vel", "0.0"), ("Target Pos", "1000"), ("Target Vel", "0.0"), ("Max Vel", "1000.0"), ("Max Acc", "10000.0") ] for i, (label, val) in enumerate(params): ttk.Label(control_frame, text=label).pack(side="left", padx=5) e = ttk.Entry(control_frame, width=8) e.insert(0, val) e.pack(side="left", padx=5) self.entries[label] = e btn = ttk.Button(control_frame, text="Calculate & Plot", command=self.calculate) btn.pack(side="left", padx=20) # Info Label self.info_lbl = ttk.Label(root, text="Ready", foreground="blue") self.info_lbl.pack(pady=5) # Plot Area self.fig, self.axs = plt.subplots(3, 1, figsize=(10, 8), sharex=True) self.canvas = FigureCanvasTkAgg(self.fig, master=root) self.canvas.get_tk_widget().pack(fill="both", expand=True) def get_float(self, name): try: return float(self.entries[name].get()) except ValueError: return 0.0 def calculate(self): start_p = self.get_float("Start Pos") start_v = self.get_float("Start Vel") target_p = self.get_float("Target Pos") target_v = self.get_float("Target Vel") v_max = self.get_float("Max Vel") a_max = self.get_float("Max Acc") start_s = MotionState(0, start_p, start_v, 0) # Run Algorithm path = s3_plan(start_s, target_p, target_v, v_max, a_max) # Sampling total_t = path.total_time() sim_duration = total_t * 1.1 + 0.0001 times = [] pos = [] vel = [] acc = [] steps = 1000 dt = sim_duration / steps final_s = MotionState() for i in range(steps + 1): t = i * dt s = s3_at_time(path, t) times.append(t) pos.append(s.p) vel.append(s.v) acc.append(s.a) if i == steps: final_s = s # Update Info err = abs(final_s.p - target_p) self.info_lbl.config(text=f"Total Time: {total_t:.4f}s | Segments: {len(path.nodes)} | Final P: {final_s.p:.4f} (Err: {err:.4f}) | Final V: {final_s.v:.4f}") # Plotting for ax in self.axs: ax.clear() self.axs[0].plot(times, pos, label="Position", color="blue") self.axs[0].axhline(y=target_p, color="green", linestyle="--", label="Target P") self.axs[0].set_ylabel("Position") self.axs[0].grid(True) self.axs[0].legend() self.axs[1].plot(times, vel, label="Velocity", color="red") self.axs[1].axhline(y=target_v, color="orange", linestyle="--", label="Target V") self.axs[1].set_ylabel("Velocity") self.axs[1].grid(True) self.axs[2].plot(times, acc, label="Acceleration", color="purple") self.axs[2].set_ylabel("Acceleration") self.axs[2].set_xlabel("Time (s)") self.axs[2].grid(True) self.canvas.draw() if __name__ == "__main__": root = tk.Tk() app = App(root) root.mainloop()