52 lines
1.3 KiB
Python
52 lines
1.3 KiB
Python
import numpy as np
|
|
from scipy.optimize import fsolve
|
|
|
|
# Crankshaft geometry
|
|
Lc = 7.0 # mm
|
|
La = 20.0 # mm
|
|
|
|
|
|
# Functions
|
|
def crank_position(theta : float):
|
|
crank_contrib = Lc * np.cos(theta)
|
|
arm_contrib = np.sqrt(La**2 - (Lc * np.sin(theta)) ** 2)
|
|
return (Lc + La) - crank_contrib - arm_contrib
|
|
|
|
|
|
def crank_velocity(theta : float):
|
|
sin_theta = np.sin(theta)
|
|
cos_theta = np.cos(theta)
|
|
denominator = np.sqrt(La**2 - (Lc * sin_theta) ** 2)
|
|
return Lc * sin_theta - (Lc**2 * sin_theta * cos_theta) / denominator
|
|
|
|
|
|
def dneedle_dtheta(theta : float):
|
|
# Derivative (Jacobian H)
|
|
sin_t = np.sin(theta)
|
|
cos_t = np.cos(theta)
|
|
sqrt_term = np.sqrt(La**2 - (Lc * sin_t) ** 2)
|
|
dcrank = -Lc * sin_t
|
|
darm = (Lc**2 * sin_t * cos_t) / sqrt_term
|
|
return -dcrank - darm
|
|
|
|
|
|
|
|
def find_theta(x, theta0=0.0):
|
|
"""
|
|
Find theta for a given position x using numerical root-finding.
|
|
|
|
Parameters:
|
|
x (float): Desired position.
|
|
Lc (float): Length of crank.
|
|
La (float): Length of arm.
|
|
theta0 (float): Initial guess for theta (default: 0.0).
|
|
|
|
Returns:
|
|
float: Theta that satisfies the equation.
|
|
"""
|
|
def equation(theta):
|
|
return crank_position(theta) - x
|
|
|
|
theta_solution = fsolve(equation, theta0)[0]
|
|
return theta_solution
|