/*
    Main program for LEGO car control
    Author: Tobias Åkesson
    date / version : 2002-04-25 
*/

#include "io6811.h"
#include "Vectors.h"
#include "intr6811.h"

/* CONSTANTS */

#define P               16      // for use with steering control
#define STEERING_LEFT   0x00    // Left turn output  (OC4) setValue: < 0 OLD: 0x01
#define STEERING_RIGHT  0x80    // --- right ---     (OC5) setValue > 0 OLD: 0x04
#define STEERING_MASK   0xFF    // Steering mask: bit0:FF bit2:RW   OLD:0x05
#define DRIVE_MASK      0xFF    // Lego drive motor OLD:0x0A
#define DRIVE_FF        0x00    // Forward direction (OC2)    OLD:0x08
#define DRIVE_RW        0x08    // Backward direction (OC3)  OLD:0x02
#define SCALE           7       // Engine control resolution , for now: 3 bit
#define MATCH_COUNT     2       // # of similair bytes that have to match

#define charAbs(x)      ((x < 0)?~x:x)    // Absolute for signed int

/* Static variables */

    // STEERING
    signed   char   steeringSetValue = 0;           // Input from SCI
    unsigned char   steeringMeassureValue = 128;    // from potentiometer (A/D)

    // DRIVE
    signed   char   driveSetValue = 0;              // From SCI
    
    // SCI
    unsigned char   lastRecieved[MATCH_COUNT];            // last value from SCI
    unsigned char   currentByte = 0;
    // MISC
    unsigned int tmp = 0;

/*
    Handles incomping data from serial device
    
    - Sets value to steering and speed
    - Only handles one byte at a time
    
    - LSB:0 DRIVE ; LSB:1 STEERING --> 7 bit resolution
*/
interrupt void SCI_interrupt(void) {        
    char tmp = SCSR;

    //for matching
    char match = 1;
    char i;
    
    if (tmp & 0x20) { // is it a read
        tmp = SCDR;

        for (i=0;i < MATCH_COUNT; i++)  // Match ..
            if (lastRecieved[i] != tmp)  match = 0;

        if (match) {     // "error check" NBR identical bytes ...
            steeringSetValue = (tmp & 0x0F) << 4;   // steer on bit 0-3
            driveSetValue = (tmp & 0xF0);           // drive 4-7
        }    
        lastRecieved[++currentByte % MATCH_COUNT] = tmp;    // Circular register
    }                    
}

/*
    Main car control, P regulation of steering
    - Convert set Values to I/O
    - Steering with P regulator and feedback from potentiometer
    - Timing 372 clk.
*/

void control(void) {
    int diff;
    char driveMask,steeringMask; // mask for switching OC2-5
    char drivePulseLength, steeringPulseLength;

    // DRIVE
    driveMask = (driveSetValue < 0)?DRIVE_RW:DRIVE_FF;    
    
    drivePulseLength = (char) (charAbs(driveSetValue) >> 4);    // Just shrink 7 --> 3 bit
    
    // STEERING
    // left: <0 right: >0 -- steering in opposite direction ...
    diff = steeringSetValue - (int)(steeringMeassureValue - 70); // Ändrat av Kristoffer
    steeringMask = (diff < 0)?STEERING_LEFT:STEERING_RIGHT;
    
    // proportional regulation.
    // from diff to counterSet
    steeringPulseLength = (char) (charAbs(diff) >> 2);  // Shrink 7 --> 5 bit
    steeringPulseLength = (steeringPulseLength > SCALE)?SCALE:steeringPulseLength;
    

    // Send data to Lattice circuit
    PORTB = steeringMask | (steeringPulseLength << 4) | driveMask | drivePulseLength;
}

/*
    Sets up ports and properties
*/
void initialize(void) {

  // Sets up I/O ports
    PACTL = 0x88;   // PORTA output (H-bridge)
    PORTA = 0x00;   // PORTA = 0    

    PORTB = 0x00;   // Just in case, defined output

    DDRC  = 0xFF;   // PORTC is output 
    PORTC = 0x00;   // PORTC = 0

    DDRD  = 0x06; // PORTD pin 2 set up as output -
    PORTD = 0x00; // - wired to TX-enable on transciever

    // SCI comm    
    BAUD  = 0x30; // 9600 bps
    SCCR2 = 0x24; // Set up for reading
    
    // A/D converter set up
    ADCTL = 0x20;   // Automatic restart, one port E0
    OPTION = 0x80;  // A/D voltage set
}

/* Aquire data from A/D converter */
void aquire(void) {
    ADCTL = 0x00;                       // Single scan strobe
    while((ADCTL & 0x80) != 0x80);      // wait ~10 microsec for conversion
    steeringMeassureValue = ADR1;       // Read from A/DR1
                                        // Convert to linear scale
}

/* Initialization & main loop */
void main(void) {
    initialize();
    enable_interrupt();
    while(1) {
        aquire();   // A/D Conversion
        control();  // Main control loop / P regulator
    }
}