/*
 2piLineFollower.c - Mark Moran 2014
 Redistribution and use in source and binary forms, with or without
 modification, are permitted freely.
 Inspired by Pololu's QTRSensors & 3pi Line Follower apps written by Ben Schmidel et al.
 Written for ATmega328p running about 8MHz with internal RC time source.
 (Should work with any AVR chip with 16bit & 8bit timers and enough pins)
 Will work with external clock source but may need to change clock scale prefactor.
 Drives two motors connected to SN754410 quad H-bridge.
 Detects lines using Pololu QTR-8RC chip (with sensors 7 & 8 removed)
 */

#include <avr/io.h> 
#include <util/delay.h>


/*************** MOTOR FUNCTIONS ***************/

#define MOTORPORT   PORTD
#define MOTORDDR    DDRD
#define PIN1A       PD3
#define PIN2A       PD2
#define PIN3A       PD7
#define PIN4A       PD4
#define PIN12EN     PD5
#define PIN34EN     PD6
#define PIN12CNTR   OCR0B
#define PIN34CNTR   OCR0A

#define LEFTA       PIN4A
#define LEFTB       PIN3A
#define LEFTON      PIN34EN
#define LEFTCNTR    PIN34CNTR
#define LEFTBIT     COM0A1
#define RIGHTA      PIN1A
#define RIGHTB      PIN2A
#define RIGHTON     PIN12EN
#define RIGHTCNTR   PIN12CNTR
#define RIGHTBIT    COM0B1


static inline void initMotors(void) {
    TCCR0A = (1<<WGM00) | (1<<WGM01);                  //set Timer0 to fast PWM
    TCCR0B = (1<<CS00) | (1<<CS01);                    //clock at CPU/64 frequency (488Hz)
    
    // set all 6 motor  pins to outputs
    MOTORDDR |= (1<<PIN1A) | (1<<PIN2A) | (1<<PIN3A) | (1<<PIN4A) | (1<<PIN12EN) | (1<<PIN34EN);
}

static inline void setLeftSpeed(int16_t speed) {
    uint8_t reverse = 0;
    if (speed < 0) {
        speed = -speed;
        reverse = 1;
    }
    if (speed > 255)
        speed = 255;
    
    if (speed == 0)
        TCCR0A &= ~(1<<LEFTBIT);                    // disconnect timer from left enable pin
    else {
        TCCR0A |= (1<<LEFTBIT);                     // re-connect timer to left enable pin
        LEFTCNTR = speed;
        if (reverse) {
            MOTORPORT |= (1<<LEFTB);
            MOTORPORT &= ~(1<<LEFTA);
        }
        else {
            MOTORPORT |= (1<<LEFTA);
            MOTORPORT &= ~(1<<LEFTB);
        }
    }
}

static inline void setRightSpeed(int16_t speed) {
    uint8_t reverse = 0;
    if (speed < 0) {
        speed = -speed;
        reverse = 1;
    }
    if (speed > 255)
        speed = 255;
    
    if (speed == 0)
        TCCR0A &= ~(1<<RIGHTBIT);                     // disconnect timer from right enable pin
    else {
        TCCR0A |= (1<<RIGHTBIT);                      // recoonnect timer to right enable pin
        RIGHTCNTR = speed;
        if (reverse) {
            MOTORPORT |= (1<<RIGHTB);
            MOTORPORT &= ~(1<<RIGHTA);
        }
        else {
            MOTORPORT |= (1<<RIGHTA);
            MOTORPORT &= ~(1<<RIGHTB);
        }
    }
}

static inline void setSpeed(int16_t leftSpeed, int16_t rightSpeed) {
    setLeftSpeed(leftSpeed);
    setRightSpeed(rightSpeed);
}

static inline void brake(void) {
    // hard brake by setting both motor sides high (or low)
    MOTORPORT |= (1<<LEFTA) | (1<<LEFTB) | (1<<RIGHTA) | (1<<RIGHTB);
}


/*************** QTR FUNCTIONS ***************/

#define READPORT    PORTC
#define READDDR     DDRC
#define READPIN     PINC        // lines connected to PC0 - PC5

#define LEDPORT     PORTB       // optionally turn off IR LEDs to save energy
#define LEDDDR      DDRB
#define PINLEDON    PB5

#define SLOW        1
#define FAST        0

#define MAXTICKS    2500
#define QTRCNT      6



static inline void initQTRs(void) {
    TCCR1B = (1<<CS11);                             // set Timer1 to /8 prescaling
    
    LEDDDR |= (1<<PINLEDON);                        // enable LED pin as output
    LEDPORT |= (1<<PINLEDON);                       // currently keep LEDs on all the time
}

uint16_t QTRtime[QTRCNT], QTRmax[QTRCNT], QTRmin[QTRCNT];

