File content as of revision 12:eab1bc88fe68:

#include "mbed.h"
#include "TextLCD.h"
#include "MCP23S17.h"
//#include "24LCxx_I2C.h"


#define FACTOR 1

#define SENSOR_ADDR (41 << 1)
#define SENSOR_WHITE_REG 148
#define SENSOR_RED_REG 150
#define SENSOR_GREEN_REG 152
#define SENSOR_BLUE_REG 154

#define WHITE_THRESHOLD 15

//--------------------------------------
//each LED type is defined here
#define NOTUSED    0
#define STD_RED    1
#define STD_GREEN  2
#define STD_BLUE   3
#define STD_YELLOW 4
#define GREEN_YELLOW 5
#define RGB        6


//--------------------------------------
//Current LED Colour Definitions
#define ERROR   0       //off or wrong
#define RED     1
#define GREEN   2
#define BLUE    3
#define YELLOW  4



//--------------------------------------
#define NEWBUTTON   0x80
#define UPBUTTON    0x04
#define DOWNBUTTON  0x01
#define LEFTBUTTON  0x03
#define RIGHTBUTTON 0x02
#define TICKBUTTON  0x05

void SelectSensorPort(uint8_t SensorID);
void CreateOutputTable(void);
void SaveSensorData(void);

//-------------------------------------------------------------------------------
//----------- LED's -------------------------------------------------------------
//-------------------------------------------------------------------------------
// Detection LED's use the onboard MBED LED's
// These are all blue on the MBED but on DSE PCB they areas follows
// LED1 = GREEN
// LED2 = BLUE
// LED3 = RED
// LED4 = YELLOW
DigitalOut GreenLed(LED1);
DigitalOut BlueLed(LED2);
DigitalOut RedLed(LED3);
DigitalOut YellowLed(LED4);

//I2C address selection pins
DigitalOut I2C_ID_Bit0(p13);
DigitalOut I2C_ID_Bit1(p14);
DigitalOut I2C_ID_Bit2(p12);
DigitalOut I2C_ID_Bit3(p11);

//mode switch
DigitalIn ModeSwitch1(p19);
DigitalIn ModeSwitch2(p29);
DigitalIn ModeSwitch3(p30);

//buttons
DigitalIn Button1(p22);
DigitalIn Button2(p23);
DigitalIn Button3(p24);
DigitalIn Button4(p8);

//-------------------------------------------------------------------------------

//-------------------------------------------------------------------------------
//----------- Analogue Inputs ---------------------------------------------------
//-------------------------------------------------------------------------------
//Analogue inputs are used to set thresholds for detector levels
//AnalogIn RedTrimmer(p15);
//AnalogIn GreenTrimmer(p16);
//AnalogIn BlueTrimmer(p17);
//AnalogIn HysTrimmer(p18);

AnalogIn   Trimmer1(p15);
AnalogIn   Trimmer2(p16); 
AnalogIn   Trimmer3(p17);

//-----------------------------------------------------

//-------------------------------------------------------------------------------
//------------ Text display -----------------------------------------------------
//-------------------------------------------------------------------------------
//debug display. Standard display driver set up in 4 bit mode
//final version uses I2C port
//TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2); // rs, e, d4-d7

//set up I2C Communication to LCD
I2C i2c_lcd(p9,p10); // SDA, SCL

//I2C Portexpander PCF8574 for LCD
TextLCD_I2C lcd(&i2c_lcd, 0x40, TextLCD::LCD16x2); // I2C bus, PCF8574 Slaveaddress, LCD Type ok

//-------------------------------------------------------------------------------

//-------------------------------------------------------------------------------
//------------ I2C EEProm   -----------------------------------------------------
//-------------------------------------------------------------------------------

//C24LCXX_I2C ConfigEEPROM(p9, p10, 0x01, p11, 400000); // Create an instance of the class C2424LCXX_I2C, p28: SDA, p29:SDL, p5: wired to WP input of 24LCxx, address: A3=0,A2=0,A=1, on 3.3V I2C bus
//C24LCXX_I2C mem(p13, p14, 00000100, NC , 400000); 



//-------------------------------------------------------------------------------
//------------ Create SPI bus -----------------------------------------------------
//-------------------------------------------------------------------------------
//set up the SPI port for use as output driver
SPI spi(p5, p6, p7);

char Opcode = 0x40;
 
