Sinusoidal Disturbance Injection Sina Administrator Posts: 675 Threads: 50 Joined: Jun 2008 02-03-2011, 12:01 PM 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. Code: ```close del gat #define servoperiod               P5 servoperiod=(I10/8388608) #define RTIperiod                 P6 RTIperiod=(I10/8388608*(I8+1)) #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) //I105=\$10f0 //I106=3 // 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 OPEN PLC 0 CLEAR OutputReg = Magnitude * SIN(((Counter*RTIperiod)%SinPeriod)/SinPeriod*360) Counter=Counter+1 CLOSE I5=3 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: Code: ```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. *** Banned Posts: 527 Threads: 22 Joined: Jun 2010 02-08-2011, 09:20 AM (This post was last modified: 02-08-2011, 09:23 AM by ***.) Here's another example (a little different from Sina's) for your reference: Code: ```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 Close``` meindert.norg Junior Member Posts: 39 Threads: 16 Joined: Jan 2011 02-25-2011, 01:29 PM 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. Sina Administrator Posts: 675 Threads: 50 Joined: Jun 2008 02-25-2011, 03:46 PM Meindert, 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. Regards, Sina Sattari Hardware Engineering Manager Delta Tau Data Systems, Inc. meindert.norg Junior Member Posts: 39 Threads: 16 Joined: Jan 2011 03-01-2011, 12:14 PM Sina, 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. Thanks, Meindert Norg « Next Oldest | Next Newest »