Unverified Commit 64926394 authored by David Verelst's avatar David Verelst Committed by GitHub

Merge pull request #6 from DTUWindEnergy/check_iteration

Check iteration
parents eddfec36 239d2eb1
......@@ -35,7 +35,8 @@ module misc_mod
! Time delay
type Ttdelay
real(mk) xz(40)
integer::stepno1=0
real(mk) xz_old(40)
integer :: stepno1 = 0
end type Ttdelay
contains
!**************************************************************************************************
......@@ -105,9 +106,9 @@ function lowpass2orderfilt(dt, stepno, filt, x)
y = a1*filt%y1_old + a2*filt%y2_old + b0*x + b1*filt%x1_old + b2*filt%x2_old
endif
! Save previous values
filt%x2 = filt%x1
filt%x2 = filt%x1_old
filt%x1 = x
filt%y2 = filt%y1
filt%y2 = filt%y1_old
filt%y1 = y
filt%stepno1=stepno
! Output
......@@ -153,9 +154,9 @@ function notch2orderfilt(dt,stepno,filt,x)
y = a1*filt%y1_old + a2*filt%y2_old + b0*x + b1*filt%x1_old + b2*filt%x2_old
endif
! Save previous values
filt%x2 = filt%x1
filt%x2 = filt%x1_old
filt%x1 = x
filt%y2 = filt%y1
filt%y2 = filt%y1_old
filt%y1 = y
filt%stepno1 = stepno
! Output
......@@ -200,9 +201,9 @@ function bandpassfilt(dt, stepno, filt, x)
y = a1*filt%y1_old + a2*filt%y2_old + b0*x + b1*filt%x1_old + b2*filt%x2_old
endif
! Save previous values
filt%x2 = filt%x1
filt%x2 = filt%x1_old
filt%x1 = x
filt%y2 = filt%y1
filt%y2 = filt%y1_old
filt%y1 = y
filt%stepno1 = stepno
! Output
......@@ -222,8 +223,11 @@ function timedelay(dt, stepno, filt, Td, x)
filt%xz(k) = x
end do
endif
if (stepno .gt. filt%stepno1) then
filt%xz_old = filt%xz
endif
do k = 40, 2, -1
filt%xz(k) = filt%xz(k - 1)
filt%xz(k) = filt%xz_old(k - 1)
end do
filt%xz(1) = x
! Output
......@@ -232,6 +236,7 @@ function timedelay(dt, stepno, filt, Td, x)
else
timedelay = filt%xz(n)
endif
filt%stepno1 = stepno
return
end function timedelay
!**************************************************************************************************
......
......@@ -81,7 +81,7 @@
! integer*4 i,j,ido
integer*4 i,j
!real*8 tol,param(50),y(2),t,tend,timestep
real*8 tol,y(2),t,tend,timestep
real*8 tol,y(2),t,tend
!parameter(tol=1.d-5)
real*8 theta
real*8 work(15),relerr,abserr
......@@ -92,54 +92,54 @@
relerr=1.d-6
iflag=1
! Check if the time has changed
timestep=array1(1)-oldtime
if (timestep.gt.0.d0) then
stepno=stepno+1
! Handle inputs from other DLLs
if (stepno.eq.1) array1(5) = 0.d0
if (array1(1)-oldtime.gt.0.d0) then
timestep=array1(1)-oldtime
oldtime=array1(1)
yold=ynew
! Loop for all blades
do i=1,nblades
! Initial conditions for pitch angle and velocity
t=0.d0
y(1:2)=yold(1:2,i)
! Actual and reference position and velocity of pitch angle
theta_ref=array1(i+1)
! Compute pitch angle and velocity at next step
tend=timestep
if ((array1(1).gt.time_stuck).and.(time_stuck.gt.0.d0).and.(array1(5).lt.1.d0).and.(i.eq.1)) then
if (stuck_type.eq.1) then
stuck_angle=yold(1,1)
endif
y(1)=stuck_angle
y(2)=0.d0
elseif ((array1(1).gt.time_runaway).and.(time_runaway.gt.0.d0).and.(array1(5).lt.1.d0)) then
y(1)=y(1)+y(2)*timestep
y(2)=max(-vmax,y(2)-amax)
elseif (array1(5).gt.0.d0) then
y(1)=y(1)+y(2)*timestep
y(2)=min(vmax,y(2)+amax)
else
call rkf45(ode,2,y,t,tend,relerr,abserr,iflag,work,iwork)
endif
! Apply hard limits on angles
if (y(1).lt.theta_min) then
y(1)=theta_min
y(2)=0.d0
endif
if (y(1).gt.theta_max) then
y(1)=theta_max
y(2)=0.d0
endif
! Save results
ynew(1:2,i)=y(1:2)
! Fill output array2
oldarray2(i)=y(1)
oldarray2(nblades+i)=y(2)
oldarray2(2*nblades+i)=(y(2)-yold(2,i))/timestep
enddo
stepno=stepno+1
endif
! Handle inputs from other DLLs
if (stepno.eq.1) array1(5) = 0.d0
! Loop for all blades
do i=1,nblades
! Initial conditions for pitch angle and velocity
t=0.d0
y(1:2)=yold(1:2,i)
! Actual and reference position and velocity of pitch angle
theta_ref=array1(i+1)
! Compute pitch angle and velocity at next step
tend=timestep
if ((array1(1).gt.time_stuck).and.(time_stuck.gt.0.d0).and.(array1(5).lt.1.d0).and.(i.eq.1)) then
if (stuck_type.eq.1) then
stuck_angle=yold(1,1)
endif
y(1)=stuck_angle
y(2)=0.d0
elseif ((array1(1).gt.time_runaway).and.(time_runaway.gt.0.d0).and.(array1(5).lt.1.d0)) then
y(1)=y(1)+y(2)*timestep
y(2)=max(-vmax,y(2)-amax)
elseif (array1(5).gt.0.d0) then
y(1)=y(1)+y(2)*timestep
y(2)=min(vmax,y(2)+amax)
else
call rkf45(ode,2,y,t,tend,relerr,abserr,iflag,work,iwork)
endif
! Apply hard limits on angles
if (y(1).lt.theta_min) then
y(1)=theta_min
y(2)=0.d0
endif
if (y(1).gt.theta_max) then
y(1)=theta_max
y(2)=0.d0
endif
! Save results
ynew(1:2,i)=y(1:2)
! Fill output array2
oldarray2(i)=y(1)
oldarray2(nblades+i)=y(2)
oldarray2(2*nblades+i)=(y(2)-yold(2,i))/timestep
enddo
! Insert output
array2(1:3*nblades)=oldarray2(1:3*nblades)
return
......
......@@ -4,4 +4,5 @@ module servo_with_limits_data
real(mk) omega0,beta0,vmax,amax,theta_min,theta_max,time_runaway,time_stuck,stuck_angle
real(mk) oldtime,theta_ref,yold(2,3),ynew(2,3)
real(mk) oldarray2(100)
real(mk) timestep
end module servo_with_limits_data
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment