                         .module valve6.c
 0000                   .dbfile valve6.c
                         .area text
                       ;  IX -> 0,x
 0000                 _SCI_isr::
 0000                   .dbfile C:\ICCTJP\include/isrdecl.h
 0000                   .dbfunc SCI_isr
 0000                   .dbline 41
                       ; /************************************************************************************************************************
                       ; *   Program:        crap.c                          *
                       ; *   Description:    This program was written to test the pneumatic                      *
                       ; *                   valves and cylinders.  It will prompt the user  *
                       ; *                   to select the desired function (manual or       *
                       ; *                   automatic positioning) and to select the desired      *
                       ; *                   characteristics of the PWM signal for the       *
                       ; *                   valves.                 *
                       ; *   Programmer:     Scott Nortman                   *
                       ; *   Date:           March 16, 2000                  *
                       ; ************************************************************************************************************************/
                       ; 
                       ; /**************************** Constants *********************************/
                       ; #define pulse_handler TOC2_isr
                       ; #define valve_sensor analog(1)
                       ; #define in  0
                       ; #define out 1
                       ; #define P 0
                       ; #define I 1
                       ; #define D 2
                       ; #define dt 4096
                       ; /**************************** End of Constants **************************/
                       ; 
                       ; /*************************** Interrupts *********************************/
                       ; #pragma interrupt_handler TOC2_isr, RTI_isr
                       ; /*************************** End of Interrupts **************************/
                       ; 
                       ; /************************** Includes ************************************/
                       ; #include <tjpbase.h>
                       ; #include <analog.h>
                       ; #include <vectors.h>
                       ; #include <mil.h>
                       ; #include <hc11.h>
                       ; /************************ End of includes *******************************/
                       ; 
                       ; /************************** Global Variables*****************************/
                       ; long duty_cycle;    //duty_cycle in e clocks
                       ; long period;        //period in e clocks
                       ; int desired_position;
                       ; int error[3] = {0};
                       ; int k[3] = {300, 0, 0};
 0000                 L2:
 0000 3B                rti
                       ;  IX -> 0,x
 0001                 _SPI_isr::
 0001                   .dbfunc SPI_isr
 0001                   .dbline 47
                       ;     char clear[]= "\x1b\x5B\x32\x4A\x04"; 
                       ; 
                       ; /************************ End of Global Variables ***********************/
                       ; 
                       ; 
                       ; /************************** Function Declarations  **********************/
 0001                 L3:
 0001 3B                rti
                       ;  IX -> 0,x
 0002                 _PAIE_isr::
 0002                   .dbfunc PAIE_isr
 0002                   .dbline 53
                       ; void init_pulse();      //Initilizes TOC for PWM interrupts
                       ; void move(int);
                       ; void pulse_handler();   //This is the function executed at each TOC2_isr
                       ; void move_cylinder(int);
                       ; long pidControl(int error[3], int k[3], int deltat);
                       ; /******************** End of Function Declarations  *********************/
 0002                 L4:
 0002 3B                rti
                       ;  IX -> 0,x
 0003                 _PAV_isr::
 0003                   .dbfunc PAV_isr
 0003                   .dbline 59
                       ; 
                       ; 
                       ; /**************************** Main **************************************/
                       ; 
                       ; void main(void)
                       ; {
 0003                 L5:
 0003 3B                rti
                       ;  IX -> 0,x
 0004                 _TOF_isr::
 0004                   .dbfunc TOF_isr
 0004                   .dbline 65
                       ;     /* VT100 clear screen */ 
                       ;     char clear[]= "\x1b\x5B\x32\x4A\x04"; 
                       ;     
                       ;     /* VT100 position cursor at (x,y) = (3,12) command is "\x1b[3;12H"*/
                       ;     char place_home[]= "\x1b[1;1H"; /*Home*/
                       ;     
 0004                 L6:
 0004 3B                rti
                       ;  IX -> 0,x
 0005                 _TOC3_isr::
 0005                   .dbfunc TOC3_isr
 0005                   .dbline 89
                       ;     char place_text[] = "\x1b[10;1H";/*position for text to be entered*/
                       ;     
                       ;     desired_position = 150;
                       ;     
                       ;     SET_BIT(PACTL, 0x80);  //Data direction for porta
                       ;     CLEAR_BIT(PACTL, 0x03); //Set RTI rate
                       ;     SET_BIT(TMSK2, 0x40);//Enable RTI interrupt 
                       ;     init_pulse();
                       ;     init_analog();
                       ;     init_serial();
                       ; 
                       ;     printf("%s", clear);  
                       ;     printf("%s", place_home);
                       ;     
                       ;     period = 60000;
                       ;     INTR_ON();
                       ;     while (1)
                       ;     {
                       ;         duty_cycle = pidControl(error, k, dt);
                       ;         
                       ;         if (duty_cycle < 0)
                       ;         {
                       ;             CLEAR_BIT(PORTA, 0x20);
                       ;             duty_cycle = -1 * duty_cycle;
 0005                 L7:
 0005 3B                rti
                       ;  IX -> 0,x
 0006                 _TIC3_isr::
 0006                   .dbfunc TIC3_isr
 0006                   .dbline 101
                       ;         }
                       ;         else
                       ;         {
                       ;             SET_BIT(PORTA, 0x20);
                       ;         }
                       ;     }
                       ;     
                       ; }
                       ; 
                       ; 
                       ; /**************************** End of Main *******************************/
                       ; 
 0006                 L8:
 0006 3B                rti
                       ;  IX -> 0,x
 0007                 _TIC2_isr::
 0007                   .dbfunc TIC2_isr
 0007                   .dbline 107
                       ; 
                       ; 
                       ; /************************** Function Defintions *************************/
                       ; 
                       ; void init_pulse(void)
                       ; {
 0007                 L9:
 0007 3B                rti
                       ;  IX -> 0,x
 0008                 _TIC1_isr::
 0008                   .dbfunc TIC1_isr
 0008                   .dbline 113
                       ;     INTR_OFF();
                       ;     SET_BIT(TCTL1, 0x80);   //Enable OC2 for falling edge on first sucessful compare (safe default)
                       ;     CLEAR_BIT(TCTL1, 0x40);
                       ;     TOC2 = 0;   //Set TOC2 to zero
                       ;     SET_BIT(TMSK1, 0x40);   //Enable OC2 mask
                       ; }
 0008                 L10:
 0008 3B                rti
                       ;  IX -> 0,x
 0009                 _IRQ_isr::
 0009                   .dbfunc IRQ_isr
 0009                   .dbline 125
                       ; 
                       ; void pulse_handler()
                       ; {   //printf("ISR\n");
                       ; 
                       ;     //enable toggle
                       ;     //CLEAR_BIT(TCTL1, 0x80);
                       ;     //SET_BIT(TCTL1, 0x40);
                       ;     if (duty_cycle <= 100)  
                       ;     //This handles if duty cycle is less than or equal to 100 e clocks ( almost always off)
                       ;     {
                       ;         SET_BIT(TCTL1, 0x80);   //Enable OC2 for falling edge (make sure pin goes low)
                       ;         CLEAR_BIT(TCTL1, 0X40);
 0009                 L11:
 0009 3B                rti
                       ;  IX -> 0,x
 000A                 _XIRQ_isr::
 000A                   .dbfunc XIRQ_isr
 000A                   .dbline 131
                       ;         TOC2 += period;         //Add the period to TOC2
                       ;     }
                       ;     
                       ;     else if ((period - duty_cycle) <= 100)  //This handles what to do if the pin should almost always be high
                       ;     {
                       ;         CLEAR_BIT(TCTL1, 0x80); //Set the pin to rising edge
 000A                 L12:
 000A 3B                rti
                       ;  IX -> 0,x
 000B                 _SWI_isr::
 000B                   .dbfunc SWI_isr
 000B                   .dbline 137
                       ;         SET_BIT(TCTL1, 0X40);
                       ;         TOC2 += period;         //Add period
                       ;     }
                       ;     
                       ;     else        //This is for the general case
                       ;     {
 000B                 L13:
 000B 3B                rti
                       ;  IX -> 0,x
 000C                 _ILLOP_isr::
 000C                   .dbfunc ILLOP_isr
 000C                   .dbline 143
                       ;         if (TCTL1 & 0x40)   //Test bit 6.  If it is a one, you are now high and must change to go low
                       ;         {
                       ;             CLEAR_BIT(TCTL1, 0x40);  //Clear bit 6 to set for falling edge on next interrupt
                       ;             SET_BIT(TCTL1, 0X80);
                       ;             TOC2 += duty_cycle;     //Add the duty cycle e clocks if you are now high
                       ;         }
 000C                 L14:
 000C 3B                rti
                       ;  IX -> 0,x
 000D                 _COP_isr::
 000D                   .dbfunc COP_isr
 000D                   .dbline 149
                       ;         
                       ;         else    //Test bit 6.  If it is low, you are now low and must change to go high
                       ;         {
                       ;             SET_BIT(TCTL1, 0xC0);   //Set bit 6 to go high next interrupt
                       ;             TOC2 += (period - duty_cycle);
                       ;         }
 000D                 L15:
 000D 3B                rti
                       ;  IX -> 0,x
 000E                 _CLMON_isr::
 000E                   .dbfunc CLMON_isr
 000E                   .dbline 155
                       ;     }
                       ;     
                       ;     
                       ;     CLEAR_FLAG(TFLG1, 0x40);    //Clear the interrupt flag
                       ; }   //This is the end of the interrupt routine
                       ; 
 000E                 L16:
 000E 3B                rti
                         .area data
 0000                 _drew::
 0000                   .blkw 1
                         .area idata
 0000 0000              .word _interrupt_vectors
                         .area data
 0002                 _error::
 0002                   .blkb 2
                         .area idata
 0002 0000              .word 0
                         .area data
 0004                   .blkb 4
                         .area idata
 0004 00000000          .byte 0,0,0,0
                         .area data
 0008                 _k::
 0008                   .blkb 2
                         .area idata
 0008 012C              .word 300
                         .area data
 000A                   .blkb 2
                         .area idata
 000A 0000              .word 0
                         .area data
 000C                   .blkb 2
                         .area idata
 000C 0000              .word 0
                         .area data
 000E                 _clear::
 000E                   .blkb 6
                         .area idata
 000E 1B5B324A0400      .byte 27,91,50,'J,4,0
                         .area text
 000F                 L18:
 000F 1B5B324A0400      .byte 27,91,50,'J,4,0
 0015                 L19:
 0015 1B5B313B314800    .byte 27,91,49,59,49,'H,0
 001C                 L20:
 001C 1B5B31303B314800  .byte 27,91,49,48,59,49,'H,0
                       ;  lreg1 -> -4,x
                       ;  lreg2 -> -8,x
                       ;  IX -> 0,x
                       ;          ?temp -> 3,x
                       ;     place_text -> 7,x
                       ;     place_home -> 15,x
                       ;          clear -> 22,x
 0024                 _main::
 0024 BD0000            jsr __enterb
 0027 9C                .byte 0x9c
 0028                   .dbfile valve6.c
 0028                   .dbfunc main
 0028                   .dbline 61
 0028 EC00              ldd 0,x
 002A C30016            addd #22
 002D 18CE000F          ldy #L18
 0031 3C                pshx
 0032 8F                xgdx
 0033 CC0006            ldd #6
 0036 BD0000            jsr __asgnblk
 0039 38                pulx
 003A                   .dbline 64
 003A EC00              ldd 0,x
 003C C3000F            addd #15
 003F 18CE0015          ldy #L19
 0043 3C                pshx
 0044 8F                xgdx
 0045 CC0007            ldd #7
 0048 BD0000            jsr __asgnblk
 004B 38                pulx
 004C                   .dbline 66
 004C EC00              ldd 0,x
 004E C30007            addd #7
 0051 18CE001C          ldy #L20
 0055 3C                pshx
 0056 8F                xgdx
 0057 CC0008            ldd #8
 005A BD0000            jsr __asgnblk
 005D 38                pulx
 005E                   .dbline 68
 005E CC0096            ldd #150
 0061 FD0000            std _desired_position
 0064                   .dbline 70
 0064 18CE1026          ldy #0x1026
 0068 181C0080          bset 0,y,#128
 006C                   .dbline 71
 006C 18CE1026          ldy #0x1026
 0070 181D0003          bclr 0,y,#0x3
 0074                   .dbline 72
 0074 18CE1024          ldy #0x1024
 0078 181C0040          bset 0,y,#64
 007C                   .dbline 73
 007C BD012A            jsr _init_pulse
 007F                   .dbline 74
 007F BD0000            jsr _init_analog
 0082                   .dbline 75
 0082 BD0000            jsr _init_serial
 0085                   .dbline 77
 0085 EC00              ldd 0,x
 0087 C30016            addd #22
 008A 37                pshb
 008B 36                psha
 008C CC0415            ldd #L21
 008F BD0000            jsr _printf
 0092 1838              puly
 0094                   .dbline 78
 0094 EC00              ldd 0,x
 0096 C3000F            addd #15
 0099 37                pshb
 009A 36                psha
 009B CC0415            ldd #L21
 009E BD0000            jsr _printf
 00A1 1838              puly
 00A3                   .dbline 80
 00A3 18CE0411          ldy #L22
 00A7 BD0000            jsr __ly2reg
 00AA 18CE0002          ldy #_period
 00AE BD0000            jsr __lreg2y
 00B1 0E                        cli
                       
 00B2                   .dbline 81
 00B2 206C              bra L24
 00B4                 L23:
 00B4                   .dbline 84
 00B4 CC1000            ldd #4096
 00B7 37                pshb
 00B8 36                psha
 00B9 CC0008            ldd #_k
 00BC 37                pshb
 00BD 36                psha
 00BE CC0002            ldd #_error
 00C1 BD02FB            jsr _pidControl
 00C4 1838              puly
 00C6 1838              puly
 00C8 EC00              ldd 0,x
 00CA C30003            addd #3
 00CD 188F              xgdy
 00CF BD0000            jsr __lreg2y
 00D2 EC00              ldd 0,x
 00D4 C30003            addd #3
 00D7 188F              xgdy
 00D9 BD0000            jsr __ly2reg
 00DC 18CE0006          ldy #_duty_cycle
 00E0 BD0000            jsr __lreg2y
 00E3                   .dbline 86
 00E3 18CE0006          ldy #_duty_cycle
 00E7 BD0000            jsr __ly2reg
 00EA 18CE040D          ldy #L28
 00EE BD0000            jsr __ly2reg2
 00F1 BD0000            jsr __lcmp
 00F4 2C22              bge L26
 00F6                   .dbline 88
 00F6 18CE1000          ldy #0x1000
 00FA 181D0020          bclr 0,y,#0x20
 00FE                   .dbline 89
 00FE 18CE0409          ldy #L29
 0102 BD0000            jsr __ly2reg
 0105 18CE0006          ldy #_duty_cycle
 0109 BD0000            jsr __ly2reg2
 010C BD0000            jsr __lmul
 010F 18CE0006          ldy #_duty_cycle
 0113 BD0000            jsr __lreg2y
 0116                   .dbline 90
 0116 2008              bra L27
 0118                 L26:
 0118                   .dbline 93
 0118 18CE1000          ldy #0x1000
 011C 181C0020          bset 0,y,#32
 0120                   .dbline 94
 0120                 L27:
 0120                   .dbline 95
 0120                 L24:
 0120                   .dbline 82
 0120 2092              bra L23
 0122                 L17:
 0122 8F                xgdx
 0123 C3001C            addd #28
 0126 8F                xgdx
 0127 35                txs
 0128 38                pulx
 0129 39                rts
                       ;  IX -> 0,x
 012A                 _init_pulse::
 012A 0F                        sei
                       
 012B                   .dbfunc init_pulse
 012B                   .dbline 109
 012B 18CE1020          ldy #0x1020
 012F 181C0080          bset 0,y,#128
 0133                   .dbline 110
 0133 18CE1020          ldy #0x1020
 0137 181D0040          bclr 0,y,#0x40
 013B                   .dbline 111
 013B CC0000            ldd #0
 013E FD1018            std 0x1018
 0141                   .dbline 112
 0141 18CE1022          ldy #0x1022
 0145 181C0040          bset 0,y,#64
 0149                   .dbline 113
 0149                 L30:
 0149 39                rts
                       ;  lreg1 -> -4,x
                       ;  lreg2 -> -8,x
                       ;  IX -> 0,x
 014A                 _TOC2_isr::
 014A BD0000            jsr __enterb
 014D 82                .byte 0x82
 014E                   .dbfunc TOC2_isr
 014E                   .dbline 121
 014E 18CE0006          ldy #_duty_cycle
 0152 BD0000            jsr __ly2reg
 0155 18CE0405          ldy #L34
 0159 BD0000            jsr __ly2reg2
 015C BD0000            jsr __lcmp
 015F 2E29              bgt L32
 0161                   .dbline 124
 0161 18CE1020          ldy #0x1020
 0165 181C0080          bset 0,y,#128
 0169                   .dbline 125
 0169 18CE1020          ldy #0x1020
 016D 181D0040          bclr 0,y,#0x40
 0171                   .dbline 126
 0171 18CE0002          ldy #_period
 0175 BD0000            jsr __ly2reg2
                         ; vol
 0178 FC1018            ldd 0x1018
 017B BD0000            jsr __ud2lreg
 017E BD0000            jsr __ladd
 0181 BD0000            jsr __lreg2d
 0184 FD1018            std 0x1018
 0187                   .dbline 127
 0187 7E022B            jmp L33
 018A                 L32:
 018A                   .dbline 129
 018A 18CE0002          ldy #_period
 018E BD0000            jsr __ly2reg
 0191 18CE0006          ldy #_duty_cycle
 0195 BD0000            jsr __ly2reg2
 0198 BD0000            jsr __lsub
 019B 18CE0405          ldy #L34
 019F BD0000            jsr __ly2reg2
 01A2 BD0000            jsr __lcmp
 01A5 2E28              bgt L35
 01A7                   .dbline 131
 01A7 18CE1020          ldy #0x1020
 01AB 181D0080          bclr 0,y,#0x80
 01AF                   .dbline 132
 01AF 18CE1020          ldy #0x1020
 01B3 181C0040          bset 0,y,#64
 01B7                   .dbline 133
 01B7 18CE0002          ldy #_period
 01BB BD0000            jsr __ly2reg2
                         ; vol
 01BE FC1018            ldd 0x1018
 01C1 BD0000            jsr __ud2lreg
 01C4 BD0000            jsr __ladd
 01C7 BD0000            jsr __lreg2d
 01CA FD1018            std 0x1018
 01CD                   .dbline 134
 01CD 205C              bra L36
 01CF                 L35:
 01CF                   .dbline 138
 01CF 18CE1020          ldy #0x1020
 01D3 181F004028        brclr 0,y,#64,L37
 01D8                   .dbline 140
 01D8 18CE1020          ldy #0x1020
 01DC 181D0040          bclr 0,y,#0x40
 01E0                   .dbline 141
 01E0 18CE1020          ldy #0x1020
 01E4 181C0080          bset 0,y,#128
 01E8                   .dbline 142
 01E8 18CE0006          ldy #_duty_cycle
 01EC BD0000            jsr __ly2reg2
                         ; vol
 01EF FC1018            ldd 0x1018
 01F2 BD0000            jsr __ud2lreg
 01F5 BD0000            jsr __ladd
 01F8 BD0000            jsr __lreg2d
 01FB FD1018            std 0x1018
 01FE                   .dbline 143
 01FE 202B              bra L38
 0200                 L37:
 0200                   .dbline 147
 0200 18CE1020          ldy #0x1020
 0204 181C00C0          bset 0,y,#192
 0208                   .dbline 148
 0208 18CE0002          ldy #_period
 020C BD0000            jsr __ly2reg
 020F 18CE0006          ldy #_duty_cycle
 0213 BD0000            jsr __ly2reg2
 0216 BD0000            jsr __lsub
 0219 BD0000            jsr __lregmov
                         ; vol
 021C FC1018            ldd 0x1018
 021F BD0000            jsr __ud2lreg
 0222 BD0000            jsr __ladd
 0225 BD0000            jsr __lreg2d
 0228 FD1018            std 0x1018
 022B                   .dbline 149
 022B                 L38:
 022B                   .dbline 150
 022B                 L36:
 022B                 L33:
 022B                   .dbline 153
 022B 18CE1023          ldy #0x1023
 022F 181D00BF          bclr 0,y,#0xbf
 0233                   .dbline 154
 0233                 L31:
 0233 08                inx
 0234 08                inx
 0235 35                txs
 0236 38                pulx
 0237 3B                rti
 0238                 L40:
 0238 1B5B313B314800    .byte 27,91,49,59,49,'H,0
                       ;  lreg1 -> -4,x
                       ;  lreg2 -> -8,x
                       ;  IX -> 0,x
                       ;  func temp: 2,x - 4,x
                       ;     place_home -> 5,x
 023F                 _RTI_isr::
 023F BD0000            jsr __enterb
 0242 8C                .byte 0x8c
 0243                   .dbfunc RTI_isr
 0243                   .dbline 161
                       ; void RTI_isr(void)
                       ; {
                       ; 
                       ;     
                       ;     /* VT100 position cursor at (x,y) = (3,12) command is "\x1b[3;12H"*/
                       ;     char place_home[]= "\x1b[1;1H"; /*Home*/
 0243 EC00              ldd 0,x
 0245 C30005            addd #5
 0248 18CE0238          ldy #L40
 024C 3C                pshx
 024D 8F                xgdx
 024E CC0007            ldd #7
 0251 BD0000            jsr __asgnblk
 0254 38                pulx
 0255                   .dbline 162
                       ;     error [2] = error[1];
 0255 FC0004            ldd _error+2
 0258 FD0006            std _error+4
 025B                   .dbline 163
                       ;     error[1] = error[0];
 025B FC0002            ldd _error
 025E FD0004            std _error+2
 0261                   .dbline 164
                       ;     error[0] = (desired_position - valve_sensor);
 0261 CC0001            ldd #1
 0264 BD0000            jsr _analog
 0267 ED02              std 2,x
 0269 FC0000            ldd _desired_position
 026C A302              subd 2,x
 026E FD0002            std _error
 0271                   .dbline 167
                       ;     
                       ;     
                       ;     printf("%s", place_home);
 0271 EC00              ldd 0,x
 0273 C30005            addd #5
 0276 37                pshb
 0277 36                psha
 0278 CC0415            ldd #L21
 027B BD0000            jsr _printf
 027E 1838              puly
 0280                   .dbline 168
                       ;     printf("%s", clear);
 0280 CC000E            ldd #_clear
 0283 37                pshb
 0284 36                psha
 0285 CC0415            ldd #L21
 0288 BD0000            jsr _printf
 028B 1838              puly
 028D                   .dbline 170
                       ;     
                       ;     printf("Valve sensor:  %d\n", valve_sensor);
 028D CC0001            ldd #1
 0290 BD0000            jsr _analog
 0293 37                pshb
 0294 36                psha
 0295 CC03F2            ldd #L44
 0298 BD0000            jsr _printf
 029B 1838              puly
 029D                   .dbline 172
                       ;     
                       ;     printf("element 0:  %d\n", error[0]);
 029D FC0002            ldd _error
 02A0 37                pshb
 02A1 36                psha
 02A2 CC03E2            ldd #L45
 02A5 BD0000            jsr _printf
 02A8 1838              puly
 02AA                   .dbline 173
                       ;     printf("element 1:  %d\n", error[1]);
 02AA FC0004            ldd _error+2
 02AD 37                pshb
 02AE 36                psha
 02AF CC03D2            ldd #L46
 02B2 BD0000            jsr _printf
 02B5 1838              puly
 02B7                   .dbline 174
                       ;     printf("element 2:  %d\n", error[2]);
 02B7 FC0006            ldd _error+4
 02BA 37                pshb
 02BB 36                psha
 02BC CC03C2            ldd #L48
 02BF BD0000            jsr _printf
 02C2 1838              puly
 02C4                   .dbline 175
                       ;     printf("PGain:  %d\n", k[P]);
 02C4 FC0008            ldd _k
 02C7 37                pshb
 02C8 36                psha
 02C9 CC03B6            ldd #L50
 02CC BD0000            jsr _printf
 02CF 1838              puly
 02D1                   .dbline 176
                       ;     printf("DC:  %ld\n", duty_cycle);
 02D1 18CE0006          ldy #_duty_cycle
 02D5 BD0000            jsr __ly2reg
 02D8 183C              pshy
 02DA 183C              pshy
 02DC 1830              tsy
 02DE BD0000            jsr __lreg2y
 02E1 CC03AC            ldd #L51
 02E4 BD0000            jsr _printf
 02E7 1838              puly
 02E9 1838              puly
 02EB                   .dbline 177
                       ;     CLEAR_FLAG(TFLG2, 0x40);
 02EB 18CE1025          ldy #0x1025
 02EF 181D00BF          bclr 0,y,#0xbf
 02F3                   .dbline 179
                       ;     
                       ; }
 02F3                 L39:
 02F3 8F                xgdx
 02F4 C3000C            addd #12
 02F7 8F                xgdx
 02F8 35                txs
 02F9 38                pulx
 02FA 3B                rti
                       ;  lreg1 -> -4,x
                       ;  lreg2 -> -8,x
                       ;  IX -> 0,x
                       ;         deltaM -> 2,x
                       ;        deltaMd -> 6,x
                       ;        deltaMi -> 10,x
                       ;        deltaMp -> 14,x
                       ;         deltat -> 26,x
                       ;              k -> 24,x
                       ;          error -> 20,x
 02FB                 _pidControl::
 02FB BD0000            jsr __enterb
 02FE D2                .byte 0xd2
 02FF                   .dbfunc pidControl
 02FF                   .dbline 187
                       ; 
                       ; long pidControl(int error[3], int k[3], int deltat)
                       ; {
                       ;     long deltaMp, deltaMi, deltaMd;
                       ;     long deltaM;
                       ;     
                       ;     //proportional
                       ;     deltaMp = (k[P] * error[0]);
 02FF 1AEE18            ldy 24,x
 0302 18EC00            ldd 0,y
 0305 1AEE14            ldy 20,x
 0308 18EE00            ldy 0,y
 030B BD0000            jsr __muli
 030E BD0000            jsr __d2lreg
 0311 EC00              ldd 0,x
 0313 C3000E            addd #14
 0316 188F              xgdy
 0318 BD0000            jsr __lreg2y
 031B                   .dbline 189
                       ;     //integral
                       ;     deltaMi = k[I] * error[0] * deltat;
 031B 1AEE18            ldy 24,x
 031E 18EC02            ldd 2,y
 0321 1AEE14            ldy 20,x
 0324 18EE00            ldy 0,y
 0327 BD0000            jsr __muli
 032A 1AEE1A            ldy 26,x
 032D BD0000            jsr __muli
 0330 BD0000            jsr __d2lreg
 0333 EC00              ldd 0,x
 0335 C3000A            addd #10
 0338 188F              xgdy
 033A BD0000            jsr __lreg2y
 033D                   .dbline 191
                       ;     //derivative
                       ;     deltaMd = k[D] * (error[0] - error[1]);
 033D 1AEE14            ldy 20,x
 0340 18EC00            ldd 0,y
 0343 1AEE14            ldy 20,x
 0346 18A302            subd 2,y
 0349 188F              xgdy
 034B 183C              pshy ; spill
 034D 1AEE18            ldy 24,x
 0350 18EC04            ldd 4,y
 0353 1838              puly ; reload
 0355 BD0000            jsr __muli
 0358 BD0000            jsr __d2lreg
 035B EC00              ldd 0,x
 035D C30006            addd #6
 0360 188F              xgdy
 0362 BD0000            jsr __lreg2y
 0365                   .dbline 193
                       ;     
                       ;     deltaM = deltaMp + deltaMi + deltaMd;
 0365 EC00              ldd 0,x
 0367 C3000E            addd #14
 036A 188F              xgdy
 036C BD0000            jsr __ly2reg
 036F EC00              ldd 0,x
 0371 C3000A            addd #10
 0374 188F              xgdy
 0376 BD0000            jsr __ly2reg2
 0379 BD0000            jsr __ladd
 037C EC00              ldd 0,x
 037E C30006            addd #6
 0381 188F              xgdy
 0383 BD0000            jsr __ly2reg2
 0386 BD0000            jsr __ladd
 0389 EC00              ldd 0,x
 038B C30002            addd #2
 038E 188F              xgdy
 0390 BD0000            jsr __lreg2y
 0393                   .dbline 194
                       ;     return (deltaM);
 0393 EC00              ldd 0,x
 0395 C30002            addd #2
 0398 188F              xgdy
 039A BD0000            jsr __ly2reg
 039D BD0000            jsr __lret
 03A0 0012              .word 18
 03A2                 L52:
 03A2 8F                xgdx
 03A3 C30012            addd #18
 03A6 8F                xgdx
 03A7 35                txs
 03A8 38                pulx
 03A9 1838              puly
 03AB 39                rts
                         .area bss
 0000                 _desired_position::
 0000                   .blkb 2
 0002                 _period::
 0002                   .blkb 4
 0006                 _duty_cycle::
 0006                   .blkb 4
 000A                 _timertjp::
 000A                   .blkb 2
 000C                 _days::
 000C                   .blkb 2
 000E                 _hours::
 000E                   .blkb 2
 0010                 _minutes::
 0010                   .blkb 2
 0012                 _seconds::
 0012                   .blkb 2
 0014                 _msec::
 0014                   .blkb 2
                         .area text
 03AC                 L51:
 03AC 44433A2020256C640A00  .byte 'D,'C,58,32,32,37,'l,'d,10,0
 03B6                 L50:
 03B6 504761696E3A202025640A00  .byte 'P,'G,'a,'i,'n,58,32,32,37,'d,10,0
 03C2                 L48:
 03C2 656C656D656E7420323A202025640A00  .byte 'e,'l,'e,'m,'e,'n,'t,32,50,58,32,32,37,'d,10,0
 03D2                 L46:
 03D2 656C656D656E7420313A202025640A00  .byte 'e,'l,'e,'m,'e,'n,'t,32,49,58,32,32,37,'d,10,0
 03E2                 L45:
 03E2 656C656D656E7420303A202025640A00  .byte 'e,'l,'e,'m,'e,'n,'t,32,48,58,32,32,37,'d,10,0
 03F2                 L44:
 03F2 56616C76652073656E736F723A202025  .byte 'V,'a,'l,'v,'e,32,'s,'e,'n,'s,'o,'r,58,32,32,37
 0402 640A00            .byte 'd,10,0
 0405                 L34:
 0405 00000064          .word 0,100
 0409                 L29:
 0409 FFFFFFFF          .word 65535,65535
 040D                 L28:
 040D 00000000          .word 0,0
 0411                 L22:
 0411 00007FFF          .word 0,32767
 0415                 L21:
 0415 257300            .byte 37,'s,0
