/* TEST Aug 2011 for speed control */
/* Apr 2011 version -- output compatible with nidas; code for 8-port board
   - support for speed counter
   - auto shutdown if stuck
   - speed control! through IPS
      - add capacitor on motor voltage
      - checks current for overlimit (12/15A)
      - "proportional" speed control
   - add Novatel GPS
   - add Crista IMU (binary, serial)
 * May, 31 2011
   - slight rewrite to set duty based on setpoint speed,
     rather than initial speed/duty cycle.  
     Hopefully better control this way.
 * June 10, 2011
   - change to interrupt-driven output (and <still> see character losses)
   *  Apr 28, 2012 am still missing IMU messages -- a few changes to try to fix (pull-downs may be needed
      as well -- seems like I'm mostly missing 0's!  [haven't loaded this yet]
 now know that I'm losing them on the input side...
 * Latest todos:
 * - check output buffer size (just upped from 600->800)
 * - check TRH wiring
 * - is something strange after 9:?
 * - still getting resets
 *
 * May 2012
   - fixed bug setting que_out to 0 which removed binary 0 values from IMU
   - tweaked buffer size for IMU by adding 20 characters -- not really needed after all
   - changed initial delay from 10 to 5s (found Delayus runs twice as fast as documented)
   - set default speed to 3m/s since 4m/s was too fast at 30V -- this is a good value
   - changed "!" to "C:!" to facilitate looking at start/restart times
   - changed min duty to 5 to avoid testing for 0
   - added ntime1[] to avoid reporting partial serial messages (that don't parse in R)
   - moved delay to before timers to avoid case where trolley is coasting and speed counted over delay time!
 * Aug 2012
   - added write pcounts to eeprom to enable different start-up speeds
 *
 */

// was 20MHz before 8/13/13
#define XTAL_FREQ 18MHZ // not exactly 18.432, but just used by Delay_Ms
//#define EEPROMSIZE 256
#include <pic18.h>
//#include <htc.h>

/* new #pragma stuff (see chipinfo.html) */
#pragma config OSC = ECIO
#pragma config PWRT = ON
#pragma config WDT = ON
#pragma config WDTPS = 128 // default WDT period is 18ms x 128 = 2.3s
#pragma config LVP = OFF

#define QTOP 729
#define QBOT 0
#define QLEN 730
#define NCHAN 8 /* number of serial ports in use on the CPU board */

char rfc[QLEN];
int rfpop, rfpush;
int Iflag;
unsigned char pcount, pcounts;
char pbuf[350];
const char hc[16] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};

#include "delay.h"

char que_out() {
    char c;
    if (rfpush != rfpop) {
        c = rfc[rfpop];
        if (rfpop == QTOP)
            rfpop = QBOT;
        else
            rfpop++;
    } else {
        c = 0;
        TXIE = 0;
    }
    return c;
}

void que_in(char qdata) {
    rfc[rfpush] = qdata;
    if (rfpush == QTOP)
        rfpush = QBOT;
    else
        rfpush++;
    TXIE = 1;
 }

void itox2b(char i, char *c) {
    /* should be even more efficient than before */
    c[1] = hc[i & 0x0F];
    i = i >> 4;
    c[0] = hc[i];
}

void interrupt HI_ISR(void) {
    char c;
    if (CCP2IF == 1) {
        /* 100ms should be over; time to read counter and start A/D */
        pcount = TMR0L; // grab counter value and reset
        pcount = (TMR0H << 8) | pcount;
        TMR0H = 0; // clear timer
        TMR0L = 0;

        ADCON0 = 0x81; // set up for A/D conversion of V
        CCP2IF = 0; // reset interrupt
    } else {
        if (TXIF) { // don't bother trying to output a character if TX full 
            c = que_out();
            if (c != 0) TXREG = c; /* output character */
        }
    }
}

void itox4b(int i, char *c) {
    c[3] = hc[i & 0x000F];
    i = i >> 4;
    c[2] = hc[i & 0x000F];
    i = i >> 4;
    c[1] = hc[i & 0x000F];
    i = i >> 4;
    c[0] = hc[i];
}

void putch(unsigned char c) {
    while (!TXIF)
        continue;
    TXREG = c;
}