// Next create a MCP23S17
// mbed p20 is connected to ~chipSelect on the MCP23S17
MCP23S17 chip = MCP23S17(spi, p20, Opcode);

//-------------------------------------------------------------------------------

//-------------------------------------------------------------------------------
//------------ colour sensor Comms Pins -----------------------------------------
//-------------------------------------------------------------------------------
//TMC Sensor: this is the raw I2C Port, the clock like is muxed
I2C i2c(p28, p27); //pins for I2C communication (SDA, SCL)


typedef struct{
    uint16_t White;
    uint16_t Red;
    uint16_t Green;
    uint16_t Blue;
}Colour;

Colour SensorData;


//-------------------------------------------------------------------------------
//------------ Serial Port for RS485 -----------------------------------------
//-------------------------------------------------------------------------------
Serial RS485(p26,p25);
//DigitalOut RS485DIR(p21);
char SendMessage = 0;
char CommsData = 0;


//-------------------------------------------------------------------------------
//The system looks up the type of LED that is being detected for Each sensor and will fill in the result table & update the associated Output list
//outputs start at 0 torough to 15
/*
char Config[32]={   //RGB,    0,  //sensor 0 , uses 3 outputs  - 0,1,2
                    GREEN_YELLOW,   0,  //sensor 0 , uses 2 outputs  - 0,1
                    NOTUSED,        2,  //sensor 1 , uses 2 outputs  - 2,3
                    NOTUSED,        4,  //sensor 2 , uses 1 outputs  - 4
                    NOTUSED,        5,  //sensor 3 , uses 1 outputs  - 5
                    NOTUSED,        6,  //sensor 4                    
                    NOTUSED,        7,  //sensor 5
                    NOTUSED,        8,  //sensor 6
                    NOTUSED,        9,  //sensor 7
                    NOTUSED,        10,  //sensor 8
                    NOTUSED,        11,  //sensor 9
                    NOTUSED,        12,  //sensor 10
                    NOTUSED,        13,  //sensor 11
                    NOTUSED,        14,  //sensor 12
                    NOTUSED,        15,  //sensor 13
                    NOTUSED,        15,  //sensor 14
                    NOTUSED,        15,  //sensor 15
                };
*/
/*                
char Config[32]={   //RGB,    0,  //sensor 0 , uses 3 outputs  - 0,1,2
                    GREEN_YELLOW,   0,  //sensor 0 , uses 2 outputs  - 0,1
                    GREEN_YELLOW,   2,  //sensor 1 , uses 2 outputs  - 2,3
                    STD_RED,        4,  //sensor 2 , uses 1 outputs  - 4
                    STD_RED,        5,  //sensor 3 , uses 1 outputs  - 5
                    NOTUSED,        6,  //sensor 4                    
                    NOTUSED,        7,  //sensor 5
                    NOTUSED,        8,  //sensor 6
                    NOTUSED,        9,  //sensor 7
                    NOTUSED,        10,  //sensor 8
                    NOTUSED,        11,  //sensor 9
                    NOTUSED,        12,  //sensor 10
                    NOTUSED,        13,  //sensor 11
                    NOTUSED,        14,  //sensor 12
                    NOTUSED,        15,  //sensor 13
                    NOTUSED,        15,  //sensor 14
                    NOTUSED,        15,  //sensor 15
                };
*/

char Config[32]={   //RGB,    0,  //sensor 0 , uses 3 outputs  - 0,1,2
                    STD_RED,      0,  //sensor 0 , uses 3 outputs  - 0,1,2
                    STD_GREEN,    1,  //sensor 1
                    STD_BLUE,      2,  //sensor 2
                    STD_YELLOW,      3,  //sensor 3
                    GREEN_YELLOW,      4,  //sensor 4                    
                    RGB,      5,  //sensor 5
                    STD_RED,      6,  //sensor 6
                    STD_GREEN,      7,  //sensor 7
                    STD_RED,      8,  //sensor 8
                    STD_RED,      9,  //sensor 9
                    STD_RED,      10,  //sensor 10
                    STD_RED,      11,  //sensor 11
                    STD_RED,      12,  //sensor 12
                    STD_RED,      13,  //sensor 13
                    STD_RED,      14,  //sensor 14
                    RGB,      15,  //sensor 15
                };

//new config trpe

