diff --git a/flapopt/OptParams.jl b/flapopt/OptParams.jl
index 323cca962..ddcb25629 100644
--- a/flapopt/OptParams.jl
+++ b/flapopt/OptParams.jl
@@ -47,11 +47,11 @@ end
 
 "Some optimization numerical error is causing spiky oscillations in Δy
 https://github.com/avikde/robobee3d/pull/127#issuecomment-580050283"
-function smoothΔy(Δy, ny, N, dt; ord=2, cutoff_freq=0.5)
+function smoothTraj(traj, ny, N, dt; ord=2, cutoff_freq=0.5)
 	# For each coord, fit a spline
 	# tvec = collect(0:(N))*dt
 	myfilter = digitalfilter(Lowpass(cutoff_freq; fs=1/dt), Butterworth(ord))
-	smoothCoord(i) = filtfilt(myfilter, Δy[i:ny:(N+1)*ny])
+	smoothCoord(i) = filtfilt(myfilter, traj[i:ny:(N+1)*ny])
 	trajNny = hcat([smoothCoord(i) for i=1:ny]...)
 	return trajNny'[:]
 end
@@ -63,7 +63,7 @@ function reconstructTrajFromΔy(m::Model, opt::OptOptions, POPTS, traj::Abstract
 	np = length(pnew)
 	dtold = paramold[end] # dt is the last param
 	dtnew = pnew[end]
-	Δy = smoothΔy(Δy, ny, N, dtnew)
+	Δy = smoothTraj(Δy, ny, N, dtnew)
 	Δyk = k -> Δy[(k-1)*ny+1 : k*ny]
 
 	# Also convert the output traj with the Δy, new T, and inputs
diff --git a/flapopt/Wing2DOF.jl b/flapopt/Wing2DOF.jl
index 7040cef98..c3f90d404 100644
--- a/flapopt/Wing2DOF.jl
+++ b/flapopt/Wing2DOF.jl
@@ -45,7 +45,7 @@ NOTE:
     r2h::Float64 = 0.551 # r2hat second moment -- see [Whitney (2010)]. Default from 0.929 * 0.49^0.732
     SEA::Bool = false # series-elastic https://github.com/avikde/robobee3d/pull/126
     kSEA::Float64 = 1000 # SEA spring const
-    Amp::AbstractArray = [0.0,0.0] # Stroke, hinge amplitude for the desired output kinematics (set to 0 to not use)
+    Amp::AbstractArray = [0.0,0.0,0.0] # Stroke1, hinge, stroke2 amplitude for the desired output kinematics (set to 0 to not use)
 end
 
 # Fixed params -----------------
diff --git a/flapopt/run_w2d.jl b/flapopt/run_w2d.jl
index 866ed0e2a..977a60556 100644
--- a/flapopt/run_w2d.jl
+++ b/flapopt/run_w2d.jl
@@ -19,7 +19,7 @@ m = Wing2DOFModel(
 	ko = 30.0,
 	ma = 6,
 	ka = 240,
-	Amp = deg2rad.([120, 140]))
+	Amp = deg2rad.([120, 140, 0]))
 ny, nu = cu.dims(m)
 
 function getInitialParams()
@@ -253,7 +253,7 @@ end
 ret1 = KINTYPE==1 ? Dict("traj"=>traj0, "param"=>param0) : opt1(m, traj0, param0, 2, 0.1, 0.0) # In ID force tau2=0
 
 # 2. Try to optimize
-ret2 = opt1(m, ret1["traj"], ret1["param"], 1, 3.0; Φ=120)#; print_level=3, max_iter=10000)
+ret2 = opt1(m, ret1["traj"], ret1["param"], 1, 1.9)#; Φ=[90,90])#; print_level=3, max_iter=10000)
 
 # testManyShifts(ret1, [0], 0.6)
 
