Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
Sinusoidal Disturbance Injection
I received a question on how to inject a sinusoidal disturbance before / after PID in PMAC for some system identification purpose, so I wrote the following code and I'm posting it since it might be useful for others too.

del gat

#define servoperiod               P5
#define RTIperiod                 P6

#define SinFreq                   P10
#define MaxCycle                  P11
#define Counter                   P12
#define SinPeriod                 P13
#define Magnitude                 P14
#define OutputReg                 M10

// Injection before Servo ---------------------------------
//OutputReg->X:$10F0,0,24,S      ; for use as a master position (injection before servo)
// Injection before Servo ---------------------------------

// Injection after servo ----------------------------------
OutputReg->Y:$BF,0,24,S         ; for torque injection after servo
// Injection after servo ----------------------------------

SinFreq = 10                     ;Hz

SinPeriod = 1000/SinFreq         ;msec

Magnitude = 4000

OutputReg = Magnitude * SIN(((Counter*RTIperiod)%SinPeriod)/SinPeriod*360)

Enable PLC 0

For injection before the servo, you have to point Ix05 of the desired motor to $10F0 (open register where the PLC writes the data) and set Ix06=3 to set the master following in offset mode. The magnitude units would be 1/32 of a count.

For injection after the servo, change the address of the OutputReg Parameter based upon the following table:

Motor #   Address
--------- ----------
1         Y:$BF
2         Y:$13F
3         Y:$1BF
4         Y:$23F
5         Y:$2BF
6         Y:$33F
7         Y:$3BF
8         Y:$43F
9         Y:$4BF
10        Y:$53F
11        Y:$5BF
12        Y:$63F
13        Y:$6BF
14        Y:$73F
15        Y:$7BF
16        Y:$83F
17        Y:$8BF
18        Y:$93F
19        Y:$9BF
20        Y:$A3F
21        Y:$ABF
22        Y:$B3F
23        Y:$BBF
24        Y:$C3F
25        Y:$CBF
26        Y:$D3F
27        Y:$DBF
28        Y:$E3F
29        Y:$EBF
30        Y:$F3F
31        Y:$FBF
32        Y:$103F

The magnitude units would be 1/256 of Ixx69. (a magnitude of 256 represents (10 / 32767) = 0.000305V DAC output)

Here is what I have if I do injection before servo:


And here is what I get if I do injection after servo:

Sina Sattari
Hardware Engineering Manager
Delta Tau Data Systems, Inc.
Here's another example (a little different from Sina's) for your reference:

I8 = 0  ; RTI = Servo Period
I5 = 3  ; Enable foreground and background PLCs
I15 = 0 ; Trig calcs in degrees

M100->X:$000000,0,24,S ; 24-bit servo cycle counter

#define ServoCounter M100

#define Amplitude                        P1000 ; 18-bit DAC bits - User Input
#define FreqHz                            P1001 ; Desired Frequency in Hz - User Input
#define SinOutputPCalc            P1002 ; Stores output value before outputting to DAC
#define OldI129                            P1003 ; Ixx29 value from DAC calibration - User Input
#define ServoCyclesPermsec    P1004 ; Servo Cycles/millisecond conversion factor
#define Offset                            P1005 ; Additional offset if needed (18-bit DAC bits) - User Input
#define Phase                                P1006 ; Additional phase offset if needed (degrees) - User Input
#define SinOutputDACBias        I129  ; DAC bias variable (this PLCC adjusts this value)
#define msecpersec                    1000  ; milliseconds per second conversion factor

OldI129 = -1373 ; Put your old Ixx29 value here that you obtained from DAC calibration before running the PLC

Amplitude = 3000 ; 18-bit DAC bits
FreqHz = 1             ; Hz
Phase = 0                 ; Phase offset (degrees)
ServoCyclesPermsec = 8388608/I10 ; servo cycles/ms

Open PLCC0 Clear
SinOutputPCalc = Amplitude*Sin(360*FreqHz*ServoCounter/ServoCyclesPermsec/msecpersec+Phase) + Offset
SinOutputDACBias = SinOutputPCalc + OldI129
Sina / Charles,

Thanks for your examples. They gave me a good head-start to get going with my Swept-Sine analysis.

For the method of injecting a position signal into the servo-loop I ran into a limitation: by using the master position as injection point, the injected signal does not 'go through' the differentiator. The result is that the Kd (I131) can not be measured/estimated.

Do you have a similar suggestion to inject a position signal that will go through the differentiator? Preferably a method that does not force me to 'open and close' the servo loop (like reconfiguring the ECT). I'd like to make this method an 'add-on' to an already existing and running servo-loop.

Thank you.


If you write to commanded position instead of master position then it will be as if you are generating the trajectory. You should use caution writing commands into desired position though. The Mx61 of suggested M-variables points to commanded position for motor x.

Sina Sattari
Hardware Engineering Manager
Delta Tau Data Systems, Inc.

Thanks. Actually, I found that both the Master Position and the Commanded Position will 'go through' the PID Velocity Feedforward Gain (Ixx32). Setting this parameter to match the PID Derivative Gain (Ixx31) should result in a measurement as if one was measuring a PID controller that has the servo-error as input. I have not tried this yet though, because for now, Charles' suggestion to inject signals into the SinOutputDACBias works fine for me.


Meindert Norg

Forum Jump:

Users browsing this thread: 1 Guest(s)