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]
PID Problem
Bookmarks