' BASIC-style pseudocode for a notch filter
' Initialize parameters
center_freq = 50         ' Center frequency in Hz (f_0)
sampling_rate = 1000      ' Sampling rate in Hz
bandwidth = 5             ' Bandwidth in Hz
attenuation = -20         ' Attenuation in dB

' Derived constants
omega_0 = 2 * PI * center_freq / sampling_rate    ' Digital radian frequency
bw = bandwidth / sampling_rate                    ' Bandwidth in terms of the sampling rate
d = -attenuation / 20                             ' Attenuation factor
alpha = sin(omega_0) / (2 * bw)                   ' Filter sharpness

' Filter coefficients
b0 =  1
b1 = -2 * cos(omega_0)
b2 =  1
a0 =  1 + alpha
a1 = -2 * cos(omega_0)
a2 =  1 - alpha

' Normalize coefficients
b0 = b0 / a0
b1 = b1 / a0
b2 = b2 / a0
a1 = a1 / a0
a2 = a2 / a0

' Variables for storing past input/output values
x_1 = 0
x_2 = 0
y_1 = 0
y_2 = 0

'Main loop (input is the signal you want to filter)
DO WHILE (true)

   'Assume input_signal is the current sample of the input signal
   input_signal = GET_INPUT()  ' Get input signal value
   
   'Apply the notch filter difference equation
   output_signal = b0 * input_signal + b1 * x_1 + b2 * x_2 - a1 * y_1 - a2 * y_2
       
   'Update past values
   x_2 = x_1
   x_1 = input_signal
   y_2 = y_1
   y_1 = output_signal
   
   print input_signal
   print output_signal
   'Output the filtered signal
   SEND_OUTPUT(output_signal)
   
LOOP

Option explicit
'
' Array positions for PID parameters and working variables
'
Const Kp=0
Const Ki=1
Const Kd=2
Const tau=3
Const limMin=4
Const limMax=5
Const limMinInt=6
Const limMaxInt=7
Const T=8
Const integrator=9
Const prevError=10
Const differentiator=11
Const prevMeasurement=12
Const out=13
'
Dim pid_params(13)
'
pid_params(Kp)=2.0
pid_params(Ki)=0.5
pid_params(Kd)=0.25

' Derivative low-pass filter time constant
pid_params(tau)=0.02

'Output limits
pid_params(limMin)=-10.0
pid_params(limMax)=10.0

'Integrator limits
pid_params(limMinInt)=-5.0
pid_params(limMaxInt)=5.0

'Sample time (in seconds)
pid_params(T)=0.01

'Controller "memory"
pid_params(integrator)=0.0
pid_params(prevError)=0.0 'Required for integrator'
pid_params(differentiator)=0.0
pid_params(prevMeasurement)=0.0 'Required for differentiator

'Controller output
pid_params(out)=0.0


Dim float setpoint=1.0
Dim integer near_enough=0
Timer =0
'
' Initialise a PID loop with a 10mSec loop time
'
Math PID INIT 1,pid_params(),pidint
'
' Start the PID controller
'
Math pid start 1
Do
Loop Until near_enough
Print "test over"
Math pid stop 1
Math pid close 1
End

'
'Subroutine called every 10mSec
'
Sub pidint
 Local float measurement=test(pid_params(out)) 'simulate an input measurement
 Local float output=Math(PID 1,setpoint,measurement) 'update the controller
 Print Int(Timer),Str$(measurement,8,5),Str$(output,8,5)
 If Abs(measurement-setpoint)<0.00001 Then near_enough=1
End Sub

Function test(inp As float) As float
 Static float output=0.0
 Const alpha=0.02
 output=(pid_params(T)*inp + output)/(1.0 + alpha * pid_params(T))
 test=output
End Function