//LED Output requirement :type is defined here
uint8_t OutputCount[7]={
                        0,  //NOTUSED
                        1,  //STD_RED
                        1,  //STD_GREEN
                        1,  //STD_BLUE
                        1,  //STD_YELLOW
                        2,  //GREEN_YELLOW
                        3}; //RGB


char LEDState[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; //current detected LED states

uint16_t OutputData = 0;    //word containing the current output states of the board
uint16_t OutputDataB = 0x8000;// test roll
 
uint8_t SensorNumber = 0;   //current sensor number
char SensorResult = 0;      //detected colour

uint8_t ConfigureSensor;

uint8_t DisplayScreen = 0;
uint8_t DisplaySensorType;
uint8_t DisplaySensorOutput = 0;
uint8_t ButtonState = 0;

char LEDTypes[7][6+1]={
                    "SPARE ", //0
                    "RED   ", //1
                    "GREEN ", //2
                    "BLUE  ", //3
                    "YELLOW", //4
                    "GN/YEL", //5
                    "RGB   "
                    }; //6


//------------TSC Stuff----------------
int sensor_addr = 41 << 1;

uint8_t SensorConfig = 0x30;
// bit  description              setting
//  0   SD,   0 = enabled             0
//  1   AF,   0 = Automode            0
//  2   TRIG, 0 = no trigrer          0
//  3   N/F,  0                       0
//  4   IT0   0                       0
//  5   IT1   1 = 160ms               1
//  6   IT2   0                       0
//  7   N/F   0                       0
//------------TSC Stuff----------------

uint16_t Red = 1;
uint16_t Green = 2;
uint16_t Blue = 3;
uint16_t White = 4;

uint32_t Temp = 0;

uint16_t RedProp = 0;
uint16_t GreenProp = 0;
uint16_t BlueProp = 0;



//working thresholds from pot inputs, scaled 0-100
uint16_t    RedThreshold    = 0;
uint16_t    GreenThreshold  = 0;
uint16_t    BlueThreshold   = 0;
uint16_t    Hysteresis      = 0;

    
uint16_t   SingleThreshold = 0;
uint16_t   MixRThreshold   = 0;
uint16_t   MixGThreshold   = 0;

//------------------------- filter routine if required -------------------------------
uint16_t Filter(uint32_t *Acc,uint16_t NewData, uint8_t factor)
{
    uint32_t Temp = 0;
    Temp = *Acc >> factor ;
    Temp = *Acc - Temp + NewData;
    *Acc = Temp;
    return  *Acc >> factor;
}


void ConfigureTSC(uint8_t Address)
{
    I2C i2c(p28, p27);          //pins for I2C communication (SDA, SCL)
    i2c.frequency(200000);

    for (ConfigureSensor = 0; ConfigureSensor < 16; ConfigureSensor++)
    {
        if((Config[ConfigureSensor <<1]) != NOTUSED)
        { 
            SelectSensorPort(ConfigureSensor); //set up H/W mux for this sensor
              
            char id_regval[1] = {146};
            char data[1] = {0};
            i2c.write(sensor_addr,id_regval,1, true);
            i2c.read(sensor_addr,data,1,false);         // Initialize color sensor
        
            char enable_register[2] = {128,3};          //enable the sensor
            i2c.write(sensor_addr,enable_register,2,false);
        
            char timing_register[2] = {0x81,0xC0};      //approx 100ms
            i2c.write(sensor_addr,timing_register,2,false);
        
            char control_register[2] = {0x8F,2};        //gain = x16
            i2c.write(sensor_addr,control_register,2,false);
        }
    }
}


Colour GetSensorData(char SensorNumber)
{
    Colour NewData;
    char AddrReg[1] = {0};
    char DataReg[5] ={0,0,0,0,0};


    SelectSensorPort(SensorNumber);
        
    NewData.White = 0;
    NewData.Red = 0;
    NewData.Green = 0;
    NewData.Blue = 0;

    AddrReg[0] = SENSOR_WHITE_REG;
    i2c.write(SENSOR_ADDR, AddrReg,1, true);
    i2c.read(SENSOR_ADDR,DataReg,2, false);
    NewData.White = ((int)DataReg[1] << 8) | DataReg[0];
 
    AddrReg[0] = SENSOR_RED_REG;
    i2c.write(SENSOR_ADDR, AddrReg,1, true);
    i2c.read(SENSOR_ADDR,DataReg,2, false);
    NewData.Red = ((int)DataReg[1] << 8) | DataReg[0];
    
    AddrReg[0] = SENSOR_GREEN_REG;
    i2c.write(SENSOR_ADDR, AddrReg,1, true);
    i2c.read(SENSOR_ADDR,DataReg,2, false);
    NewData.Green = ((int)DataReg[1] << 8) | DataReg[0];

    AddrReg[0] = SENSOR_BLUE_REG;
    i2c.write(SENSOR_ADDR, AddrReg,1, true);
    i2c.read(SENSOR_ADDR,DataReg,2, false);
    NewData.Blue = ((int)DataReg[1] << 8) | DataReg[0];
          
    return NewData;
}

uint8_t GetModeSwitch(void)
{
    uint8_t Result = 0;
 
    if (!ModeSwitch1)
    { 
        Result += 1;
    } 
    
    if (!ModeSwitch2)
    { 
        Result += 2;
    }     
    
    if (!ModeSwitch3)
    { 
        Result += 4;
    }     
    
    return Result;       
}


uint8_t GetButtonState(void)
{
    uint8_t ButtonStateNew = 0;
    if (Button1 == 1)
    {
        ButtonStateNew = 1;
    }
    
    if (Button2 == 1)
    {
        ButtonStateNew = 2;
    }
    
    if (Button3 == 1)
    {
        ButtonStateNew = 3;
    }
    if (Button4 == 1)
    {
        ButtonStateNew = 4;
    }
    
    if(ButtonStateNew != 0)
    {
        if(ButtonState == 0)
        {
            ButtonState =  (ButtonStateNew | NEWBUTTON); 
        }
        else
        {
          ButtonState =  ButtonStateNew;   
        }
    }
    else
    {
        ButtonState = 0;
    }
    
    
    return ButtonState;
}


void InitOutputs(void)
{
    //  Set all 8 Port A bits to output direction
    chip.direction(PORT_A, 0x00);
    //  Set all 8 Port B bits to output direction
    chip.direction(PORT_B, 0x00); 
}

void SetOutputBit(char bit)
{
    uint16_t OutputWord = 1;
//    char BitA, BitB, PortA, PortB;
    
    OutputWord <<= bit;
    OutputData |= OutputWord;
    
    //BitA = OutputWord & 0xFF;
    //BitB = OutputWord >> 8;
    
    //PortA = chip.read(PORT_A);
    //PortA |=BitA;
    //chip.write(PORT_A, PortA);
    
    //PortB = chip.read(PORT_B);
    //PortB |=BitB;
    //chip.write(PORT_B, PortB);   
}

void ClearOutputBit(char bit)
{
    uint16_t OutputWord = 1;
//    char BitA, BitB, PortA, PortB;
    
    OutputWord <<= bit;
    OutputWord = ~OutputWord;
    OutputWord &= OutputWord;
        
    //BitA = OutputWord & 0xFF;
    //BitB = OutputWord >> 8;
    
    //PortA = chip.read(PORT_A);
    //PortA &=BitA;
    //chip.write(PORT_A, PortA);
    
    //PortB = chip.read(PORT_B);
    //PortB &=BitB;
    //chip.write(PORT_B, PortB);   
}

void WriteOutputPort(uint16_t FullPort)
{
    chip.write(PORT_A, FullPort & 0xFF); 
    chip.write(PORT_B, FullPort >> 8);     
}


void GetThresholds(void)
{
    //Load the pot values ant make 0 to 100%  value 
    
    SingleThreshold = 50 - ( Trimmer1.read_u16()/1310 );
    MixRThreshold   = 50 - ( Trimmer2.read_u16()  /1310 );
    MixGThreshold   = 50 - ( Trimmer3.read_u16()  /1310 );
    //Hysteresis      = HysTrimmer.read_u16()   * 100 >> 11;      

    
    RedThreshold    = 45; //default for yellow detect (red)
    GreenThreshold  = 35;//default for yellow detect (green)
    BlueThreshold   = 10;//not used
    Hysteresis      = 5;
}

void SelectSensorPort(uint8_t SensorID)
{
  
    if ( SensorID < 16)
    {
        if((SensorID & 0x01) == 0x01)
        {
            I2C_ID_Bit0 = 1;
        }
        else
        {   
            I2C_ID_Bit0 = 0;
        }
        
        if((SensorID & 0x02) == 0x02)
        {
            I2C_ID_Bit1 = 1;
        }
        else
        {   
            I2C_ID_Bit1 = 0;
        }
        
        if((SensorID & 0x04) == 0x04)
        {
            I2C_ID_Bit2 = 1;
        }
        else
        {   
            I2C_ID_Bit2 = 0;
        } 
        
        if((SensorID & 0x08) == 0x08)
        {
            I2C_ID_Bit3 = 1;
        }
        else
        {   
            I2C_ID_Bit3 = 0;
        }      
    } 
}

void ScaleColours(void)
{
     //make 12 bit only
    Red >>=4;
    Green >>=4;
    Blue >>=4;
    White >>=4;   
}


void UpdateDisplay(void)
{
    lcd.locate(0, 0);
    lcd.printf("R%02i G%02i B%02i W%03i",RedProp,GreenProp,BlueProp,White); 
    lcd.locate(0, 1);       
    //lcd.printf("R%02i G%02i B%02i H%02i ",RedThreshold, GreenThreshold, BlueThreshold, Hysteresis); 
    lcd.printf("S%02i R%02i G%02i D%01i ",SingleThreshold, MixRThreshold, MixGThreshold, GetModeSwitch()  );
}


char GetResults(uint8_t ThisSensor)
{
    char LEDColour = ERROR;
       
    if(White > WHITE_THRESHOLD) //looking at intensity somthing is on!
    {  
        switch(Config[(ThisSensor << 1)])
        {
            case STD_RED:
            {
                if((RedProp - GreenProp  > SingleThreshold) && (RedProp - BlueProp  > SingleThreshold))
                {
                    LEDColour = RED;
                }          
            }
            break;
            
            case STD_GREEN:
            {
                if((GreenProp - RedProp  > SingleThreshold) && (GreenProp - BlueProp  > SingleThreshold))
                {
                    LEDColour = GREEN;
                }             
            }
            break;       
    
            case STD_BLUE:
            {
                if((BlueProp - RedProp  > SingleThreshold) && (BlueProp - GreenProp   > SingleThreshold))
                {
                    LEDColour = BLUE;
                }             
            }
            break; 
            
            case STD_YELLOW:
            {
                if ((RedProp - GreenProp) < (2* SingleThreshold)) //check it is not red!
                { 
                    if( (GreenProp <= (MixGThreshold-Hysteresis)) && (RedProp >=(MixRThreshold+Hysteresis)) ) 
                    {             
                        LEDColour = YELLOW;
                    }
                } 
            }
            break;   
            
            case GREEN_YELLOW:
            {

                if( (GreenProp <= (MixGThreshold-Hysteresis)) && (RedProp >=(MixRThreshold+Hysteresis)) ) 
                { //pretty sure it's Yellow
                    if ((RedProp - GreenProp) < 60) //check it is not red!
                    {
                        LEDColour = YELLOW;
                    }
                } 
                else 
                {
                    if( (GreenProp > (MixGThreshold+Hysteresis)) && (RedProp <=(MixRThreshold-Hysteresis)) ) 
                    { //pretty sure it's green
                        LEDColour = GREEN;
                    } 
                }
            }
            break;
            
            case RGB:
            {
                if((RedProp - GreenProp  > SingleThreshold) && (RedProp - BlueProp  > SingleThreshold))
                {
                    LEDColour = RED;
                }
                else
                {       
                    if((GreenProp - RedProp  > SingleThreshold) && (GreenProp - BlueProp  > SingleThreshold))
                    {
                        LEDColour = GREEN;
                    }
                    else
                    {           
                        if((BlueProp - RedProp  > SingleThreshold) && (BlueProp - GreenProp   > SingleThreshold))
                        {
                            LEDColour = BLUE;
                        }
                    }
                }
            }
            break;       
                       
            case NOTUSED:
            default:
            {
                LEDColour = ERROR;       
            }
            break; 
        }
    }
    return LEDColour;
}


void UpdateOutputs(void)
{
    //good LED so turn on Outputs
    switch (Config[(SensorNumber << 1)])
    {
        case STD_RED:
        case STD_GREEN:                    
        case STD_BLUE:                    
        case STD_YELLOW: 
        {
            if (SensorResult == Config[(SensorNumber << 1)])
            {
                SetOutputBit(Config[((SensorNumber << 1)+1)]);
            }
            else
            {
                ClearOutputBit(Config[((SensorNumber << 1)+1)]);
            }  
        }
        break;
        
        case GREEN_YELLOW:
        {
            if (SensorResult == YELLOW)
            { 
                SetOutputBit(Config[((SensorNumber << 1)+1)]); //base output
                ClearOutputBit(    Config[((SensorNumber << 1)+1)] +1);//next output
            }
            else 
            {   
                if (SensorResult == GREEN)
                {
                    ClearOutputBit(Config[((SensorNumber << 1)+1)]); //base output
                    SetOutputBit(    Config[((SensorNumber << 1)+1)] +1);//next output
                }
                else
                {
                    ClearOutputBit(Config[((SensorNumber << 1)+1)]); //base output
                    ClearOutputBit(    Config[((SensorNumber << 1)+1)] +1);//next output
                }
            }      
        }
        break;
        
        case RGB:
        {
            if (SensorResult == RED)
            { 
                SetOutputBit(Config[((SensorNumber << 1)+1)]);          //base output
                ClearOutputBit(Config[((SensorNumber << 1)+1)] +1); //next output
                ClearOutputBit(Config[((SensorNumber << 1)+1)] +2); //next output
            }
            else 
            {   
                if (SensorResult == GREEN)
                {
                    ClearOutputBit(Config[((SensorNumber << 1)+1)]);          //base output
                    SetOutputBit(Config[((SensorNumber << 1)+1)] +1); //next output
                    ClearOutputBit(Config[((SensorNumber << 1)+1)] +2); //next output
                }
                else
                {
                    if (SensorResult == BLUE)
                    {
                        ClearOutputBit(Config[((SensorNumber << 1)+1)]);          //base output
                        ClearOutputBit(Config[((SensorNumber << 1)+1)] +1); //next output
                        SetOutputBit(Config[((SensorNumber << 1)+1)] +2); //next output  
                    }
                    else  
                    {
                        ClearOutputBit(Config[((SensorNumber << 1)+1)]);          //base output
                        ClearOutputBit(Config[((SensorNumber << 1)+1)] +1); //next output
                        ClearOutputBit(Config[((SensorNumber << 1)+1)] +2); //next output               
                    }
                    
                }
            }          
        }
        break;  
        
        default:
        {
            ClearOutputBit(Config[((SensorNumber << 1)+1)]);          //base output  
        }
         break;                       
     }   //end switch                                  
}

void ShowLeds(char WhichColour)
{
    RedLed = 1;
    GreenLed = 1; 
    BlueLed = 1;           
    YellowLed = 1; 
    switch (WhichColour)
    {
        case RED:
        {
            RedLed = 0;    
        }
        break;
        
         case GREEN:
        {
            GreenLed = 0;    
        }
        break;
        
        case BLUE:
        {
            BlueLed = 0;    
        }
        break; 
        
        case YELLOW:
        {
            YellowLed = 0;    
        }
        break; 
        
        default:
        {
        }
        break;              
    }    
}


void SaveSensorData(void)
{
  //nothing here right now  
}




void InitCommsPort(void)
{
    Serial RS485(p26,p25); 
    RS485.baud(19200);
    RS485.format(8,RS485.None,1); //8 data, no Parity, 1 stop bit
    //set up the other stuff!     
    LPC_PINCON->PINSEL4 &= 0x0ffff;
    LPC_PINCON->PINSEL4 |= 0x0AAAA;
    
    LPC_UART1->RS485CTRL = (1<<3)|(1<<4)|(1<<5);
    LPC_UART1->RS485DLY = 1;
    RS485.putc('*');
}
    
void HandleRS485(void)
{      
    //check for character in buffer
    if (RS485.readable())
    {
        CommsData = RS485.getc();
        if (CommsData== '*') //check for correct request
        {
            RS485.putc('*');
            CommsData = (char)(OutputData >> 8);
            RS485.putc(CommsData);
            CommsData = (char)(OutputData & 0xFF);
            RS485.putc(CommsData);
        }           
    }    
}


void HandleDisplay(void)
{
  uint8_t CurrentButton =0;  
    /*
    #define NEWBUTTON   0x80
#define UPBUTTON    0x01
#define DOWNBUTTON  0x02
#define LEFTBUTTON  0x03
#define RIGHTBUTTON 0x04
#define TICKBUTTON  0x05
*/
    CurrentButton = GetButtonState();

    //------------------- select the sensor ----------------
    if (CurrentButton == (UPBUTTON | NEWBUTTON))
    {
        DisplayScreen++;
        if (DisplayScreen == 18)
        {
            DisplayScreen = 0;
        }    
    }
    else
    {
        if (CurrentButton == (DOWNBUTTON | NEWBUTTON))
        {
            DisplayScreen--;
            if (DisplayScreen == 255)
            {
                DisplayScreen = 16;
            }
        }    
    }
 
    //check for config save
    if (DisplayScreen == 17)
    {
        lcd.locate(0, 0);
        lcd.printf("Left to save"); 
           
        if (CurrentButton == (RIGHTBUTTON | NEWBUTTON))
        {
            SaveSensorData();
        }      
    }
 
 
 
    //------------------- select the sensor type ---------------- 
    DisplaySensorType = Config[(DisplayScreen-1)*2];  
     
    if (CurrentButton == (RIGHTBUTTON | NEWBUTTON))
    {
        if (DisplaySensorType < 6)
        {
            DisplaySensorType++;
            Config[(DisplayScreen-1)*2] = DisplaySensorType;
        }    
    }
    else
    {
        if (CurrentButton == (LEFTBUTTON | NEWBUTTON))
        {
            if (DisplaySensorType > 0)
            {
                DisplaySensorType--;
                Config[(DisplayScreen-1)*2] = DisplaySensorType;
            }
        }    
    }
 
    //update output list
    CreateOutputTable();
 
    //show current config setting
    DisplaySensorType = Config[(DisplayScreen-1)*2];
    DisplaySensorOutput = Config[(DisplayScreen-1)*2 +1];
   
    if (DisplayScreen !=0)
    {
        lcd.locate(0, 0);
        lcd.printf("Sensor %02i %s", DisplayScreen, LEDTypes[DisplaySensorType]); 
        lcd.locate(0, 1);       
        lcd.printf("First Output %02i ",DisplaySensorOutput);
    }
}   

void CreateOutputTable(void)
{
    uint8_t  CurrentSensor;
    uint8_t  CurrentOutput = 0; //set to first output
        
    for (CurrentSensor = 0; CurrentSensor <15; CurrentSensor++)
    {
        //set the start output for next sensor
        CurrentOutput += OutputCount[Config[(CurrentSensor)*2]];   
        if( CurrentOutput >15 )
        {
            CurrentOutput = 15;  
        }
        Config[(CurrentSensor+1)*2 +1] = CurrentOutput;
        
    }
}    
    

int main()
{
    InitOutputs();          //set up output driver chip
    ConfigureTSC(0);        //Initialise all TCS34725's
    InitCommsPort();        //Initialise the 485 port

    
    while(1) 
    {
        GetThresholds();    //update thresholds   
        wait(0.1);          //100ms Update Cycle
        OutputData = 0;
        
        for(SensorNumber = 0; SensorNumber < 16; SensorNumber++)
        {
            if(Config[((SensorNumber << 1))] != NOTUSED) //check to see if the sensor is being used
            {  
                SensorData =  GetSensorData(SensorNumber);

                White = SensorData.White;
                Red   = SensorData.Red;
                Green = SensorData.Green;
                Blue  = SensorData.Blue;

                ScaleColours();
                
                // work out % of colour mix
                Temp = (Red+Blue+Green);
                RedProp = (Red* 100) / Temp ;
                GreenProp = (Green* 100) / Temp ;
                BlueProp = (Blue* 100) / Temp ;

                SensorResult = GetResults(SensorNumber);
                UpdateOutputs();
                
                if (SensorNumber == GetModeSwitch())
                {
                    if (DisplayScreen == 0)
                    {
                        UpdateDisplay();
                        ShowLeds(SensorResult);
                    }
                }
    
            } //end if
            else
            {
                if (DisplayScreen == 0)
                {
                    if (SensorNumber == GetModeSwitch())
                    {
                        lcd.locate(0, 0);
                        lcd.printf("INVALID SENSOR  "); 
                        lcd.locate(0, 1);       
                        lcd.printf("%02i              ",GetModeSwitch()  );
                    }
                }
            }
                       
            
            
        } //end for 
        WriteOutputPort(OutputData);  
        //WriteOutputPort(OutputDataB);
        //OutputDataB>>=1;
        //if(OutputDataB == 0)
        //{
        //    OutputDataB = 0x8000;
        //} 
        HandleDisplay();
        //Handle Comms
        HandleRS485();           
    } //while(1)
}//main