diff --git a/flapopt/w2d_paramopt.jl b/flapopt/w2d_paramopt.jl
index 9cf8ff6a4..4dd8f7735 100644
--- a/flapopt/w2d_paramopt.jl
+++ b/flapopt/w2d_paramopt.jl
@@ -44,10 +44,13 @@ end
 kinType -- 0 => ID'ed real data, 1 => openloop sim with param0 then truncate, 2 => generate kinematics(t)
 fix -- Make traj satisfy dyn constraint with these params?
 """
-function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, uampl=65, starti=214, verbose=true, freq=0.165, N=80)
+function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, uampl=65, starti=1115, verbose=true, freq=0.165, N=80)
+	Ncyc = (m.Amp[3]!=0.0 ? 2 : 1)
+	N *= Ncyc
 	if kinType==1
-		opt = cu.OptOptions(false, false, 1/(N*freq), 1, :none, 1e-8, false) # sim
-		N = opt.boundaryConstraint == :symmetric ? N÷2 : N
+		initialdt = 1/(N*freq)
+		opt = cu.OptOptions(false, false, initialdt, 1, :none, 1e-8, false) # sim
+		N = (opt.boundaryConstraint == :symmetric ? N÷2 : N)*Ncyc
 		trajt, traj0 = createInitialTraj(m, opt, N, freq, [1e3, 1e2], param0, starti; uampl=uampl, verbose=verbose)
 	elseif kinType==0
 		# Load data
@@ -60,15 +63,29 @@ function initTraj(m, param0, kinType=0; fix=false, makeplot=false, Ψshift=0, ua
 		error("Not implemented")
 	end
 
-	@views tcoord(i) = traj0[i:ny:(N+1)*ny]
+	@views tcoord(i, Nn=N) = traj0[i:ny:(Nn+1)*ny]
 	currentAmpl(i) = maximum(tcoord(i)) - minimum(tcoord(i))
 	for i=1:2
 		if m.Amp[i] != 0.0 # Set stroke/hinge amplitude https://github.com/avikde/robobee3d/pull/127
-			tcoord(i) .*= m.Amp[i]/currentAmpl(i) # scale pos
-			tcoord(i+2) .*= m.Amp[i]/currentAmpl(i) # scale vel
+			iampl = currentAmpl(i)
+			if i == 1 && Ncyc > 1
+				# https://github.com/avikde/robobee3d/pull/129 scale all by the second ampl then the first half
+				tcoord(i) .*= m.Amp[3]/iampl
+				tcoord(i+2) .*= m.Amp[3]/iampl
+				tcoord(i, N÷2) .*= m.Amp[1]/m.Amp[3]
+				tcoord(i+2, N÷2) .*= m.Amp[1]/m.Amp[3]
+			else
+				tcoord(i) .*= m.Amp[i]/iampl # scale pos
+				tcoord(i+2) .*= m.Amp[i]/iampl # scale vel
+			end
 		end
 	end
 
+	if Ncyc > 1 && m.Amp[1] != m.Amp[3]
+		# need to smooth the traj
+		traj0[1:(N+1)*ny] = cu.smoothTraj(traj0, ny, N, initialdt; ord=2, cutoff_freq=1.5)
+	end
+
 	if fix
 		traj0 = cu.fixTrajWithDynConst(m, opt, traj0, param0)
 	end
@@ -147,7 +164,7 @@ end
 """One-off ID or opt"""
 function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, testAfter=false, testReconstruction=false, max_iter=4000, print_level=1, wARconstraintLinCbar=3.2, Φ=nothing)
 	# A polytope constraint for the params: cbar >= cbarmin => -cbar <= -cbarmin. Second, τ2 <= 2*τ1 => -2*τ1 + τ2 <= 0
-	print(mode==2 ? "ID" : "Opt", " Φ=", isnothing(Φ) ? "-" : Φ, ", minal=", minal, ", τ2/1 lim=", τ21ratiolim, " => ")
+	print(mode==2 ? "ID" : "Opt", " Φ=", isnothing(Φ) ? "-" : round.(Φ, digits=1), ", minal=", minal, ", τ2/1 lim=", τ21ratiolim, " => ")
 
     # cbar, τ1, mwing, wΨ, τ2, Aw, dt  = param
 	# Poly constraint
@@ -163,11 +180,17 @@ function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, t
 	dp = [mlb; 0; 0; 0; wARb]
 
 	if !isnothing(Φ)
-		m.Amp[1] = deg2rad(Φ)
+		if length(Φ) == 1
+			m.Amp[1] = deg2rad(Φ)
+			m.Amp[3] = 0.0
+		else
+			m.Amp[1] = deg2rad(Φ[1])
+			m.Amp[3] = deg2rad(Φ[2])
+		end
 		# get new input traj
 		traj = initTraj(m, param, KINTYPE; uampl=75, verbose=false)[3]
 	end
-
+	Ncyc = (m.Amp[3]!=0.0 ? 2 : 1)
 	ret = cu.optAffine(m, opt, traj, param, POPTS, mode, σamax; test=testAffine, Cp=Cp, dp=dp, print_level=print_level, max_iter=max_iter, testTrajReconstruction=testReconstruction)
 	# append unactErr
 	ret["unactErr"] = ret["eval_g"](ret["x"])[1:N] # 1 unact DOF
@@ -184,7 +207,7 @@ function opt1(m, traj, param, mode, minal, τ21ratiolim=2.0; testAffine=false, t
 	ret["FD∞"] = norm(ret["comps"][6][1,:], Inf) # Also store drag (should be same as uinf for scaling but dynamics)
 	
 	println(ret["status"], ", ", round.(ret["param"]', digits=3), 
-	", fHz=", round(1000/(N*ret["param"][end]), digits=1), 
+	", fHz=", round(1000/(Ncyc*N*ret["param"][end]), digits=1), 
 	", al[mg]=", round(ret["al"] * 1000/9.81, digits=1), 
 	", u∞=", round(ret["u∞"], digits=1), 
 	", pow=", round(mean(abs.(ret["mechPow"])), digits=1),