View Full Version : PID Problem
Tobias
- 19th March 2011, 03:16
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.
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]
HenrikOlsson
- 19th March 2011, 07:22
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
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:
If pid_Out.15 Then pid_Out = 0
That should make pid_Out go more or less positive but never negative.
/Henrik.
Tobias
- 19th March 2011, 13:40
Here is the entire 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]
HenrikOlsson
- 19th March 2011, 15:43
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.
Darrel Taylor
- 19th March 2011, 15:54
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
Tobias
- 19th March 2011, 20:45
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
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]
Tobias
- 19th March 2011, 21:42
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.
HenrikOlsson
- 20th March 2011, 01:03
Hi,
Still not quite there....
Below is the code with the If pid_Out.15 Then pid_Out = 0 included.
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.
Tobias
- 20th March 2011, 01:40
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.
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]
HenrikOlsson
- 20th March 2011, 08:28
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.
Powered by vBulletin® Version 4.1.7 Copyright © 2024 vBulletin Solutions, Inc. All rights reserved.