File content as of revision 0:270ea1abdd2d:

// ColourSensor.cpp

#include "ColourSensor_H.h"
#include "mbed.h"

ColourSensor::ColourSensor(PinName sensor1, PinName sensor2, PinName Out):
    inOne(sensor1), inTwo(sensor2), solenoid(Out) {
        // Class constructor for colour sensor & solenoid
        // Sets initial conditions of operation
        
        initialConditions();
}

void ColourSensor::initialConditions() {
    // set initial conditions & variables
    //printf("Setting Initial Conditions\n\r");
    sensorOne = inOne.read();
    sensorOneOld = inOne.read();
    sensorOneCheck = inOne.read();
    sensorTwo = inTwo.read();
    sensorTwoOld = inTwo.read();
    sensorTwoCheck = inTwo.read();
    
    
    newDetection = 0;
    diskHave = 0;
    
    solenoidState = 0;
    
    toggleA = 0;
    toggleB = 0;
    toggleC = 0;
    toggleConst = 1;
    
    flagColour = 0;
    
    solenoidSet(0);
}

void ColourSensor::solenoidOn() {
    // Interrupt (software): Turn ON solenoid
    //printf("Turning solenoid back ON\n\r");
    
    solenoidSet(0);
}

void ColourSensor::solenoidSet(bool state) {
    // Turns solenoid on/off, sets solenoid's pull strength
    // state: pass 1 or 0 for solenoid ON or OFF respectively
    
    solenoid.write(state);
    
    //printf("Solenoid has been set to %i\n\r", state);
    
    solenoidState = state;
}

void ColourSensor::readSensor() {
    // Interrupt function (software): reads in colour detection state
    
    // Update variables
    sensorOneOld = sensorOne;
    sensorTwoOld = sensorTwo;
    
    //printf("Reading colour detection state\n\r");
    
    sensorOne = inOne.read();
    sensorTwo = inTwo.read();
    
    toggleA = 1;
}

void ColourSensor::convert() {
    // Converts sensor logic to sensible logic
    
    sensorIfOld = sensorIf; // Transfer variable states
    newDetection = 0;
    
    sensorIf = !(sensorOne xor sensorTwo);
    
    if(sensorIf && !sensorIfOld) {
        sensorColour = sensorOne & sensorTwo;
        newDetection = 1;
        toggleB = 1;
    }
    if(!diskHave)solenoidSet(0);
}

void ColourSensor::ColourCheck(){
    sensorOneCheck = inOne.read();
    sensorTwoCheck = inTwo.read();
    bool sensorColourCheck = sensorOneCheck & sensorTwoCheck;
   if( sensorColour == sensorColourCheck )toggleC = 1;
   else toggleB = 0;
   
   }
    
    
    
    

void ColourSensor::makeColourActive() {
    // Interrupt function (software): reactivates colour processing 
    toggleConst = 1;
    
    //printf("Colour processing Activated\n\r");
}

void ColourSensor::process() {
    // Processes change of state in colour detection
    //printf("Colour process() called\n\r");
    
    if (!diskHave && newDetection) {
        // If: No disk & colour newly detected, turn on solenoid
        //printf("Colour detected; collecting disk\n\r");
        
        solenoidSet(1);
        diskHave = 1;
        diskColour = sensorColour;
    } 
    
    else if ((diskHave && newDetection) && (diskColour == sensorColour)) {
        // If: Have disk & colour newly detected & disk colour is same as detected colour, turn off solenoid
        //printf("Correct colour detected; depositing disk\n\r");
        
        solenoidSet(0);
        flagColour = 1;
        diskHave = 0;
    }
    
    /*else if ((diskHave && newDetection) && (sensorTwo != diskColour)) {
        // If: Have disk & colour newly detected & disk colour is NOT same as detected colour, update variables
        //printf("Wrong colour detected; keeping disk\n\r");
    }*/
    
    else {
        //printf("Cool (y)\n\r");
    }
    
    // Temporarily disable colour processing
    //printf("Disabling colour processing\n\r");
    toggleConst = 0;
    toggleC = 0;
}