static inline void readQTRs(uint8_t forceSlow) {
    uint8_t lastPin, i, done = 0;
    
    for (i = 0; i < QTRCNT; i++)                    // clear out previous times
        QTRtime[i] = 0;
    
    READDDR |= 0b00111111;                          // set pins to output
    READPORT |= 0b00111111;                         // drive them high
    
    _delay_us(10);                                  // wait 10us to charge capacitors
    
    READDDR &= 0b11000000;                          // set pins to input
    READPORT &= 0b11000000;                         // turn off pull-up registers
    
    TCNT1 = 0;                                      // start 16bit timer at 0
    lastPin = READPIN;
    
    while ((TCNT1 < MAXTICKS) && ((done < QTRCNT) || forceSlow))     // if forceSlow, always take MAXTICKS time
    {
        if (lastPin != READPIN)                     // if any of the pins changed
        {
            lastPin = READPIN;
            for (i = 0; i < QTRCNT; i++)
            {
                if ((QTRtime[i] == 0) && (!(lastPin & (1<<i))))    // did pin go low for the first time
                {
                    QTRtime[i] = TCNT1;
                    done++;
                }
            }
        }
    }
    if (done < QTRCNT)                              // if we timed out, set any pins that didn't go low to max
        for (i = 0; i < QTRCNT; i++)
            if (QTRtime[i] == 0)
                QTRtime[i] = MAXTICKS;
}

void calibrateQTRs(void) {
    uint8_t i, j;

    for (j = 0; j < 10; j++) {                      // take 10 readings and find min and max values
        readQTRs(SLOW);
        for (i = 0; i < QTRCNT; i++) {
            if (QTRtime[i] > QTRmax[i])
                QTRmax[i] = QTRtime[i];
            if (QTRtime[i] < QTRmin[i])
                QTRmin[i] = QTRtime[i];
        }
    }
}

void readCalibrated(void) {
    uint8_t i;
    uint16_t range;
    
    readQTRs(FAST);
    
    for (i = 0; i < QTRCNT; i++) {                  // normalize readings 0-1000 relative to min & max
        if (QTRtime[i] < QTRmin[i])                 // check if reading is within calibrated reading
            QTRtime[i] = 0;
        else if (QTRtime[i] > QTRmax[i])
            QTRtime[i] = 1000;
        else {
            range = QTRmax[i] - QTRmin[i];
            if (!range)                             // avoid div by zero if min & max are equal (broken sensor)
                QTRtime[i] = 0;
            else
                QTRtime[i] = ((int32_t)(QTRtime[i]) - QTRmin[i]) * 1000 / range;
        }
    }
}

uint16_t readLine(void) {
    uint8_t i, onLine = 0;
    uint32_t avg;                                   // weighted total, long before division
    uint16_t sum;                                   // total values (used for division)
    static uint16_t lastValue = 0;                  // assume line is initially all the way left (arbitrary)
    
    readCalibrated();
    
    avg = 0;
    sum = 0;
    
    for (i = 0; i < QTRCNT; i++) {                  // if following white line, set QTRtime[i] = 1000 - QTRtime[i]
        if (QTRtime[i] > 50) {                      // only average in values that are above a noise threshold
            avg += (uint32_t)(QTRtime[i]) * (i * 1000);
            sum += QTRtime[i];
            if (QTRtime[i] > 200)                   // see if we're above the line
                onLine = 1;
        }
    }
    
    if (!onLine) {
        if (lastValue < ((QTRCNT-1)*1000/2))        // if last read right of center, return 0 (assume veered right)
            return 0;
        else                                        // otherwise return max (assume veered left)
            return (QTRCNT-1)*1000;
    }
    
    lastValue = avg/sum;                            // no chance of div by zero since onLine was true
    
    return lastValue;
}

void spinAndCalibrate(void) {
    uint8_t i;
    
    for (i = 0; i < QTRCNT; i++) {                  // reset minimums and maximums
        QTRmax[i] = 0;
        QTRmin[i] = MAXTICKS;
    }
    
    for(i = 0; i < 60; i++) {
        if ((i < 15) || (i >= 45))
            setSpeed(40, -40);
        else
            setSpeed(-40, 40);
        
        calibrateQTRs();
        _delay_ms(5);
    }
    setSpeed(0,0);
}

#define MAX 60

int main(void) {
    int16_t proportional, derivative, lastProportional = 0, powerDiff;
    uint16_t position;
    uint32_t integral = 0;
    uint8_t i;
    
    initMotors();
    initQTRs();
    
     _delay_ms(1000);                                	// pull hand away from switch
    
    spinAndCalibrate();
 
    while (1) {
        position = readLine();                          // 0 = far right, 5000 = far left
        
        proportional = ((int16_t)position) - 2500;      // "proportional" term should be 0 when we on the line
        derivative = proportional - lastProportional;   // change of position
        integral += proportional;                       // sum of positions
        lastProportional = proportional;                // remember the last position
        
        powerDiff = proportional/20 + integral/10000 + derivative*3/2;
        
        if (powerDiff > MAX)
            powerDiff = MAX;
        else if (powerDiff < -MAX)
            powerDiff = -MAX;
        
        if (powerDiff < 0)
            setSpeed(MAX, MAX+powerDiff);
        else
            setSpeed(MAX-powerDiff, MAX);
    }
    return 0;
}