Hi,
yeah i didnt really understand the “MWE”…
Down below i added the now (hopefully) working MWE.
Yes i tried to do so…
In the example below the equations are similar to my real equations, just a lot shorter.
This code below works like my original one, and the dDG2/4/6 do not get updated:
using DataFrames
using Interpolations
using Plots
function lin_interpolate(df::DataFrame, col1::Symbol, col2::Symbol, col1_value::Float64)
# extract the columns in which you want to interpolate
x = collect(df[!, col1])
y = collect(df[!, col2])
# Check if the col1_value is within the range of the dataframe
if col1_value < minimum(x) || col1_value > maximum(x)
return "error"
end
# linear interpolate (Module)
itp = linear_interpolation(x, y)
# calculate the interpolation for var1_value
col2_value = itp(col1_value)
# return the interpolated value
return col2_value
end
## BODIES - GEOMETRICAL VALUES
# Initialize the geometrical values
# First Body
l1 = 20.0
l2 = 20.0
m1 = 20.0
φ0 = deg2rad(20.0)
ψ0 = deg2rad(0.0)
# Initial length of the springs
x10 = 0.0
l0Fbt = sqrt((l1+x10*sin(φ0))^2+(l2+x10*cos(φ0)))
J10 = φ0
J20 = deg2rad(90.0) + φ0 - ψ0
#
## FORCES - BELT/AIRBAG
# Initialise lookup tables for the forces
Fbt_v = DataFrame(Distance = Float64[], Force = Float64[])
push!(Fbt_v, (-100.0, 0.0))
push!(Fbt_v, (0.0, 0.0))
push!(Fbt_v, (100.0, 0.0))
push!(Fbt_v, (200.0, 3500.0))
push!(Fbt_v, (1000.0, 3500.0))
#
## TORQUES - JOINTS
# Initialise lookup tables for the torques
Mg1_v = DataFrame(Angle = Float64[], Torque = Float64[])
push!(Mg1_v, (deg2rad(-180.0), 20.0))
push!(Mg1_v, (deg2rad(180.0), 20.0))
#
## SIMULATION VARIABLES
# save the input Acceleration in a DataFrame
acc = DataFrame(Time = Float64[], Acc = Float64[])
push!(acc, (0.0, 0.0))
push!(acc, (0.05, 200.0))
Δt = 2.5e-5
T = round.(last(acc.Time), digits=5) # Length of Simulation
n = T / Δt # Number of Steps the Simulation takes -> calculated from input acc data
t0 = round.(acc[1,1], digits=5) # Starting time
results = DataFrame(Steps=Int64[], Time = Float64[], Acc= Float64[], DG1 = Float64[], DG2 = Float64[], DG3 = Float64[], DG4 = Float64[], DG5 = Float64[], DG6 = Float64[], dDG2 = Float64[], dDG4 = Float64[], dDG6 = Float64[], dFbt = Float64[], Fbt = Float64[], dJ1 = Float64[], Mg1 = Float64[]);
#
## EQUATIONS
## Initialize the equations
eq_motion1 = "dDG2=2*l1*m1*cos(DG3)*cos(DG5)*DG1+DG4*DG6+Mg1, dDG4=l1*m1*cos(DG3)*tan(DG5)*DG4-l1*m1*cos(DG3)*cos(DG5)*DG6+Mg1, dDG6=2*l1*m1*cos(DG3)*sin(DG5)*DG4+DG2/DG6+Mg1"
eq_motion = split(eq_motion1, ",")
# Equations string
f_dDG2 = eq_motion[1]
f_dDG4 = eq_motion[2]
f_dDG6 = eq_motion[3]
parsed_dDG2 = Meta.parse(f_dDG2)
parsed_dDG4 = Meta.parse(f_dDG4)
parsed_dDG6 = Meta.parse(f_dDG6)
# Equations
c_dDG2 = eval(parsed_dDG2)
c_dDG4 = eval(parsed_dDG4)
c_dDG6 = eval(parsed_dDG6)
#
## SIMULATION LOOP
for step in 1:n
println(step)
# Calculate the current time
t = t0 + step * Δt
# Access previous values from results DataFrame if we are not at the first step
if step > 1
DG1 = results.DG1[end]
DG2 = results.DG2[end]
DG3 = results.DG3[end]
DG4 = results.DG4[end]
DG5 = results.DG5[end]
DG6 = results.DG6[end]
x1_p = results.DG1[end] # x1
dx1_p = results.DG2[end] # x1'
φ_p = results.DG3[end] # φ
dφ_p = results.DG4[end] # φ'
ψ_p = results.DG5[end] # ψ
dψ_p = results.DG6[end] # ψ'
else
# Use initial conditions for the first step
DG1 = 0.0;
DG2 = 56.0*1000/3.6;
dDG2 = 0.0
DG3 = deg2rad(20.0);
DG4 = 0.0;
dDG4 = 0.0;
DG5 = 0.0;
DG6 = 0.0;
dDG6 = 0.0;
x1_p = DG1; # x1 at t=0
dx1_p = DG2; # x1' at t=0
φ_p = DG3; # φ at t=0
dφ_p = DG4; # φ' at t=0
ψ_p = DG5; # ψ at t=0
dψ_p = DG6; # ψ' at t=0
end
# Calculate the lenghts of the springs/angles
lFbt = sqrt((l1+x1_p*sin(φ_p))^2+(l2+x1_p*cos(φ_p)))
J1 = φ_p
# Calculate the length difference of the springs to t0
dlFbt = lFbt - l0Fbt
dJ1 = J1 - J10
# Calculate the t-1 forces/Torques
Fbt = lin_interpolate(Fbt_v, :Distance, :Force, dlFbt)
Mg1 = lin_interpolate(Mg1_v, :Angle, :Torque, dJ1)
# check the boundaries of Input acc
if t < last(acc.Time)
ax4 = lin_interpolate(acc, :Time, :Acc, t)
#println(acc_a)
else
println("Simulation finished -> no acc data for that timestep")
break
end
#
# calculate the Forces
# Euler method: Calculate new states
# Calculate accelerations
dDG2 = c_dDG2
dDG4 = c_dDG4
dDG6 = c_dDG6
# Update velocities and positions (Euler method)
DG2 = DG2 + Δt * dDG2; # Velocity of x1
DG1 = DG1 + Δt * DG2; # Position of x1
DG4 = DG4 + Δt * dDG4; # Velocity of φ
DG3 = DG3 + Δt * DG4; # Position of φ
DG6 = DG6 + Δt * dDG6; # Velocity of ψ
DG5 = DG5 + Δt * DG6; # Position of ψ
# Store current values in the DataFrame
push!(results, (step, t, ax4, DG1, DG2, DG3, DG4, DG5, DG6, dDG2, dDG4, dDG6, dlFbt, Fbt, dJ1, Mg1));
end
rename!(results,
:DG1 => :x1,
:DG2 => :vx1,
:DG3 => :Phi,
:DG4 => :vPhi,
:DG5 => :Psi,
:DG6 => :vPsi
)
## PLOTS
plot(results.Time, results.Acc, label="acc", xlabel="Time (s)", ylabel="", legend=:topright)
plot!(results.Time, results.x1, label="x1", linestyle=:dash)
plot!(results.Time, results.vx1, label="vx1", linestyle=:dash)
plot!(results.Time, results.Phi, label="Phi", linestyle=:dash)
plot!(results.Time, results.vPhi, label="vPhi", linestyle=:dash)
plot!(results.Time, results.Psi, label="Psi", linestyle=:dash)
plot!(results.Time, results.vPsi, label="vPsi", linestyle=:dash)