void rfsend(char ich, int ib, int nb, char *buf) {
    int i;
    if (ich < 10) que_in(ich + '0'); // id character 0-9
    else que_in((ich - 10) + 'A'); // A-Z
    que_in(':'); // ":" as nominal separation character
    for (i = ib; i < nb; i++) que_in(buf[i]); // the (ASCII) data
    que_in(0x0a); // newline termination character
}

/* Note in delay.h says must be used with full optimization; 
   this might explain running about 2x too slow.  */
void DelayMs(unsigned char cnt) {
    unsigned char i;
    while (cnt--) {
        i = 4;
        while (i--) {
            DelayUs(uS_CNT); /* Adjust for error */
        }
    }
}

unsigned char spi_comm(unsigned char c) {
    /* use this both for reading and writing
     *  ignore rc if writing;
     *  set c=anything for reading */
    unsigned char rc;
    SSPBUF = c;
    while (BF == 0);
    rc = SSPBUF; // also clears BF
    return rc;
}

void maxcomm(char *ival) {
    ival[1] = spi_comm(ival[1]);
    ival[0] = spi_comm(ival[0]);
}

void maxinit(char pch, char iset) {
    /* This code changes a bit -- now set demux to correct channel, then toggle demux
       enable to toggle max enable pin.
       This code could change again for another board version with more than 8 ports! */
    char ival[2];
    PORTB = (PORTB & 0xF8) | pch;
    PORTB &= 0xF7;
    ival[1] = 0xC0;
    ival[0] = iset;
    maxcomm(ival);
    PORTB |= 0x08;
    PORTB &= 0xF7;
    ival[1] = 0x40;
    ival[0] = 0x00;
    maxcomm(ival);
    PORTB |= 0x08;
}

void maxread(char pch, char *ival) {
    PORTB = (PORTB & 0xF8) | pch;
    PORTB &= 0xF7;
    ival[1] = 0x00;
    ival[0] = 0x00;
    maxcomm(ival);
    PORTB |= 0x08;
}

void maxsend(char pch, char *ival) {
    PORTB = (PORTB & 0xF8) | pch;
    PORTB &= 0xF7;
    ival[1] = 0x80;
    maxcomm(ival);
    PORTB |= 0x08;
}

