ColourSensor.cpp

00001 // ColourSensor.cpp
00002 
00003 #include "ColourSensor_H.h"
00004 #include "mbed.h"
00005 
00006 ColourSensor::ColourSensor(PinName sensor1, PinName sensor2, PinName Out):
00007     inOne(sensor1), inTwo(sensor2), solenoid(Out) {
00008         // Class constructor for colour sensor & solenoid
00009         // Sets initial conditions of operation
00010         
00011         initialConditions();
00012 }
00013 
00014 void ColourSensor::initialConditions() {
00015     // set initial conditions & variables
00016     //printf("Setting Initial Conditions\n\r");
00017     sensorOne = inOne.read();
00018     sensorOneOld = inOne.read();
00019     sensorOneCheck = inOne.read();
00020     sensorTwo = inTwo.read();
00021     sensorTwoOld = inTwo.read();
00022     sensorTwoCheck = inTwo.read();
00023     
00024     
00025     newDetection = 0;
00026     diskHave = 0;
00027     
00028     solenoidState = 0;
00029     
00030     toggleA = 0;
00031     toggleB = 0;
00032     toggleC = 0;
00033     toggleConst = 1;
00034     
00035     flagColour = 0;
00036     
00037     solenoidSet(0);
00038 }
00039 
00040 void ColourSensor::solenoidOn() {
00041     // Interrupt (software): Turn ON solenoid
00042     //printf("Turning solenoid back ON\n\r");
00043     
00044     solenoidSet(0);
00045 }
00046 
00047 void ColourSensor::solenoidSet(bool state) {
00048     // Turns solenoid on/off, sets solenoid's pull strength
00049     // state: pass 1 or 0 for solenoid ON or OFF respectively
00050     
00051     solenoid.write(state);
00052     
00053     //printf("Solenoid has been set to %i\n\r", state);
00054     
00055     solenoidState = state;
00056 }
00057 
00058 void ColourSensor::readSensor() {
00059     // Interrupt function (software): reads in colour detection state
00060     
00061     // Update variables
00062     sensorOneOld = sensorOne;
00063     sensorTwoOld = sensorTwo;
00064     
00065     //printf("Reading colour detection state\n\r");
00066     
00067     sensorOne = inOne.read();
00068     sensorTwo = inTwo.read();
00069     
00070     toggleA = 1;
00071 }
00072 
00073 void ColourSensor::convert() {
00074     // Converts sensor logic to sensible logic
00075     
00076     sensorIfOld = sensorIf; // Transfer variable states
00077     newDetection = 0;
00078     
00079     sensorIf = !(sensorOne xor sensorTwo);
00080     
00081     if(sensorIf && !sensorIfOld) {
00082         sensorColour = sensorOne & sensorTwo;
00083         newDetection = 1;
00084         toggleB = 1;
00085     }
00086     if(!diskHave)solenoidSet(0);
00087 }
00088 
00089 void ColourSensor::ColourCheck(){
00090     sensorOneCheck = inOne.read();
00091     sensorTwoCheck = inTwo.read();
00092     bool sensorColourCheck = sensorOneCheck & sensorTwoCheck;
00093    if( sensorColour == sensorColourCheck )toggleC = 1;
00094    else toggleB = 0;
00095    
00096    }
00097     
00098     
00099     
00100     
00101 
00102 void ColourSensor::makeColourActive() {
00103     // Interrupt function (software): reactivates colour processing 
00104     toggleConst = 1;
00105     
00106     //printf("Colour processing Activated\n\r");
00107 }
00108 
00109 void ColourSensor::process() {
00110     // Processes change of state in colour detection
00111     //printf("Colour process() called\n\r");
00112     
00113     if (!diskHave && newDetection) {
00114         // If: No disk & colour newly detected, turn on solenoid
00115         //printf("Colour detected; collecting disk\n\r");
00116         
00117         solenoidSet(1);
00118         diskHave = 1;
00119         diskColour = sensorColour;
00120     } 
00121     
00122     else if ((diskHave && newDetection) && (diskColour == sensorColour)) {
00123         // If: Have disk & colour newly detected & disk colour is same as detected colour, turn off solenoid
00124         //printf("Correct colour detected; depositing disk\n\r");
00125         
00126         solenoidSet(0);
00127         flagColour = 1;
00128         diskHave = 0;
00129     }
00130     
00131     /*else if ((diskHave && newDetection) && (sensorTwo != diskColour)) {
00132         // If: Have disk & colour newly detected & disk colour is NOT same as detected colour, update variables
00133         //printf("Wrong colour detected; keeping disk\n\r");
00134     }*/
00135     
00136     else {
00137         //printf("Cool (y)\n\r");
00138     }
00139     
00140     // Temporarily disable colour processing
00141     //printf("Disabling colour processing\n\r");
00142     toggleConst = 0;
00143     toggleC = 0;
00144 }