PID Problem

[HenrikOlsson]

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]

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]

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]

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]

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]

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]

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.