PID Problem

Using the PID code from a thread a few months ago on an 18F4431. I can't get the PID to decrement. PID part of code is below and in bold. I compare a square wave input with a square wave signal from a hall effect on an electric motor. I change Duty1-4 to get the speed of the motor to match speed from input square wave. The PID will increment the Duty1 but will not lower it when needed. I can't get a handle on this to figure out why yet.

Code:

define CODE_SIZE 32 define osc 20 Include "modedefs.bas" ' Mode definitions for Serout INCLUDE "incPID.pbp" 'Include the PID routine. 'These variables are declared by the incPID routine but 'the user needs to assign values to them. pid_Kp = $0700 'Set Kp to 7.0 pid_Ki = $0080 'Set Ki to 0.5 pid_Kd = $0225 'Set Kd to 2.14 pid_Ti = 4 'Update I-term every 8th call to PID pid_I_Clamp = 10 'Clamp I-term to max ±100 ADValue VAR WORD '<---This is your variable. SetPoint VAR WORD '<---This is your variable. OnOff1 var PortE.1 'Input Signal from GPS to activate/deactivate row OnOff2 var PortE.0 'Input Signal from GPS to activate/deactivate row OnOff3 var PortA.5 'Input Signal from GPS to activate/deactivate row OnOff4 var PortB.5 'Input Signal from GPS to activate/deactivate row Switch1 var PortD.0 ''Output to 4066; activates Speed1 Switch2 var PortE.2 ''Output to 4066; activates Speed2 Switch3 var PortC.0 ''Output to 4066; activates Speed3 Switch4 var PortC.3 ''Output to 4066; activates Speed4 MotorSpeed var PortC.1 'Input from 4066 Population var PortC.2 'Input from GPS Enable1 var PortD.1 ''Output Turns on Motor1 driver Enable2 var PortD.2 ''Output Turns on Motor2 driver Enable3 var PortD.3 ''Output Turns on Motor3 driver Enable4 var PortD.4 ''Output Turns on Motor4 driver SinglePair var PortD.7 'Input Determines code path Input1 var PortB.0 'PWM0 Input2 var PortB.1 'PWM1 Input3 var PortB.2 'PWM2 Input4 var PortB.3 'PWM3 LCD var PortD.5 Duty1 Var Word Duty2 Var Word Duty3 Var Word Duty4 Var Word Duty1S data word 00300 Duty2S data word 00300 Duty3S data word 00300 Duty4S data word 00300 BCD1 var byte BCD10 var byte LowRate var Long HighRate var Long TotalRate var Long DisplayTenths var word MotorCount var byte 'Tracks motorspeed input Mod1 var byte 'Offset tracker to correct motorspeed Mod2 Var Byte 'Offset tracker to correct motorspeed Mod3 var byte 'Offset tracker to correct motorspeed Mod4 Var byte 'Offset tracker to correct motorspeed PosMin1 var bit PosMin2 var bit PosMin3 var bit PosMin4 var bit HighTime1 var Long LowTime1 var Long HighTime2 var Long LowTime2 var Long HighTime3 var Long LowTime3 var Long HighTime4 var Long LowTime4 var Long TotalTime1 var Long TotalTime2 var Long TotalTime3 var Long TotalTime4 var Long Freq var word UpperLimit VAR WORD LowerLimit VAR WORD Gain var word Prefix con $FE ' needed before each command LcdCls CON $51 ' clear LCD (use PAUSE 5 after) LcdLine1 CON $00 ' move cursor to line 1 LcdLine2 con $40 ' move curson to line 2 LcdLine3 con $14 ' move curson to line 3 lcdLine4 con $54 ' move curson to line 4 LcdCol1 con $00 ' move cursor to column 1 LcdCol2 con $07 ' move cursor to column 7 Backlight con $53 ' Backlighting 1-8 CursorPS con $45 'Cursor Position CursorOn con $47 'Cursof On CursorOff con $48 'Cursor Off INTCON = 0 'Interrupts off for now INTCON2.7 = 1 'Pull Ups disabled ADCON0 = 0 'A/D OFF ANSEL0 = %00000000 'All analogue channels to digital ANSEL1 = %00000000 'All analogue channels to digital TRISA = %111111 'PortA to inputs TRISB = %00100000 'PortB to outputs, except B5 TRISC = %00000110 'PortC, Switch3 and Switch4 outputs; Motorspeed and Population inputs TRISD = %00000000 'PortD, Switch 1 output; Enable1-4 TRISE = %011 PWMCON0=%01111111 'All odd PWMs enabled, all independent SSPCON=%00000000 'Disables Serial Port PTCON0 = %00001000 'PTCON0 = %00000010 back up Freq=%11111111 PTPERL =Freq.LowByte ' PTPERH =Freq.HighByte ' PTPER = $0100 or 256d for ~19.45kHz PWMCON1 = 1 ' updates enabled, overrides sync w/timebase PTCON1 = %11000000 ' PWM time base is ON, counts up 500hz FLTCONFIG = %00000010 ' enable fault A, cycle-by-cycle mode High Enable1 High Enable2 High Enable3 High Enable4 MotorCount = 1 read 0,Duty1.LowByte read 1,Duty1.HighByte read 2,Duty2.LowByte read 3,Duty2.HighByte read 4,Duty3.LowByte read 5,Duty3.HighByte read 6,Duty4.LowByte read 7,Duty4.HighByte PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte pause 2000 pid_Out_Clamp = 511 'Clamp the final output to ±511 LowerLimit=200 UpperLimit=800 'Gain=10 RunAround: Gosub BCD 'SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",dec5 pid_Error] 'SEROUT2 LCD,84, [Prefix,CursorPS,0, #BCD1, " ", #TotalRate, " ", #MotorCount] If BCD1 = 0 then 'Tests row control with motor speed at 50% Duty1=300 Duty2=300 Duty3=300 Duty4=300 PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM 'GosuB AdjustRPM endif If BCD1 = 1 then 'run time%" RunTime ", #MotorCount," ", SEROUT2 LCD,84, [Prefix,CursorPS,0, #motorcount," ", #Duty1," ", #Duty2," ", #Duty3," ", #Duty4 ] PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM GosuB AdjustRPM endif 'If BCD1 = 1 then ' GoSub MeasureRPM ' gosub Display1 ' If MotorCount = 4 or Motorcount >4 then ' MotorCount = 1 ' else ' MotorCount = MotorCount + 1 ' endif 'endif If BCD1 = 2 then GoSub MeasureRPM gosub Display2 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 3 then GoSub MeasureRPM gosub Display3 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 4 then GoSub MeasureRPM gosub Display4 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 5 then SEROUT2 LCD,84, [Prefix,CursorPS,0, "1 2 3 4 P"] SEROUT2 LCD,84, [Prefix,CursorPS,64, #OnOff1," ", #OnOff2," ",#OnOff3," ",#OnOff4," ", #Population] Low Switch1 Low Switch2 Low Switch3 Low Switch4 endif goto RunAround AdjustRPM: If TotalRate = 0 then Duty1 = 0 Duty2 = 0 Duty3 = 0 Duty4 = 0 else Select case MotorCount Case 1 If OnOff1 = 1 then ''''''''CHECK WHY NOT REDUCING DUTY pid_Error = TotalRate-TotalTime1 'Calculate the error SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",#pid_Sign, " ",dec5 pid_Error] Gosub PID 'Result returned in pid_Drive pid_Out = ABS pid_Out 'Convert from two's comp. to absolute Duty1 = pid_Out ' If TotalRate > TotalTime1 then ' If Duty1>LowerLimit then ' Duty1 = Duty1-Gain ' endif ' endif ' If TotalRate < TotalTime1 then ' If DUty1<UpperLimit then ' Duty1 = Duty1+Gain ' endif ' endif endif Case 2 If OnOff2 = 1 then If TotalRate > TotalTime2 then If Duty2>LowerLimit then Duty2 = Duty2-Gain endif endif If TotalRate < TotalTime2 then If DUty2<UpperLimit then Duty2 = Duty2+Gain endif endif endif Case 3 If OnOff3=1 then If TotalRate > TotalTime3 then If Duty3>LowerLimit then Duty3 = Duty3-Gain endif endif If TotalRate < TotalTime3 then If DUty3<UpperLimit then Duty3 = Duty3+Gain endif endif endif Case 4 If OnOff4=1 then If TotalRate > TotalTime4 then If Duty4>LowerLimit then Duty4 = Duty4-Gain endif endif If TotalRate < TotalTime4 then If DUty4<UpperLimit then Duty4 = Duty4+Gain endif endif endif end select endif Return MeasureRPM: pulsin Population, 0, LowRate pulsin Population, 1, HighRate TotalRate= LowRate+HighRate SEROUT2 LCD,84, [Prefix,CursorPS,20,#TotalRate," "] If MotorCount = 1 then If Onoff1 = 1 then High Switch1 Low Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 If HighTime1>500 then If LowTime1>500 then TotalTime1 = HighTime1+LowTime1 'else 'TotalTime1 = 0 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime1," ",#HighTime1," ",#LowTIme1, " " ] 'SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty1," " ] endif Endif If MotorCount = 2 then If Onoff2 = 1 then Low Switch1 High Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 If HighTime2>500 then If LowTime2>500 then TotalTime2 = HighTime2+LowTime2 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime2," ",#HighTime2," ",#LowTIme2, " "] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty2," " ] endif EndIf If MotorCount = 3 then If Onoff3 = 1 then Low Switch1 Low Switch2 High Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 If HighTime3>500 then If LowTime3>500 then TotalTime3 = HighTime3+LowTime3 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime3," ",#HighTime3," ",#LowTIme3, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty3," " ] endif Endif If MotorCount = 4 then If Onoff4 = 1 then Low Switch1 Low Switch2 Low Switch3 High Switch4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 If HighTime4>500 then If LowTime4>500 then TotalTime4 = HighTime4+LowTime4 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime4," ",#HighTime4," ",#LowTIme4, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty4," " ] endif endif If MotorCount = 4 or Motorcount >4 then MotorCount = 1 write 0,Duty1.LowByte write 1,Duty1.HighByte write 2,Duty2.LowByte write 3,Duty2.HighByte write 4,Duty3.LowByte write 5,Duty3.HighByte write 6,Duty4.LowByte write 7,Duty4.HighByte else MotorCount = MotorCount + 1 endif return Display1: If Motorcount = 1 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime1] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime1 ] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime1] endif Return Display2: If Motorcount = 2 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime2] SEROUT2 LCD,84, [Prefix,CursorPS,20, DEC5 HighTime2] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime2] endif Return Display3: If Motorcount = 3 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime3 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime3] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime3] endif Return Display4: If Motorcount = 4 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime4 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime4] SEROUT2 LCD,84, [Prefix,CursorPS,84, DEC5 TotalTime4] endif Return RowControl: If Onoff1 = 1 then high enable1 else low enable1 endif If Onoff2 = 1 then high enable2 else low enable2 endif If Onoff3 = 1 then high enable3 else low enable3 endif If Onoff4 = 1 then high enable4 else low enable4 endif return BCD: BCD1=PORTA BCD1=BCD1 & $0F BCD1=BCD1^ $0F 'BCD10=PORTC 'BCD10=BCD10 & $F0 'BCD10=BCD10^ $F0 'BCD10 = BCD10 >>4 BCD10=PORTC BCD10=BCD10 & $0F BCD10=BCD10^ $0F return 'On Start-Up 'Check SinglePair ' If High Then goto Single ' If Low then goto Paired 'Single 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Measure Motor1-4 rpm 'Adjust Input1-4 dutycycle to maintain speed requested from Population 'GOto Single 'Paired 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Calculate offset needed for requested Population 'Measure motor1 rpm 'Adjust Input1 dutycycle to maintain speed requested from Population 'Measure offset between motor1 and motor2 'Adjust Input2 dutycycle to get required offset 'Goto Paired ' Duty1 = (LowRate*10)/492 '10% 489 ' Duty2 = (LowRate*10)/492 '20% 977 ' Duty3 = (LowRate*10)/492 '50% 2461 ' Duty4 = (LowRate*10)/492 '80% 3924 ' Duty1 = Duty1*10 ' Duty2 = Duty2*10 ' Duty3 = Duty3*10 ' Duty4 = Duty4*10 ' PDC0L = Duty1.LowByte ' PDC0H = Duty1.HighByte ' PDC1L = Duty3.LowByte ' PDC1H = Duty3.HighByte ' PDC2L = Duty2.LowByte ' PDC2H = Duty2.HighByte ' PDC3L = Duty4.LowByte ' PDC3H = Duty4.HighByte ' DisplayTenths=Duty1-(Duty1/10*10) 'SEROUT2 LCD,84, [Prefix,CursorPS,10,#TotalRate]

Re: PID Problem

Hi,
That's not all the code, is it? I don't see any values being assigned to TotalRate and/or TotalTime1 etc anywhere, or am I missing it?

You have TotalRate and TotalTime1(etc) declared as LONGS, while pid_Error is a WORD. I'm not 100% sure but I think that might be part of the problem because when subtracting two longs you end up with a LONG but pid_Error will only "get" the lower 16bits of that result which makes it go all wrong (I think). Try changing TotalRate and TotalTime to WORD size variables and see what happends.

Next thing, you have

Code:

pid_Out = ABS pid_Out Duty1 = pid_Out

If your drive system is one quadrant, ie you can supply power in one direction only (which is true if you have for example a single low side MOSFET switching the motor) then this won't work very well because when pid_Out becomes negative, ie the regulator loop wants to reverse/break the motor the ABS will make it positive and instead apply power to the motor - not good.

What you need to do, if you have a single quadrant system, is to not allow the output to swing negative. Something like:

Code:

If pid_Out.15 Then pid_Out = 0

That should make pid_Out go more or less positive but never negative.

/Henrik.

Re: PID Problem

Here is the entire code

Code:

'Do a Pulsin Loop off Population, must see rpm before starting motors, cannot rely on row control 'when board is powered up. Do another BCD with this code in it so no reprogram is needed after initial 'board test on planter define CODE_SIZE 32 define osc 20 Include "modedefs.bas" ' Mode definitions for Serout INCLUDE "incPID.pbp" 'Include the PID routine. 'These variables are declared by the incPID routine but 'the user needs to assign values to them. pid_Kp = $0700 'Set Kp to 7.0 pid_Ki = $0080 'Set Ki to 0.5 pid_Kd = $0225 'Set Kd to 2.14 pid_Ti = 4 'Update I-term every 8th call to PID pid_I_Clamp = 10 'Clamp I-term to max ±100 ADValue VAR WORD '<---This is your variable. SetPoint VAR WORD '<---This is your variable. OnOff1 var PortE.1 'Input Signal from GPS to activate/deactivate row OnOff2 var PortE.0 'Input Signal from GPS to activate/deactivate row OnOff3 var PortA.5 'Input Signal from GPS to activate/deactivate row OnOff4 var PortB.5 'Input Signal from GPS to activate/deactivate row Switch1 var PortD.0 ''Output to 4066; activates Speed1 Switch2 var PortE.2 ''Output to 4066; activates Speed2 Switch3 var PortC.0 ''Output to 4066; activates Speed3 Switch4 var PortC.3 ''Output to 4066; activates Speed4 MotorSpeed var PortC.1 'Input from 4066 Population var PortC.2 'Input from GPS Enable1 var PortD.1 ''Output Turns on Motor1 driver Enable2 var PortD.2 ''Output Turns on Motor2 driver Enable3 var PortD.3 ''Output Turns on Motor3 driver Enable4 var PortD.4 ''Output Turns on Motor4 driver SinglePair var PortD.7 'Input Determines code path Input1 var PortB.0 'PWM0 Input2 var PortB.1 'PWM1 Input3 var PortB.2 'PWM2 Input4 var PortB.3 'PWM3 LCD var PortD.5 Duty1 Var Word Duty2 Var Word Duty3 Var Word Duty4 Var Word Duty1S data word 00300 Duty2S data word 00300 Duty3S data word 00300 Duty4S data word 00300 BCD1 var byte BCD10 var byte LowRate var Long HighRate var Long TotalRate var Long DisplayTenths var word MotorCount var byte 'Tracks motorspeed input Mod1 var byte 'Offset tracker to correct motorspeed Mod2 Var Byte 'Offset tracker to correct motorspeed Mod3 var byte 'Offset tracker to correct motorspeed Mod4 Var byte 'Offset tracker to correct motorspeed PosMin1 var bit PosMin2 var bit PosMin3 var bit PosMin4 var bit HighTime1 var Long LowTime1 var Long HighTime2 var Long LowTime2 var Long HighTime3 var Long LowTime3 var Long HighTime4 var Long LowTime4 var Long TotalTime1 var Long TotalTime2 var Long TotalTime3 var Long TotalTime4 var Long Freq var word UpperLimit VAR WORD LowerLimit VAR WORD Gain var word Prefix con $FE ' needed before each command LcdCls CON $51 ' clear LCD (use PAUSE 5 after) LcdLine1 CON $00 ' move cursor to line 1 LcdLine2 con $40 ' move curson to line 2 LcdLine3 con $14 ' move curson to line 3 lcdLine4 con $54 ' move curson to line 4 LcdCol1 con $00 ' move cursor to column 1 LcdCol2 con $07 ' move cursor to column 7 Backlight con $53 ' Backlighting 1-8 CursorPS con $45 'Cursor Position CursorOn con $47 'Cursof On CursorOff con $48 'Cursor Off INTCON = 0 'Interrupts off for now INTCON2.7 = 1 'Pull Ups disabled ADCON0 = 0 'A/D OFF ANSEL0 = %00000000 'All analogue channels to digital ANSEL1 = %00000000 'All analogue channels to digital TRISA = %111111 'PortA to inputs TRISB = %00100000 'PortB to outputs, except B5 TRISC = %00000110 'PortC, Switch3 and Switch4 outputs; Motorspeed and Population inputs TRISD = %00000000 'PortD, Switch 1 output; Enable1-4 TRISE = %011 PWMCON0=%01111111 'All odd PWMs enabled, all independent SSPCON=%00000000 'Disables Serial Port PTCON0 = %00001000 'PTCON0 = %00000010 back up Freq=%11111111 PTPERL =Freq.LowByte ' PTPERH =Freq.HighByte ' PTPER = $0100 or 256d for ~19.45kHz PWMCON1 = 1 ' updates enabled, overrides sync w/timebase PTCON1 = %11000000 ' PWM time base is ON, counts up 500hz FLTCONFIG = %00000010 ' enable fault A, cycle-by-cycle mode High Enable1 High Enable2 High Enable3 High Enable4 MotorCount = 1 read 0,Duty1.LowByte read 1,Duty1.HighByte read 2,Duty2.LowByte read 3,Duty2.HighByte read 4,Duty3.LowByte read 5,Duty3.HighByte read 6,Duty4.LowByte read 7,Duty4.HighByte PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte pause 2000 pid_Out_Clamp = 500 'Clamp the final output to ±511 LowerLimit=200 UpperLimit=800 'Gain=10 RunAround: Gosub BCD 'SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",dec5 pid_Error] 'SEROUT2 LCD,84, [Prefix,CursorPS,0, #BCD1, " ", #TotalRate, " ", #MotorCount] If BCD1 = 0 then 'Tests row control with motor speed at 50% Duty1=300 Duty2=300 Duty3=300 Duty4=300 PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM 'GosuB AdjustRPM endif If BCD1 = 1 then 'run time%" RunTime ", #MotorCount," ", SEROUT2 LCD,84, [Prefix,CursorPS,0, #motorcount," ", #Duty1," ", #Duty2," ", #Duty3," ", #Duty4 ] PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM GosuB AdjustRPM endif 'If BCD1 = 1 then ' GoSub MeasureRPM ' gosub Display1 ' If MotorCount = 4 or Motorcount >4 then ' MotorCount = 1 ' else ' MotorCount = MotorCount + 1 ' endif 'endif If BCD1 = 2 then GoSub MeasureRPM gosub Display2 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 3 then GoSub MeasureRPM gosub Display3 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 4 then GoSub MeasureRPM gosub Display4 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 5 then SEROUT2 LCD,84, [Prefix,CursorPS,0, "1 2 3 4 P"] SEROUT2 LCD,84, [Prefix,CursorPS,64, #OnOff1," ", #OnOff2," ",#OnOff3," ",#OnOff4," ", #Population] Low Switch1 Low Switch2 Low Switch3 Low Switch4 endif goto RunAround AdjustRPM: If TotalRate = 0 then Duty1 = 0 Duty2 = 0 Duty3 = 0 Duty4 = 0 else Select case MotorCount Case 1 If OnOff1 = 1 then ''''''''CHECK WHY NOT REDUCING DUTY pid_Error = TotalRate-TotalTime1 'Calculate the error SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",#pid_Sign, " ",dec5 pid_Error] Gosub PID 'Result returned in pid_Drive pid_Out = ABS pid_Out 'Convert from two's comp. to absolute Duty1 = pid_Out ' If TotalRate > TotalTime1 then ' If Duty1>LowerLimit then ' Duty1 = Duty1-Gain ' endif ' endif ' If TotalRate < TotalTime1 then ' If DUty1 < UpperLimit then ' Duty1 = Duty1+Gain ' endif ' endif endif Case 2 If OnOff2 = 1 then If TotalRate > TotalTime2 then If Duty2 > LowerLimit then Duty2 = Duty2-Gain endif endif If TotalRate < TotalTime2 then If DUty2<UpperLimit then Duty2 = Duty2+Gain endif endif endif Case 3 If OnOff3=1 then If TotalRate > TotalTime3 then If Duty3 > LowerLimit then Duty3 = Duty3-Gain endif endif If TotalRate < TotalTime3 then If DUty3 < UpperLimit then Duty3 = Duty3+Gain endif endif endif Case 4 If OnOff4=1 then If TotalRate > TotalTime4 then If Duty4 > LowerLimit then Duty4 = Duty4-Gain endif endif If TotalRate < TotalTime4 then If DUty4 < UpperLimit then Duty4 = Duty4+Gain endif endif endif end select endif Return MeasureRPM: pulsin Population, 0, LowRate pulsin Population, 1, HighRate TotalRate= LowRate+HighRate SEROUT2 LCD,84, [Prefix,CursorPS,20,#TotalRate," "] If MotorCount = 1 then If Onoff1 = 1 then High Switch1 Low Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 If HighTime1>500 then If LowTime1>500 then TotalTime1 = HighTime1+LowTime1 'else 'TotalTime1 = 0 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime1," ",#HighTime1," ",#LowTIme1, " " ] 'SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty1," " ] endif Endif If MotorCount = 2 then If Onoff2 = 1 then Low Switch1 High Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 If HighTime2>500 then If LowTime2>500 then TotalTime2 = HighTime2+LowTime2 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime2," ",#HighTime2," ",#LowTIme2, " "] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty2," " ] endif EndIf If MotorCount = 3 then If Onoff3 = 1 then Low Switch1 Low Switch2 High Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 If HighTime3>500 then If LowTime3>500 then TotalTime3 = HighTime3+LowTime3 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime3," ",#HighTime3," ",#LowTIme3, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty3," " ] endif Endif If MotorCount = 4 then If Onoff4 = 1 then Low Switch1 Low Switch2 Low Switch3 High Switch4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 If HighTime4>500 then If LowTime4>500 then TotalTime4 = HighTime4+LowTime4 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime4," ",#HighTime4," ",#LowTIme4, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty4," " ] endif endif If MotorCount = 4 or Motorcount >4 then MotorCount = 1 write 0,Duty1.LowByte write 1,Duty1.HighByte write 2,Duty2.LowByte write 3,Duty2.HighByte write 4,Duty3.LowByte write 5,Duty3.HighByte write 6,Duty4.LowByte write 7,Duty4.HighByte else MotorCount = MotorCount + 1 endif return Display1: If Motorcount = 1 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime1] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime1 ] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime1] endif Return Display2: If Motorcount = 2 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime2] SEROUT2 LCD,84, [Prefix,CursorPS,20, DEC5 HighTime2] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime2] endif Return Display3: If Motorcount = 3 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime3 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime3] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime3] endif Return Display4: If Motorcount = 4 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime4 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime4] SEROUT2 LCD,84, [Prefix,CursorPS,84, DEC5 TotalTime4] endif Return RowControl: If Onoff1 = 1 then high enable1 else low enable1 endif If Onoff2 = 1 then high enable2 else low enable2 endif If Onoff3 = 1 then high enable3 else low enable3 endif If Onoff4 = 1 then high enable4 else low enable4 endif return BCD: BCD1=PORTA BCD1=BCD1 & $0F BCD1=BCD1^ $0F 'BCD10=PORTC 'BCD10=BCD10 & $F0 'BCD10=BCD10^ $F0 'BCD10 = BCD10 >>4 BCD10=PORTC BCD10=BCD10 & $0F BCD10=BCD10^ $0F return 'On Start-Up 'Check SinglePair ' If High Then goto Single ' If Low then goto Paired 'Single 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Measure Motor1-4 rpm 'Adjust Input1-4 dutycycle to maintain speed requested from Population 'GOto Single 'Paired 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Calculate offset needed for requested Population 'Measure motor1 rpm 'Adjust Input1 dutycycle to maintain speed requested from Population 'Measure offset between motor1 and motor2 'Adjust Input2 dutycycle to get required offset 'Goto Paired ' Duty1 = (LowRate*10)/492 '10% 489 ' Duty2 = (LowRate*10)/492 '20% 977 ' Duty3 = (LowRate*10)/492 '50% 2461 ' Duty4 = (LowRate*10)/492 '80% 3924 ' Duty1 = Duty1*10 ' Duty2 = Duty2*10 ' Duty3 = Duty3*10 ' Duty4 = Duty4*10 ' PDC0L = Duty1.LowByte ' PDC0H = Duty1.HighByte ' PDC1L = Duty3.LowByte ' PDC1H = Duty3.HighByte ' PDC2L = Duty2.LowByte ' PDC2H = Duty2.HighByte ' PDC3L = Duty4.LowByte ' PDC3H = Duty4.HighByte ' DisplayTenths=Duty1-(Duty1/10*10) 'SEROUT2 LCD,84, [Prefix,CursorPS,10,#TotalRate]

Re: PID Problem

Hi,
For some reason the code won't show properly at my end, there's still "only" the same amount of code as in the first message. I did get an email notification and the complete code was embedded there so it seems to be an issue with the forum or my web-browser or something.

Anyway, I think you should look into the issues I pointed out in my first reply. I'm pretty sure that should put you on the right track.

EDIT: What does the DEFINE CODE_SIZE 32 do?

/Henrik.

Re: PID Problem

Tobias,

This isn't related to your PID issue, but I thought I'd mention it.
Since you are using an 18F, which requires MPASM ...

The DEFINEs must be all CAPS.
With define osc 20, PBP will assume a 4Mhz oscillator.

define OSC 20

Re: PID Problem

Below is the code with the If pid_Out.15 Then pid_Out = 0 included.

I have a 40 tooth wheel on the output shaft of the motor I am controlling. There is another motor with a 40 tooth wheel that I am measuring the period and using it to control the speed of the four other motors. So I need to change the Duty1-4 based on the speed of the control motor and the speed of the four other motors. To bench test the code I am using a function generator to simulate the input signal and four motors are on the bench controlled by the four PWM's

I substituted the 'set point' in the example with the period of the control motor. TotalTime1 replaces ADValue.

I just don't understand how inputting the two periods into the PID routine will modify the PWM number.

At the beginning of my code I set Duty1 to 300. If I have the function generator output a period less than that of the motor being controller by Duty1, the PID changes Duty1 to 0 and if the function gererator period is greater than the Duty1 motor period, the PID changes Duty1 to 1023.
What I am having a hard time understanding is

Code:

define CODE_SIZE 32 define osc 20 Include "modedefs.bas" ' Mode definitions for Serout INCLUDE "incPID.pbp" 'Include the PID routine. 'These variables are declared by the incPID routine but 'the user needs to assign values to them. pid_Kp = $0700 'Set Kp to 7.0 pid_Ki = $0080 'Set Ki to 0.5 pid_Kd = $0225 'Set Kd to 2.14 pid_Ti = 4 'Update I-term every 8th call to PID pid_I_Clamp = 10 'Clamp I-term to max ±100 ADValue VAR WORD '<---This is your variable. SetPoint VAR WORD '<---This is your variable. OnOff1 var PortE.1 'Input Signal from GPS to activate/deactivate row OnOff2 var PortE.0 'Input Signal from GPS to activate/deactivate row OnOff3 var PortA.5 'Input Signal from GPS to activate/deactivate row OnOff4 var PortB.5 'Input Signal from GPS to activate/deactivate row Switch1 var PortD.0 ''Output to 4066; activates Speed1 Switch2 var PortE.2 ''Output to 4066; activates Speed2 Switch3 var PortC.0 ''Output to 4066; activates Speed3 Switch4 var PortC.3 ''Output to 4066; activates Speed4 MotorSpeed var PortC.1 'Input from 4066 Population var PortC.2 'Input from GPS Enable1 var PortD.1 ''Output Turns on Motor1 driver Enable2 var PortD.2 ''Output Turns on Motor2 driver Enable3 var PortD.3 ''Output Turns on Motor3 driver Enable4 var PortD.4 ''Output Turns on Motor4 driver SinglePair var PortD.7 'Input Determines code path Input1 var PortB.0 'PWM0 Input2 var PortB.1 'PWM1 Input3 var PortB.2 'PWM2 Input4 var PortB.3 'PWM3 LCD var PortD.5 Duty1 Var Word Duty2 Var Word Duty3 Var Word Duty4 Var Word Duty1S data word 00300 Duty2S data word 00300 Duty3S data word 00300 Duty4S data word 00300 BCD1 var byte BCD10 var byte LowRate var Long HighRate var Long TotalRate var Long DisplayTenths var word MotorCount var byte 'Tracks motorspeed input Mod1 var byte 'Offset tracker to correct motorspeed Mod2 Var Byte 'Offset tracker to correct motorspeed Mod3 var byte 'Offset tracker to correct motorspeed Mod4 Var byte 'Offset tracker to correct motorspeed PosMin1 var bit PosMin2 var bit PosMin3 var bit PosMin4 var bit HighTime1 var Long LowTime1 var Long HighTime2 var Long LowTime2 var Long HighTime3 var Long LowTime3 var Long HighTime4 var Long LowTime4 var Long TotalTime1 var Long TotalTime2 var Long TotalTime3 var Long TotalTime4 var Long Freq var word UpperLimit VAR WORD LowerLimit VAR WORD Gain var word Prefix con $FE ' needed before each command LcdCls CON $51 ' clear LCD (use PAUSE 5 after) LcdLine1 CON $00 ' move cursor to line 1 LcdLine2 con $40 ' move curson to line 2 LcdLine3 con $14 ' move curson to line 3 lcdLine4 con $54 ' move curson to line 4 LcdCol1 con $00 ' move cursor to column 1 LcdCol2 con $07 ' move cursor to column 7 Backlight con $53 ' Backlighting 1-8 CursorPS con $45 'Cursor Position CursorOn con $47 'Cursof On CursorOff con $48 'Cursor Off INTCON = 0 'Interrupts off for now INTCON2.7 = 1 'Pull Ups disabled ADCON0 = 0 'A/D OFF ANSEL0 = %00000000 'All analogue channels to digital ANSEL1 = %00000000 'All analogue channels to digital TRISA = %111111 'PortA to inputs TRISB = %00100000 'PortB to outputs, except B5 TRISC = %00000110 'PortC, Switch3 and Switch4 outputs; Motorspeed and Population inputs TRISD = %00000000 'PortD, Switch 1 output; Enable1-4 TRISE = %011 PWMCON0=%01111111 'All odd PWMs enabled, all independent SSPCON=%00000000 'Disables Serial Port PTCON0 = %00001000 'PTCON0 = %00000010 back up Freq=%11111111 PTPERL =Freq.LowByte ' PTPERH =Freq.HighByte ' PTPER = $0100 or 256d for ~19.45kHz PWMCON1 = 1 ' updates enabled, overrides sync w/timebase PTCON1 = %11000000 ' PWM time base is ON, counts up 500hz FLTCONFIG = %00000010 ' enable fault A, cycle-by-cycle mode Serout2 LCD, 84, [Prefix,LcdCLS] 'pause 1000 SEROUT2 LCD,84, [Prefix,CursorPS,0, "Graham Electric Planter Drive "] pause 1000 Serout2 LCD, 84, [Prefix,LcdCLS] High Enable1 High Enable2 High Enable3 High Enable4 MotorCount = 1 read 0,Duty1.LowByte read 1,Duty1.HighByte read 2,Duty2.LowByte read 3,Duty2.HighByte read 4,Duty3.LowByte read 5,Duty3.HighByte read 6,Duty4.LowByte read 7,Duty4.HighByte PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte pause 2000 pid_Out_Clamp = 500 'Clamp the final output to ±511 LowerLimit=200 UpperLimit=800 'Gain=10 RunAround: Gosub BCD 'SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",dec5 pid_Error] 'SEROUT2 LCD,84, [Prefix,CursorPS,0, #BCD1, " ", #TotalRate, " ", #MotorCount] If BCD1 = 0 then 'Tests row control with motor speed at 50% Duty1=300 Duty2=300 Duty3=300 Duty4=300 PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM 'GosuB AdjustRPM endif If BCD1 = 1 then 'run time%" RunTime ", #MotorCount," ", SEROUT2 LCD,84, [Prefix,CursorPS,0, #motorcount," ", #Duty1," ", #Duty2," ", #Duty3," ", #Duty4 ] PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM GosuB AdjustRPM endif 'If BCD1 = 1 then ' GoSub MeasureRPM ' gosub Display1 ' If MotorCount = 4 or Motorcount >4 then ' MotorCount = 1 ' else ' MotorCount = MotorCount + 1 ' endif 'endif If BCD1 = 2 then GoSub MeasureRPM gosub Display2 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 3 then GoSub MeasureRPM gosub Display3 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 4 then GoSub MeasureRPM gosub Display4 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 5 then SEROUT2 LCD,84, [Prefix,CursorPS,0, "1 2 3 4 P"] SEROUT2 LCD,84, [Prefix,CursorPS,64, #OnOff1," ", #OnOff2," ",#OnOff3," ",#OnOff4," ", #Population] Low Switch1 Low Switch2 Low Switch3 Low Switch4 endif goto RunAround AdjustRPM: If TotalRate = 0 then Duty1 = 0 Duty2 = 0 Duty3 = 0 Duty4 = 0 else Select case MotorCount Case 1 If OnOff1 = 1 then ''''''''CHECK WHY NOT REDUCING DUTY pid_Error = TotalRate-TotalTime1 'Calculate the error SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",#pid_Sign, " ",dec5 pid_Error] Gosub PID 'Result returned in pid_Drive pid_Out = ABS pid_Out 'Convert from two's comp. to absolute If pid_Out.15 Then pid_Out = 0 Duty1 = pid_Out ' If TotalRate > TotalTime1 then ' If Duty1>LowerLimit then ' Duty1 = Duty1-Gain ' endif ' endif ' If TotalRate < TotalTime1 then ' If DUty1 < UpperLimit then ' Duty1 = Duty1+Gain ' endif ' endif endif Case 2 If OnOff2 = 1 then If TotalRate > TotalTime2 then If Duty2>LowerLimit then Duty2 = Duty2-Gain endif endif If TotalRate < TotalTime2 then If DUty2 < UpperLimit then Duty2 = Duty2+Gain endif endif endif Case 3 If OnOff3=1 then If TotalRate > TotalTime3 then If Duty3>LowerLimit then Duty3 = Duty3-Gain endif endif If TotalRate < TotalTime3 then If DUty3 < UpperLimit then Duty3 = Duty3+Gain endif endif endif Case 4 If OnOff4=1 then If TotalRate > TotalTime4 then If Duty4>LowerLimit then Duty4 = Duty4-Gain endif endif If TotalRate < TotalTime4 then If DUty4 < UpperLimit then Duty4 = Duty4+Gain endif endif endif end select endif Return MeasureRPM: pulsin Population, 0, LowRate pulsin Population, 1, HighRate TotalRate= LowRate+HighRate SEROUT2 LCD,84, [Prefix,CursorPS,20,#TotalRate," "] If MotorCount = 1 then If Onoff1 = 1 then High Switch1 Low Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 If HighTime1>500 then If LowTime1>500 then TotalTime1 = HighTime1+LowTime1 'else 'TotalTime1 = 0 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime1," ",#HighTime1," ",#LowTIme1, " " ] 'SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty1," " ] endif Endif If MotorCount = 2 then If Onoff2 = 1 then Low Switch1 High Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 If HighTime2>500 then If LowTime2>500 then TotalTime2 = HighTime2+LowTime2 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime2," ",#HighTime2," ",#LowTIme2, " "] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty2," " ] endif EndIf If MotorCount = 3 then If Onoff3 = 1 then Low Switch1 Low Switch2 High Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 If HighTime3>500 then If LowTime3>500 then TotalTime3 = HighTime3+LowTime3 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime3," ",#HighTime3," ",#LowTIme3, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty3," " ] endif Endif If MotorCount = 4 then If Onoff4 = 1 then Low Switch1 Low Switch2 Low Switch3 High Switch4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 If HighTime4>500 then If LowTime4>500 then TotalTime4 = HighTime4+LowTime4 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,#TotalTime4," ",#HighTime4," ",#LowTIme4, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty4," " ] endif endif If MotorCount = 4 or Motorcount >4 then MotorCount = 1 write 0,Duty1.LowByte write 1,Duty1.HighByte write 2,Duty2.LowByte write 3,Duty2.HighByte write 4,Duty3.LowByte write 5,Duty3.HighByte write 6,Duty4.LowByte write 7,Duty4.HighByte else MotorCount = MotorCount + 1 endif return Display1: If Motorcount = 1 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime1] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime1 ] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime1] endif Return Display2: If Motorcount = 2 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime2] SEROUT2 LCD,84, [Prefix,CursorPS,20, DEC5 HighTime2] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime2] endif Return Display3: If Motorcount = 3 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime3 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime3] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime3] endif Return Display4: If Motorcount = 4 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime4 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime4] SEROUT2 LCD,84, [Prefix,CursorPS,84, DEC5 TotalTime4] endif Return RowControl: If Onoff1 = 1 then high enable1 else low enable1 endif If Onoff2 = 1 then high enable2 else low enable2 endif If Onoff3 = 1 then high enable3 else low enable3 endif If Onoff4 = 1 then high enable4 else low enable4 endif return BCD: BCD1=PORTA BCD1=BCD1 & $0F BCD1=BCD1^ $0F 'BCD10=PORTC 'BCD10=BCD10 & $F0 'BCD10=BCD10^ $F0 'BCD10 = BCD10 >>4 BCD10=PORTC BCD10=BCD10 & $0F BCD10=BCD10^ $0F return 'On Start-Up 'Check SinglePair ' If High Then goto Single ' If Low then goto Paired 'Single 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Measure Motor1-4 rpm 'Adjust Input1-4 dutycycle to maintain speed requested from Population 'GOto Single 'Paired 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Calculate offset needed for requested Population 'Measure motor1 rpm 'Adjust Input1 dutycycle to maintain speed requested from Population 'Measure offset between motor1 and motor2 'Adjust Input2 dutycycle to get required offset 'Goto Paired ' Duty1 = (LowRate*10)/492 '10% 489 ' Duty2 = (LowRate*10)/492 '20% 977 ' Duty3 = (LowRate*10)/492 '50% 2461 ' Duty4 = (LowRate*10)/492 '80% 3924 ' Duty1 = Duty1*10 ' Duty2 = Duty2*10 ' Duty3 = Duty3*10 ' Duty4 = Duty4*10 ' PDC0L = Duty1.LowByte ' PDC0H = Duty1.HighByte ' PDC1L = Duty3.LowByte ' PDC1H = Duty3.HighByte ' PDC2L = Duty2.LowByte ' PDC2H = Duty2.HighByte ' PDC3L = Duty4.LowByte ' PDC3H = Duty4.HighByte ' DisplayTenths=Duty1-(Duty1/10*10) 'SEROUT2 LCD,84, [Prefix,CursorPS,10,#TotalRate]

Re: PID Problem

I think I got it now. I need to work on percentage of needed versus observed periods and then modify the dutycycle with the Pidout as the percentage modifier. If not I'll just be writing more code for the hell of it but its worth a shot.

Re: PID Problem

Hi,
Still not quite there....

Quote:

Below is the code with the If pid_Out.15 Then pid_Out = 0 included.

Code:

pid_Out = ABS pid_Out 'Convert from two's comp. to absolute If pid_Out.15 Then pid_Out = 0

No, this completely defeats the purpose. You can't first say ABS and THEN check if it's negative - it'll NEVER be negative AFTER you've converted it to ABSsoulte. Simply remove the pid_Out = ABS pid_Out all together.

And you're still trying to squeeze the result of a LONG-LONG subtraction into a WORD sized variable and I'm not sure if or how that is going to work.

Re: PID Problem

Changed the LONGS to WORDS and removed the ABS line. Its still the same. I just don't understand how it is going to change the DUTY4 number to what is really needed. I have to be missing something.

Code:

define CODE_SIZE 32 define osc 20 Include "modedefs.bas" ' Mode definitions for Serout INCLUDE "incPID.pbp" 'Include the PID routine. ADValue VAR WORD '<---This is your variable. SetPoint VAR WORD '<---This is your variable. OnOff1 var PortE.1 'Input Signal from GPS to activate/deactivate row OnOff2 var PortE.0 'Input Signal from GPS to activate/deactivate row OnOff3 var PortA.5 'Input Signal from GPS to activate/deactivate row OnOff4 var PortB.5 'Input Signal from GPS to activate/deactivate row Switch1 var PortD.0 ''Output to 4066; activates Speed1 Switch2 var PortE.2 ''Output to 4066; activates Speed2 Switch3 var PortC.0 ''Output to 4066; activates Speed3 Switch4 var PortC.3 ''Output to 4066; activates Speed4 MotorSpeed var PortC.1 'Input from 4066 Population var PortC.2 'Input from GPS Enable1 var PortD.1 ''Output Turns on Motor1 driver Enable2 var PortD.2 ''Output Turns on Motor2 driver Enable3 var PortD.3 ''Output Turns on Motor3 driver Enable4 var PortD.4 ''Output Turns on Motor4 driver SinglePair var PortD.7 'Input Determines code path Input1 var PortB.0 'PWM0 Input2 var PortB.1 'PWM1 Input3 var PortB.2 'PWM2 Input4 var PortB.3 'PWM3 LCD var PortD.5 Duty1 Var Word Duty2 Var Word Duty3 Var Word Duty4 Var Word Duty1S data word 00300 Duty2S data word 00300 Duty3S data word 00300 Duty4S data word 00300 BCD1 var byte BCD10 var byte LowRate var word HighRate var word TotalRate var word DisplayTenths var word MotorCount var byte 'Tracks motorspeed input Mod1 var byte 'Offset tracker to correct motorspeed Mod2 Var Byte 'Offset tracker to correct motorspeed Mod3 var byte 'Offset tracker to correct motorspeed Mod4 Var byte 'Offset tracker to correct motorspeed PosMin1 var bit PosMin2 var bit PosMin3 var bit PosMin4 var bit HighTime1 var word LowTime1 var word HighTime2 var word LowTime2 var word HighTime3 var word LowTime3 var word HighTime4 var word LowTime4 var word TotalTime1 var word TotalTime2 var word TotalTime3 var word TotalTime4 var word Freq var word UpperLimit VAR WORD LowerLimit VAR WORD Gain var word Subtract var bit Prefix con $FE ' needed before each command LcdCls CON $51 ' clear LCD (use PAUSE 5 after) LcdLine1 CON $00 ' move cursor to line 1 LcdLine2 con $40 ' move curson to line 2 LcdLine3 con $14 ' move curson to line 3 lcdLine4 con $54 ' move curson to line 4 LcdCol1 con $00 ' move cursor to column 1 LcdCol2 con $07 ' move cursor to column 7 Backlight con $53 ' Backlighting 1-8 CursorPS con $45 'Cursor Position CursorOn con $47 'Cursof On CursorOff con $48 'Cursor Off INTCON = 0 'Interrupts off for now INTCON2.7 = 1 'Pull Ups disabled ADCON0 = 0 'A/D OFF ANSEL0 = %00000000 'All analogue channels to digital ANSEL1 = %00000000 'All analogue channels to digital TRISA = %111111 'PortA to inputs TRISB = %00100000 'PortB to outputs, except B5 TRISC = %00000110 'PortC, Switch3 and Switch4 outputs; Motorspeed and Population inputs TRISD = %00000000 'PortD, Switch 1 output; Enable1-4 TRISE = %011 PWMCON0=%01111111 'All odd PWMs enabled, all independent SSPCON=%00000000 'Disables Serial Port PTCON0 = %00001000 '8914 9140 2050 'PTCON0 = %00000010 back up Freq=%11111111 PTPERL =Freq.LowByte ' PTPERH =Freq.HighByte ' PTPER = $0100 or 256d for ~19.45kHz PWMCON1 = 1 ' updates enabled, overrides sync w/timebase PTCON1 = %11000000 ' PWM time base is ON, counts up 500hz FLTCONFIG = %00000010 ' enable fault A, cycle-by-cycle mode 'These variables are declared by the incPID routine but 'the user needs to assign values to them. pid_Kp = $0700 'Set Kp to 7.0 pid_Ki = $0080 'Set Ki to 0.5 pid_Kd = $0225 'Set Kd to 2.14 pid_Ti = 4 'Update I-term every 8th call to PID pid_I_Clamp = 10 'Clamp I-term to max ±100 pid_Out_Clamp = 10000 'Clamp the final output to ±511 Serout2 LCD, 84, [Prefix,LcdCLS] 'pause 1000 SEROUT2 LCD,84, [Prefix,CursorPS,0, "Graham Electric Planter Drive "] pause 1000 Serout2 LCD, 84, [Prefix,LcdCLS] High Enable1 High Enable2 High Enable3 High Enable4 MotorCount = 1 read 0,Duty1.LowByte read 1,Duty1.HighByte read 2,Duty2.LowByte read 3,Duty2.HighByte read 4,Duty3.LowByte read 5,Duty3.HighByte read 6,Duty4.LowByte read 7,Duty4.HighByte PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte pause 2000 LowerLimit=200 UpperLimit=800 'Gain=10 RunAround: Gosub BCD 'SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",dec5 pid_Error] 'SEROUT2 LCD,84, [Prefix,CursorPS,0, #BCD1, " ", #TotalRate, " ", #MotorCount] If BCD1 = 0 then 'Tests row control with motor speed at 50% Duty1=300 Duty2=300 Duty3=300 Duty4=300 PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM 'GosuB AdjustRPM endif If BCD1 = 1 then 'run time%" RunTime ", #MotorCount," ", SEROUT2 LCD,84, [Prefix,CursorPS,0, #motorcount," ", #Duty1," ", #Duty2," ", #Duty3," ", #Duty4 ] PDC0L = Duty1.LowByte PDC0H = Duty1.HighByte PDC1L = Duty3.LowByte PDC1H = Duty3.HighByte PDC2L = Duty2.LowByte PDC2H = Duty2.HighByte PDC3L = Duty4.LowByte PDC3H = Duty4.HighByte gosub RowControl GoSub MeasureRPM GosuB AdjustRPM endif 'If BCD1 = 1 then ' GoSub MeasureRPM ' gosub Display1 ' If MotorCount = 4 or Motorcount >4 then ' MotorCount = 1 ' else ' MotorCount = MotorCount + 1 ' endif 'endif If BCD1 = 2 then GoSub MeasureRPM gosub Display2 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 3 then GoSub MeasureRPM gosub Display3 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 4 then GoSub MeasureRPM gosub Display4 If MotorCount = 4 or Motorcount >4 then MotorCount = 1 else MotorCount = MotorCount + 1 endif endif If BCD1 = 5 then SEROUT2 LCD,84, [Prefix,CursorPS,0, "1 2 3 4 P"] SEROUT2 LCD,84, [Prefix,CursorPS,64, #OnOff1," ", #OnOff2," ",#OnOff3," ",#OnOff4," ", #Population] Low Switch1 Low Switch2 Low Switch3 Low Switch4 endif If BCD1 = 15 then endif goto RunAround AdjustRPM: '50286 15586 34700 If TotalRate = 0 then '2510 15586 52460 65536 Duty1 = 0 Duty2 = 0 Duty3 = 0 Duty4 = 0 else Select case MotorCount Case 1 If OnOff1 = 1 then ''''''''CHECK WHY NOT REDUCING DUTY ' If TotalRate>TotalTime1 then pid_Error = TotalRate-TotalTime1 'Calculate the error ' Subtract = 0 ' else ' pid_Error = TotalTime1-TotalRate 'Calculate the error ' Subtract = 1 ' endif Gosub PID SEROUT2 LCD,84, [Prefix,CursorPS,84,#bcd1," ",#pid_Sign, " ",dec5 pid_Out," ", #pid_Out_Clamp] pause 10000 'Result returned in pid_Drive 'If pid_Out.15 Then pid_Out = 0 ' Duty1 = pid_Out endif Case 2 If OnOff2 = 1 then If TotalRate > TotalTime2 then If Duty2>LowerLimit then Duty2 = Duty2-Gain endif endif If TotalRate < TotalTime2 then If DUty2 < UpperLimit then Duty2 = Duty2+Gain endif endif endif Case 3 If OnOff3=1 then If TotalRate > TotalTime3 then If Duty3>LowerLimit then Duty3 = Duty3-Gain endif endif If TotalRate < TotalTime3 then If DUty3 < UpperLimit then Duty3 = Duty3+Gain endif endif endif Case 4 If OnOff4=1 then If TotalRate > TotalTime4 then If Duty4>LowerLimit then Duty4 = Duty4-Gain endif endif If TotalRate < TotalTime4 then If DUty4 < UpperLimit then Duty4 = Duty4+Gain endif endif endif end select endif Return MeasureRPM: pulsin Population, 0, LowRate pulsin Population, 1, HighRate TotalRate= LowRate+HighRate SEROUT2 LCD,84, [Prefix,CursorPS,20,#TotalRate," "] If MotorCount = 1 then If Onoff1 = 1 then High Switch1 Low Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 PUlsin Motorspeed, 1, HighTime1 Pulsin Motorspeed, 0, LowTIme1 If HighTime1>500 then If LowTime1>500 then TotalTime1 = HighTime1+LowTime1 'else 'TotalTime1 = 0 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,Dec5 TotalTime1," ",Dec5 HighTime1," ",Dec5 LowTIme1, " " ] 'SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty1," " ] endif Endif If MotorCount = 2 then If Onoff2 = 1 then Low Switch1 High Switch2 Low Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 PUlsin Motorspeed, 1, HighTime2 Pulsin Motorspeed, 0, LowTIme2 If HighTime2>500 then If LowTime2>500 then TotalTime2 = HighTime2+LowTime2 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,Dec5 TotalTime2," ",Dec5 HighTime2," ",Dec5 LowTIme2, " "] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty2," " ] endif EndIf If MotorCount = 3 then If Onoff3 = 1 then Low Switch1 Low Switch2 High Switch3 Low Switch4 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 PUlsin Motorspeed, 1, HighTime3 Pulsin Motorspeed, 0, LowTIme3 If HighTime3>500 then If LowTime3>500 then TotalTime3 = HighTime3+LowTime3 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,Dec5 TotalTime3," ",Dec5 HighTime3," ",Dec5 LowTIme3, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty3," " ] endif Endif If MotorCount = 4 then If Onoff4 = 1 then Low Switch1 Low Switch2 Low Switch3 High Switch4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 PUlsin Motorspeed, 1, HighTime4 Pulsin Motorspeed, 0, LowTIme4 If HighTime4>500 then If LowTime4>500 then TotalTime4 = HighTime4+LowTime4 endif endif SEROUT2 LCD,84, [Prefix,CursorPS,64,Dec5 TotalTime4," ",Dec5 HighTime4," ",Dec5 LowTIme4, " " ] SEROUT2 LCD,84, [Prefix,CursorPS,10, #Duty4," " ] endif endif If MotorCount = 4 or Motorcount >4 then MotorCount = 1 write 0,Duty1.LowByte write 1,Duty1.HighByte write 2,Duty2.LowByte write 3,Duty2.HighByte write 4,Duty3.LowByte write 5,Duty3.HighByte write 6,Duty4.LowByte write 7,Duty4.HighByte else MotorCount = MotorCount + 1 endif return Display1: If Motorcount = 1 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime1] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime1 ] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime1] endif Return Display2: If Motorcount = 2 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime2] SEROUT2 LCD,84, [Prefix,CursorPS,20, DEC5 HighTime2] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime2] endif Return Display3: If Motorcount = 3 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime3 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime3] SEROUT2 LCD,84, [Prefix,CursorPS,84, #TotalTime3] endif Return Display4: If Motorcount = 4 then SEROUT2 LCD,84, [Prefix,CursorPS,64,DEC5 LowTime4 ] SEROUT2 LCD,84, [Prefix,CursorPS,20,DEC5 HighTime4] SEROUT2 LCD,84, [Prefix,CursorPS,84, DEC5 TotalTime4] endif Return RowControl: If Onoff1 = 1 then high enable1 else low enable1 endif If Onoff2 = 1 then high enable2 else low enable2 endif If Onoff3 = 1 then high enable3 else low enable3 endif If Onoff4 = 1 then high enable4 else low enable4 endif return BCD: BCD1=PORTA BCD1=BCD1 & $0F BCD1=BCD1^ $0F 'BCD10=PORTC 'BCD10=BCD10 & $F0 'BCD10=BCD10^ $F0 'BCD10 = BCD10 >>4 BCD10=PORTC BCD10=BCD10 & $0F BCD10=BCD10^ $0F return 'On Start-Up 'Check SinglePair ' If High Then goto Single ' If Low then goto Paired 'Single 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Measure Motor1-4 rpm 'Adjust Input1-4 dutycycle to maintain speed requested from Population 'GOto Single 'Paired 'If OnOff1 = 0 then Enable1 = 0 else Enable1=1 'If OnOff2 = 0 then Enable2 = 0 else Enable1=2 'If OnOff3 = 0 then Enable3 = 0 else Enable1=3 'If OnOff4 = 0 then Enable4 = 0 else Enable1=4 'Measure DutyCycle of Population 'Calculate offset needed for requested Population 'Measure motor1 rpm 'Adjust Input1 dutycycle to maintain speed requested from Population 'Measure offset between motor1 and motor2 'Adjust Input2 dutycycle to get required offset 'Goto Paired ' Duty1 = (LowRate*10)/492 '10% 489 ' Duty2 = (LowRate*10)/492 '20% 977 ' Duty3 = (LowRate*10)/492 '50% 2461 ' Duty4 = (LowRate*10)/492 '80% 3924 ' Duty1 = Duty1*10 ' Duty2 = Duty2*10 ' Duty3 = Duty3*10 ' Duty4 = Duty4*10 ' PDC0L = Duty1.LowByte ' PDC0H = Duty1.HighByte ' PDC1L = Duty3.LowByte ' PDC1H = Duty3.HighByte ' PDC2L = Duty2.LowByte ' PDC2H = Duty2.HighByte ' PDC3L = Duty4.LowByte ' PDC3H = Duty4.HighByte ' DisplayTenths=Duty1-(Duty1/10*10) 'SEROUT2 LCD,84, [Prefix,CursorPS,10,#TotalRate]

Re: PID Problem

Hi Tobias,
In the code you posted, which still doesn't show properly on the forum, you have the IF pid_Out.15 THEN pid_Out = 0 line commented but I'm sure that's not the case in the compiled and tested code?

And, as far as I can see the only thing you have "connected" to the output of the PID filter is the DUTY1 variable, I don't see how that connects to DUTY4 - if that was the purpose.

Finally, you said earlier that you're trying to control the speed of 4 motors based on the speed of anoter motor. The PID filter will only work with ONE motor. You are not going to be able to have one PID filter for each motor with this code. If that is what you need (but I'd get ONE working first) then there is a special version available that allows multiple filters to be run.

I'm sorry I'm not not much of a help here but I still haven't got the whole picture of exactly what you're trying to do. What I'd probably do is try to boil it down to the bare minimum in order to get a grip on how it works.

Also, Darrel pointed out a thing about the DEFINEs that I don't see in your posted code either.

/Henrik.