void main(void) {
    int i, k, ii, n, iv, mdelay, stuck, duty, defduty, maxduty;
    unsigned char pcountt;
    int current; /* real current, in mA (scaled by 64mA per A/D count) */
//    unsigned int eeadd = 0x00;
    char ival[2], c, ic;
    /* a temporary buffer to build ASCII from binary -- now larger for IMU */
    char tbuf[50];
    /* define one big buffer with multiple entry points (pib0) */
    /* pib0 has one last entry for testing length */
    int op, pib0i, pib[NCHAN], ntime1[NCHAN];
    const int pib0[9] = {0, 25, 55, 110, 150, 170, 240, 320, 340};
    char first;
    /* 9600=0x0B, 19200=0x0A, 38.4k=0X09  115.2K=0x01with 8N1 [this should be dim'd NCHAN, but set all 8] */
    const char baud[8] = {0x0B, 0x0B, 0x0B, 0x0B, 0x0B, 0x0A, 0x0B, 0x0B};
    const char term[8] = {'\r', '\n', '\r', '\r', '\r', '\0', '\r', '\r'};

    /* initialize misc */
    di();

    /* start with initializing PORTB that had odd interrupts
     * A desperate attempt to avoid PIC going into ozone on boot.
     * Didn't work, but jumpering RB5 to ground did! */
    LATB = 0x1F;
    PORTB = 0x1F; /* set RB0-3 high (not selecting max's) */
    TRISB = 0x20; /* Now set all except RB5 as output [RB0-4 are outputs (0xE0)] */
    RBPU = 1; /* disable port B pull-ups (only for inputs) */
    RBIP = 1; /* set RB4-7 ints as high priority (then will be ignored??) */

    SWDTEN = 0;
    first = 5;
    Iflag = 2; /* interrupt service will set this later */
    mdelay = 5;
    pcount = 0; /* start standing still! */
    /* initial trolley speed setting -- seems to be happy at 3m/s at 30V for the moment */
    /* read from eeprom and reset eeprom if wacky */
    /* 43 = 4m/s / 0.093; 32 = 3m/s; 27 = 2.5m/s */
 //   pcounts = eeprom_read(eeadd); // Set initially or subsequently changed with "!" command.
    pcounts = 27;
    stuck = 0;
    current = 0;
    rfpush = QBOT;
    rfpop = QBOT;
    for (i = 0; i < NCHAN; i++) pib[i] = pib0[i];
    for (i = 0; i < NCHAN; i++) ntime1[i] = 0;

    /* initialize UART: 115.2kb (32=38.4; 21=57.6; 10=113kb) at 20MHz clock (before 8/13/13)*/
    /* initialize UART: 115.2kb (29=38.4; 19=57.6; 9=115kb) at 18.432MHz clock (after 8/13/13)*/
    SPBRG = 9; /* was 10 for 20MHz */
    TXSTA = 0x26;
    SPEN = 1;
    CREN = 1;
    TXIP = 1; /* UART transmit is high priority interrupt */
    TXIE = 0; /* initially, disable interrupts from Tx */

    /* RESOURCES */
    /* RA0 - analog in Power voltage
       RA1 - analog in Power current
       RA2 - unused
       RA3 - unused
       RA4 - in 01A counter
       RA5 - out, unused
       RA6 - out Beeper
       RA7 - not available */

    /* RB0 - out UART select
       RB1 - out UART select
       RB2 - out UART select
       RB3 - out UART bank select
       RB4 - unused
       RB5 - unused
       RB6 - PGC [EPIC]
       RB7 - PGD [EPIC] */

    /* RC0 - unused
       RC1 - unused
       RC2 - out Motor speed control (PWM)
       RC3 - out SCK (SPI)
       RC4 - in SDI (SPI)
       RC5 - out SDO (SPI)
       RC6 - out TX (UART)
       RC7 - in RX (UART) */

    /* Timer0 - position counter (from RA4)
       Timer1 - 100ms timing, with CCP2 (for A/D conversion and position counter period)
       Timer2 - PWM input, with CCP1
       Timer3 - unused
       UART - main radio communication
       SPI - UART interface */

    /* Data output values (and other characteristics) are:
       ic   U	Device		Baud        Power   Mode        Rate
       0    U6	TCM (att)	9600        +12     232/U10-2   10
       1    U7	SHT		9600        +3.3    TTL3        2?
       2    U8	CO2	        9600        +12     232/U11-2   10 (settable)
       3    U9  Sonic          	9600        +12     232/U11-1   10 (settable)
       4    U2  -               9600        -       (TTL5)
       5    U3  IMU             115200->19200 +5    232/U10-1   10
       6    U4  GPS             9600        +3.3    TTL3        10 (settable)
       7    U5  PAR             9600        +3.3    TTL3        2?
       (Pin 17 on 34-pin feeds PPS from GPS to IMU)
     * (IMU is now baud rate down-converted using a second PIC)

       A/D channels are: (1024 counts for 3.3V Vref)
       8    A0      Voltage	R2=9.1K; R3=150K => 0.053 V/count
       9    A1      Current MAX4080FASA; 10mOhm gives 2.5V@50A => 0.064 A/count

       Position counter:
       A    A4      HI_ISR (pcount) 15 counts/rev; diam 44.5 mm; period = 0.1 s =>
                                    0.093 m/s per count @ 10 sps
       C            Echos commands entered manually
       D            Duty cycle
     */

    /* setup PortA/enable A/D */
    ADCON0 = 0x81; // Fosc/64; A/D on; (should be ready by time of GPS)
    ADCON1 = 0xC4; /* RA0/1/3 analog; Vref=Vdd=3.3V; MSB=0; rest digital */
    TRISA = 0x9B; /* RA2, RA5, and RA6 are output */
    PORTA = PORTA & 0x9F; /* set motor and beeper off */

    /* initialize SPI */
    PORTC = PORTC & 0xFB; /* force motor off (negative logic with RS232 driver!) */
    TRISC = 0x93; /* use both SPI and UART; C2 output */
    SSPCON1 = 0x21; /* SPI: enabled, idle low, master /16 */
    SSPSTAT = 0x40; /* SPI: middle rising edge */
    SSPIE = 0; /* disable interrupts from this */

    /* initialize max3111s */

    /* FOLLOW 2 LINES CRASH CODE -- WAIT until GPS is functioning to fix (6/2011) */
    /* 8/2011: GPS now functions, but why do I want PPS interrupts anyway? -- wired to IMU */
    //	INT0IE = 1; /* enable PPS interrupts */
    //	INTEDG0 = 1; /* PPS interrupt on rising edge [true?] */
    for (ic = 0; ic < NCHAN; ic++) {
        maxinit(ic, baud[ic]);
    }

    /* initialize RMT */
    /* ... send "go" to RMT CO2 after waiting a bit to stabilize */
    /* NOT NEEDED NOW THAT I'VE SET wait=5 with RMT "JB" COMMAND! */
    /* (if power drops, it should still reset itself) */
    CLRWDT();

    // delay for 5 seconds before turning motor on to allow power to settle [DelayMs is 2x slow]
    // Done before setting up timers to avoid odd timing artifacts.
    for (i = 0; i < 50; i++) {
        CLRWDT();
        DelayMs(200);
    }
    CLRWDT();

    /* set up interrupts */
    /* ...we're only interested in CCP for ms timing */
    IPEN = 1; // Use interrupt priority

    /* set up timer0 (used for 01A pulse counting) */
    T0CON = 0xA8; // on, 16 bit, RA4 input, L-H, no prescaler
    TMR0H = 0; // clear timer
    TMR0L = 0;
    /* set up timer1 and CCP2 (used for ms timing) */
    T1CON = 0xB1; // on, 16bit, FOSC/4/8
    CCP2CON = 0x0B; // Compare mode, special event clears Timer1 (and starts A/D)
    CCPR2H = 0xE1; // Period is 62500 => 100ms (20MHz), 57600 => 100ms (18.432MHz)
    CCPR2L = 0x00;
    CCP2IP = 1; // CCP interrupt is high priority
    CCP2IE = 1; // CCP interrupts enabled!
    TMR1H = 0; // clear timer
    TMR1L = 0;

    /* set up timer2 and CCP1 (used for PWM control) */
    CCP1CON = 0x3F; // PWM mode, duty "cycle" LSBs = 0x3

    PR2 = 0xFF; // want maximum (longest) period (this gives about 1ms)
    T2CON = 0x7F; // on, 16 prescale, 16 postscale (but postscale not used for PWM!)
    /* Have now wired to use RS232 driver which inverts duty cycle
       square wave, thus get 3/4 period by driving at 1/4 period.
       We'll implement this when setting the CCPR values. */
    CCPR1L = 0xBF; // want initial duty "cycle" 3/4 of period
    defduty = 0x02FF; // equivalent of bf<<2 + 03
    duty = defduty;
    maxduty = 0x03FC; // equivent of ff<<2 + 00
    CCP1IE = 0; // Don't have PWM interrupt -- just run itself!
    CCP1IP = 0; // Low priority interrupt if it ever occurred
    TMR2 = 0; // clear timer

    putch('C');
    putch(':');
    putch('!');
    putch(0x0a);

    ei();
 //   while (1) {putch('k'); DelayMs(200); CLRWDT();}

    while (1) {
        /* Check for command: (note that "cr" is not needed)
           S = Stop (motor off)
           G = Go (motor on)
           + = Increase speed setting
           - = Decrease speed setting
           C = Calibrate TCM [somewhat involved]
           c = Stop TCM calibration
           R = Start CO2 (rarely needed now that I figured out correct setting!)
           0 = Set XBee radio power low (lab testing)
           4 = Set XBee radio power high (in field)
         */
        if (RCIF) {
            c = RCREG;
            CREN = 0;
            CREN = 1; // clear any reception errors??
            if (c == 'S') {// Stop -- motor off
                duty = 0;
                CCPR1L = duty >> 2;
                CCP1CON = CCP1CON | ((duty & 0x03) << 4);
            }
            if (c == 'G') {// Go -- motor on
                duty = defduty;
                CCPR1L = duty >> 2;
                CCP1CON = CCP1CON | ((duty & 0x03) << 4);
                stuck = 0;
            }
            if (c == '+') { // Increase speed setpoint
                pcounts++;
            }
            if (c == '-') { // Decrease speed setpoint
                pcounts--;
            }
            if (c == '?') { // Echo present speed setpoint
                itox4b((int)pcounts, tbuf); // form and send counter message (send as X4)
                putch(tbuf[0]);
                putch(tbuf[1]);
                putch(tbuf[2]);
                putch(tbuf[3]);
            }
//            if (c == '!') { // Save present speed setpoint
//                eeprom_write(eeadd,pcounts);
//            }
            if (c == 'C') { // Calibrate TCM
                ic = 2;
                DelayMs(50);
                ival[0] = 'c';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'c';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = '\r';
                maxsend(ic, ival);
                DelayMs(200);
                ival[0] = 'm';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'p';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'c';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'a';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'l';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = '=';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'e';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = '\r';
                maxsend(ic, ival);
                DelayMs(200);
                ival[0] = 'g';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'o';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = '\r';
                maxsend(ic, ival);
            }
            if (c == 'c') { // Stop calibrating TCM
                ic = 2;
                DelayMs(50);
                ival[0] = 'h';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = '\r';
                maxsend(ic, ival);
            }
            if (c == 'R') { /* send "go" to RMT CO2 [in case it doesn't] */
                ic = 2;
                DelayMs(50);
                ival[0] = '\r';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'g';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = 'o';
                maxsend(ic, ival);
                DelayMs(50);
                ival[0] = '\r';
                maxsend(ic, ival);
            }
            if ((c == '0') || (c == '4')) { /* set XbPro power level */
                /* send 3 pluses and wait 2 sec (OK comes back) */
                putch('+');
                putch('+');
                putch('+');
                CLRWDT();
                DelayMs(250);
                DelayMs(250);
                DelayMs(250);
                DelayMs(250);
                CLRWDT();
                DelayMs(250);
                DelayMs(250);
                DelayMs(250);
                DelayMs(250);
                CLRWDT();
                putch('A');
                putch('T');
                putch('P');
                putch('L');
                putch(c);
                putch('\r');
                DelayMs(100);
                CLRWDT();
                putch('A');
                putch('T');
                putch('W');
                putch('R');
                putch('\r');
                CLRWDT();
            }
            /* echo command to data stream for archival */
            rfsend(0x0C, 0, 1, &c);
            c = 0x00;
        }

        //SERIAL PORTS
        for (i = 0; i < NCHAN; i++) {
            ic = (char) i;
            maxread(ic, ival);
            while ((ival[1] & 0x80) != 0) {
                pib0i = pib0[i];
                if (i == 5) { /* process IMU binary data separately */
                    if (pbuf[pib0i] != 0x55) { // first byte must be 55 -- if not...
                        pib[i] = pib0i; // ...reset pointer to beginning of buffer
                        /* note that below set is ONLY done if first byte isn't 55 already, thus allowing data byte value of 55 */
                        if (ival[0] == 0x55) {
                            pbuf[pib[i]++] = ival[0];
                        } // ...if current byte is 55, save it and go on
                    } else {
                        if (pib[i] == pib0i + 1) { // second byte must be AA
                            if (ival[0] == 0xAA) { // ...if so, save it and go on
                                pbuf[pib[i]++] = ival[0];
                            } else { // ...if not, restart search
                                pib[i] = pib0i;
                                pbuf[pib[i]] = 0x00;
                            }
                        } else { // By now, first 2 characters should be 55AA
                            pbuf[pib[i]++] = ival[0]; // ...add next byte to buffer
                            if (pib[i] >= pib0[i + 1]) {
                                pib[i] = pib0i;
                                pbuf[pib[i]] = 0x00;
                            } // ...but if buffer full, restart process
                        }
                    }
                    if ((pib[i] - pib0i) >= (4 + pbuf[pib0i + 3])) { /* if all data characters, format and copy to send buffer */
                        /* build output data message */
                        CLRWDT();
                        op = pib0i + 2;
                        itox2b(pbuf[op++], tbuf); // first data byte is packet ID
                        n = pbuf[op++]; // grab packet data length
                        itox2b(n, tbuf + 2); // TEST -- write packet length
                        for (k = 0; k < n; k++) itox2b(pbuf[op++], tbuf + 4 + 2 * k); // stuff each packet data byte
                        if (ntime1[i]==1) rfsend(ic, 0, 4 + 2 * n, tbuf); else ntime1[i] = 1; // send out (ignore CRC)
                        pib[i] = pib0i;
                        pbuf[pib0i] = 0x00; // clear out first 2 message bytes to restart loading
                        pbuf[pib0i + 1] = 0x00;
                    }
                } else { /* normal, ASCII, data feeds */
                    if (term[i] != '\0') { // normal code for ASCII data terminated by \r, \n
                        if ((ival[0] != '\r') && (ival[0] != '\n')) { /* fill buffer until end of message */
                            pbuf[pib[i]++] = ival[0];
                            if (pib[i] >= pib0[i + 1]) pib[i] = pib0i;
                        } else {
                            if (ival[0] == term[i]) { /* if end of message, copy to send buffer */
                                if (ntime1[i]==1) rfsend(ic, pib0i, pib[i], pbuf); else ntime1[i] = 1;
                                pib[i] = pib0i;
                            }
                        }
                    }
                }
                maxread(ic, ival);
                CLRWDT();
            }
        }

        //SEND position counter (was read in ISR)
        if (pcount != (255)) {
            pcountt = pcount; // ...just to be paranoid, reset pcount for next interrupt
            pcount = 255;
            // ...this is the John-inspired to shutdown power if stuck!
            // ...now, can be even more interesting with speed control...
            if (duty > 0) {
                if (pcountt == 0) stuck += 1;
                else {
                    stuck = 0;
                    // ... following values of 4/8 are somewhat arbitrary -- will need to test
                    // ... idea is: "If speed changes by XX%, change voltage by XX%."
                    // ... range of pcount is 0 to 200 during NIWOT09, range of duty is 0 to 0x3FF=1023
                    // ... thus, duty should change 5 times faster than pcount
                    // ... also diff(pcount) of 4 is 2% of full scale = change duty by 20
                    // ... NIWOT09 data show change of 0,1,2,3 most common.
                    // ... The following implements: "if speed changes by 10m/s, change duty by 100% in 1second (10 samples)"
                    duty += 4 * (unsigned int)(pcounts - pcountt);
                    // ... Even with this, found that current can get too high, so add additional tests
                    // ... Note that the old CMX relay had a 10A limit, though I replaced it with a 20A version
                    // ... Also note that the CMX is only motor current, whereas the "current" reading is entire trolley
                    // ... (including sensors and data system).  The difference is small enough to ignore in this case.
                    // ... Would be a good idea to set the Lambda Supply current limit to something reasonable as well.
                    if (current > 12000) duty -= 128; // severely reduce if current large
                    if (current > 15000) duty = 0; // and turn off completely if current > 10A
                }
                if (duty < 5) duty = 5; // limit check after above steps (but set above 0 so won't think stuck off)
                if (duty > maxduty) duty = maxduty;
                if (stuck > 20) { // 10 = 1second
                    duty = 0;
                    stuck = 0; // allows "hot" restart
                }
                CCPR1L = duty >> 2;
                CCP1CON = CCP1CON | ((duty & 0x03) << 4);
            }
            // ...now back to regular processing...
            itox4b((int)pcountt, tbuf); // form and send counter message (send as X4)
            rfsend(0x0A, 0, 4, tbuf);
            Iflag = 0;

            /* also send duty cycle while we're here */
            itox4b(duty, tbuf);
            rfsend(0x0D, 0, 4, tbuf);
            /* hopefully, more than 13us has elapsed (since interrupt), so start A/D */
            GODONE = 1;
        }

        //GET A/D readings and restart
        if ((Iflag < 2) && (GODONE == 0)) {
            /* grab A/D value */
            iv = (ADRESH << 8) | ADRESL;
            ADCON0 = 0x89; // set up for I (even if just done...)
            itox4b(iv, tbuf); // format and send value
            rfsend(8 + Iflag, 0, 4, tbuf);
            if (Iflag == 1) current = iv * 64; // use real current reading (integer in mA)
            Iflag++;
            if (Iflag < 2) GODONE = 1; // start next conversion
        }
        CLRWDT(); /* reset COP */
    }
}