File content as of revision 8:e6e4eece90f7:

#include "mbed.h"
#include "TextLCD.h"
#include "MCP23S17.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



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


void SelectSensorPort(uint8_t SensorID);

//-------------------------------------------------------------------------------
//----------- 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);


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

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

//-------------------------------------------------------------------------------
//------------ 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

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

//-------------------------------------------------------------------------------
//------------ 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;

//-------------------------------------------------------------------------------
//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_RED,        2,  //sensor 2
                    STD_GREEN,      3,  //sensor 3
                    STD_RED,        4,  //sensor 4                    
                    STD_GREEN,      5,  //sensor 5
                    STD_RED,        6,  //sensor 6
                    STD_GREEN,      7,  //sensor 7
                    STD_RED,        8,  //sensor 8
                    STD_GREEN,      9,  //sensor 9
                    STD_RED,        10,  //sensor 10
                    STD_GREEN,      11,  //sensor 11
                    STD_RED,        12,  //sensor 12
                    STD_GREEN,      13,  //sensor 13
                    STD_RED,        14,  //sensor 14
                    STD_GREEN,      15,  //sensor 15
                };
*/
//new config trpe



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;

//------------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;

//------------------------- 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;       
}

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 
    /*
    RedThreshold    = RedTrimmer.read_u16()   * 100 >> 11;
    GreenThreshold  = GreenTrimmer.read_u16() * 100 >> 11;
    BlueThreshold   = BlueTrimmer.read_u16()  * 100 >> 11;
    Hysteresis      = HysTrimmer.read_u16()   * 100 >> 11;      
    */
    
    RedThreshold    = 45;
    GreenThreshold  = 35;
    BlueThreshold   = 10;
    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("R%02i G%02i B%02i S%02i ",RedThreshold, GreenThreshold, BlueThreshold, 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  > 30) && (RedProp - BlueProp  > 30))
                {
                    LEDColour = RED;
                }          
            }
            break;
            
            case STD_GREEN:
            {
                if((GreenProp - RedProp  > 30) && (GreenProp - BlueProp  > 30))
                {
                    LEDColour = GREEN;
                }             
            }
            break;       
    
            case STD_BLUE:
            {
                if((BlueProp - RedProp  > 30) && (BlueProp - GreenProp   > 30))
                {
                    LEDColour = BLUE;
                }             
            }
            break; 
            
            case STD_YELLOW:
            {
                if( (GreenProp <= (GreenThreshold-Hysteresis)) && (RedProp >=(RedThreshold+Hysteresis)) ) 
                {             
                    LEDColour = YELLOW;
                } 
            }
            break;   
            
            case GREEN_YELLOW:
            {
                if( (GreenProp <= (GreenThreshold-Hysteresis)) && (RedProp >=(RedThreshold+Hysteresis)) ) 
                { //pretty sure it's Yellow
                
                    LEDColour = YELLOW;
                } 
                else 
                {
                    if( (GreenProp > (GreenThreshold+Hysteresis)) && (RedProp <=(RedThreshold-Hysteresis)) ) 
                    { //pretty sure it's green
                        LEDColour = GREEN;
                    } 
                }
            }
            break;
            
            case RGB:
            {
                if((RedProp - GreenProp  > 30) && (RedProp - BlueProp  > 30))
                {
                    LEDColour = RED;
                }
                else
                {       
                    if((GreenProp - RedProp  > 30) && (GreenProp - BlueProp  > 30))
                    {
                        LEDColour = GREEN;
                    }
                    else
                    {           
                        if((BlueProp - RedProp  > 30) && (BlueProp - GreenProp   > 30))
                        {
                            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;              
    }    
}



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

    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())
                {
                    UpdateDisplay();
                    ShowLeds(SensorResult);
                }
    
            } //end if
            else
            {
                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;
        //}     
    } //while(1)
}//main