Simulink 2 fully implemented #3

Quaternions.jl

@@ -1,15 +1,14 @@
-
-
-export Quaternion
-
 struct Quaternion
     i::Float64
     j::Float64
     k::Float64
     r::Float64
 
+    # TODO: Determine an acceptable tolerance. ≈ default is too strict.
     Quaternion(i, j, k, r) =
-        norm([i, j, k, r]) ≈ 1 ? new(i, j, k, r) : error("Magnitude not equal to 1.")
+        abs(norm([i, j, k, r]) - 1) < 0.1 ? new(i, j, k, r) :
+        error("Magnitude not equal to 1: " * string(norm([i, j, k, r])))
+
     Quaternion(q) = Quaternion(q[1], q[2], q[3], q[4])
     Quaternion() = new(0, 0, 0, 1)
 
@@ -52,3 +51,42 @@ Base.isapprox(a::Quaternion, b::Quaternion) = isapprox(collect(a), collect(b))
 
 LinearAlgebra.norm(q::Quaternion) = norm(collect(q))
 LinearAlgebra.normalize(q::Quaternion) = collect(q) / norm(q)
+
+# I dont think we need an actual type for this it might just be easier to keep it as a Vector or something.
+# struct EulerAngles
+#     roll::Float64
+#     pitch::Float64
+#     yaw::Float64
+
+#     EulerAngles(r, p, y) = new(r, p, y)
+#     EulerAngles() = new(0.0, 0.0, 0.0)
+# end
+
+
+function q_to_Euler(q::Quaternion)
+    # https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code_2
+
+    # roll (x-axis rotation)
+    sinr_cosp = 2 * (q.r * q.i + q.j * q.k)
+    cosr_cosp = 1 - 2 * (q.i * q.i + q.j * q.j)
+    roll = atan(sinr_cosp, cosr_cosp)
+
+    # pitch (y-axis rotation)
+    sinp = 2 * (q.r * q.j - q.k * q.i)
+    if (abs(sinp) >= 1)
+        pitch = copysign(π / 2, sinp) # use 90 degrees if out of range
+    else
+        pitch = asin(sinp)
+    end
+
+    # yaw (z-axis rotation)
+    siny_cosp = 2 * (q.r * q.k + q.i * q.j)
+    cosy_cosp = 1 - 2 * (q.j * q.j + q.k * q.k)
+    yaw = atan(siny_cosp, cosy_cosp)
+
+
+    # return EulerAngles(roll, pitch, yaw)
+    return (roll = rad2deg(roll), pitch = rad2deg(pitch), yaw = rad2deg(yaw))
+    # return (roll = roll, pitch = pitch, yaw = yaw)
+end
+

README.md

@@ -1,2 +1,3 @@
 # SADC.Jl
 
+![](DancingT.png)

SADC.jl

@@ -1,19 +1,24 @@
 using LinearAlgebra
+using NumericalIntegration
+
+
 
 include("Quaternions.jl")
 
+dt = 0.005
+time = 0.0:dt:100
+
 I = [3 0 0; 0 4 0; 0 0 2]
 T = [0; 0; 0]
-dt = 0.005
-ω₀ = [2.001; 0.001; 0.001]
-q₀ = [0; 0; 0; 1]
+ω = [2.001; 0.001; 0.001]
 
+q = Quaternion([0; 0; 0; 1])
+
+ω_last = [0; 0; 0; ω]
+ω_temp = similar(ω_last)
 
 integrate_ω(T, I, ω) = inv(I) * (T - cross(ω, I * ω))
 
-
-integrate_ω(T, I, ω₀)
-
 function q_step(β)
     mag = β .^ 2 |> sum |> sqrt
 
@@ -21,3 +26,60 @@ function q_step(β)
 
 end
 
+function integrate_vector(v, v_last)
+    v_new = similar(v)
+    for i = 1:length(v)
+        v_new[i] = integrate([0, dt], [v_last[i], v[i]])
+    end
+    return v_new
+end
+
+yaw = []
+pitch = []
+roll = []
+for t in time
+
+
+    ω′ = integrate_ω(T, I, ω)
+    ω_new = integrate_vector([ω; ω′], ω_last)
+    ω = ω_new[4:6]
+
+    β = ω_new[1:3] .- ω
+
+    q = q * q_step(β)
+
+    y, p, r = q_to_Euler(q)
+    push!(yaw, y)
+    push!(pitch, p)
+    push!(roll, r)
+
+end
+
+
+
+# Plot 
+# using Plots
+# let
+#     plot(time, yaw, label = "Yaw")
+#     plot!(time, pitch, label = "Pitch")
+#     plot!(time, roll, label = "Roll")
+#     title!("Dancing T Handle")
+#     xlabel!("Seconds")
+#     ylabel!("Degrees")
+#     savefig("DancingT.png")
+# end
+
+# Write to CSV
+# using CSV
+# using DataFrames
+
+# ts = 100
+# df = DataFrame(
+#     time = time[1:ts:end],
+#     yaw = yaw[1:ts:end],
+#     roll = roll[1:ts:end],
+#     pitch = pitch[1:ts:end],
+# )
+
+# CSV.write("ypr.csv", df, header = false)
+# df