File content as of revision 1:4c80434e0d26:

#include "mbed.h"
#include "Servo.h"
#include "ID12RFID.h"

Serial pc(USBTX, USBRX); //used for debugging
ID12RFID rfid(p14); // uart rx
//Color sensor pins:
AnalogIn redIn(p15);
AnalogIn greenIn(p16);
AnalogIn blueIn(p17);
DigitalOut colorLED(p8);

//Enums:
enum State { INIT, ID_COLOR, PICK_UP, FIND_CUP, DROP };
enum Color { NONE, RED, GREEN, BLUE, YELLOW };  

//Global variables:
State currState = INIT; //current state
Color currColor = NONE; //current ball color
int  currRFID = 0;
bool ballPickedUp = false; //tracks whether ball has been picked up yet
int totalCupNum = 4; //number of cups
int Cup[5] = { 0, 0, 0, 0, 0}; //color to rfid association
    //Note: the color enum is being ussed as the index.
//    Cup[NONE]   = 0;
//    Cup[RED]    = 5465397;
//    Cup[GREEN]  = 5465138;
//    Cup[BLUE]   = 999;
//    Cup[YELLOW] = 999;
double defaultRed;
double defaultGreen;
double defaultBlue;
double fixRed;  // for color calibration 
double fixGreen;
double fixBlue;
float minDiff = 5;
class myservoClass: public Servo  ////note: might need to use protected or private since of problem of using write to change position, but position var is not updated.

{
    
    public:
        float range;
        float position;
        float moveIncrement;
        float moveWaitSpeed;
        
        myservoClass(PinName pin) : Servo(pin)
        {
            range = 0.0005;
            position = 0.5;
            moveIncrement = 0.01;
            moveWaitSpeed = 0.01;
        }

        float getPosition()
        {
            return position;
        }

        float getRange()
        {
            return range;
        }

        void correctPositionBoundary(float &pos)
        {
            if ( pos < 0 )
                pos = 0;
            if ( pos > 1 )
                pos = 1;
        }

        void decreasePosition()
        {
            position = position - moveIncrement;
            correctPositionBoundary(position);
            write(position);
            
        }

        void increasePosition()
        {
            position = position + moveIncrement;
            correctPositionBoundary(position);
            write(position);

        }

        void decreaseRange()
        {
            range -= 0.0001;
            calibrate(range, 45.0); 
        }

        void increaseRange()
        {
            range += 0.0001;
            calibrate(range, 45.0); 
        }

        void setPosition(float targetPosition)
        {
            correctPositionBoundary(targetPosition);

            //for slower movement
            if (position < targetPosition)
            {
                for(; position<targetPosition; position += moveIncrement) 
                {
                write(position);
                wait(moveWaitSpeed);
                }
            }
            else if(position > targetPosition)
            {
                for(; position>targetPosition; position -= moveIncrement) 
                {
                write(position);
                wait(moveWaitSpeed);
                }
            }
        }

        void setRange(float r)
        {
            range = r;
            calibrate(range, 45.0); 
        }

};

myservoClass shoulder(p21);
myservoClass joint1(p22);
myservoClass joint2(p23);
myservoClass claw(p24);

void defaultRange()
{
    joint1.setRange(0.0010);
    joint2.setRange(0.0010);
    shoulder.setRange(0.0010);
}

void HomePosition()
{
    // arm sticks straight up
    claw.setPosition(.07);
    joint2.setPosition(0.65);
    joint1.setPosition(0.3);
    shoulder.setPosition(0.5);
}

void BendDown()
{

    joint2.setPosition(0.23);
    joint1.setPosition(0.47);
    shoulder.setPosition(0.5);
        
    
}
void OpenClaw()
{
    // claw opens
    claw.setPosition(0.02);
}
void CloseClaw()
{
    if(currColor == BLUE){
        // claw closes to ball size
        claw.setPosition(0.35);
        }
    else {
        claw.setPosition(0.4);
        }
}

void RFIDpos(int cupNum)
{
   // gets in position for scanning
    joint2.setPosition(.77);
//    joint1.setPosition(.4);
    joint1.setPosition(.2);
    wait(0.2);
    switch(cupNum){
    // pick up location
    case(0):
        shoulder.setPosition(0.5);
        break;
    //cup numbers counting clockwise from left of arm
    case(1):
        shoulder.setPosition(0.0);
        break;
    case(2):
        shoulder.setPosition(0.35);
        break;
    case(3):
        shoulder.setPosition(0.65);
        break;
    case(4):
        shoulder.setPosition(0.9);
        break;
    }
    joint1.setPosition(.4);
}

bool RFIDtag(int cupNum){
    RFIDpos(cupNum); //move into posion
//    wait(2);
    while(!rfid.readable()){
     // will not move past until tag is readable
     }
    
    //read for tag
    //if tag matches correct color return true
    //else return false
     if(rfid.readable())
     {
        
//        while(rfid.read() == currRFID)
//        {
//        }
        currRFID = rfid.read();
        
        
        printf("\tLooking for: %d --------> This one : %d\r\n", Cup[currColor], currRFID);
        if( Cup[currColor] == currRFID )
        {
            return true;
        }
        else
        {
            return false;
        }
     }
     return false;
}



void DropBall()
{
    joint1.setPosition(0.25);
    joint2.setPosition(0.15);
}



void calibrateColorSensor(){
 
    printf("remove any balls from color sensor\n");
    wait(2);
    printf("calibrating color sensor...\n");
    
    // initialize sum number of iterations for precision
    int numIterations = 30000;
    double redInSum = 0;
    double greenInSum = 0;
    double blueInSum = 0;
    // sum all current color readings
    for(int i = 0; i < numIterations; i++){ // first iteration starts at 0, so can't take away one from i < numIterations
        redInSum += redIn;
        greenInSum += greenIn;
        blueInSum += blueIn;
    }
    
    // average the color readings
    double redInAvg = redInSum/numIterations;
    double greenInAvg = greenInSum/numIterations;
    double blueInAvg = blueInSum/numIterations;
    
    // calculate the calibration constant to normalize the "NO COLOR" reading to around 50
    fixRed = 50/redInAvg;
    fixGreen = 50/greenInAvg;
    fixBlue = 50/blueInAvg;
    
    // adjust new color readings so that the readings will differ when colored ball is present
    defaultRed = redInAvg*fixRed;
    defaultGreen = greenInAvg*fixGreen;
    defaultBlue = blueInAvg*fixBlue;
    printf("Old Red: %.2f\n", redInAvg);
    printf("Old Green: %.2f\n", greenInAvg);
    printf("Old Blue: %.2f\n", blueInAvg);
    printf("---------------\n");
    printf("fixRed: %.2f\n", fixRed);
    printf("fixGreen: %.2f\n", fixGreen);
    printf("fixBlue: %.2f\n", fixBlue);
    printf("---------------\n");
    printf("default red: %.2f \n", defaultRed);
    printf("default green: %.2f \n", defaultGreen);
    printf("default blue: %.2f \n\n", defaultBlue);
}

Color idColor(){
    printf("place ball...\n");
    // need wait function because some readings are taken when the ball isn't even on the color sensor
    // wait assures all readings are taken from the ball and not empty space or previous ball
    wait(2.5);    
    
    printf("reading ball color...\n");
    // take average of 50 readings
    double redSum = 0;
    double greenSum = 0;
    double blueSum = 0;
    int numReadings = 10000;
    for(int i = 0; i < numReadings; i++){
        redSum += redIn*fixRed;
        greenSum += greenIn*fixGreen;
        blueSum += blueIn*fixBlue; 
    }
    double red = redSum/numReadings;
    double green = greenSum/numReadings;
    double blue = blueSum/numReadings;
    
    printf("red: %.2f \n", red);
    printf("green: %.2f \n", green);
    printf("blue: %.2f \n\n", blue);
    
    //modified color values read in from color sensor:
    float redDiff = abs(red - defaultRed);
    float blueDiff = abs(blue - defaultBlue);
    float greenDiff = abs(green - defaultGreen);
    if((redDiff < minDiff) && (blueDiff < minDiff) && (greenDiff < minDiff)){
        currColor = NONE;
        printf("NO COLOR DETECTED\n\n");
        return NONE;
    }
    
    
                        //if((red > green) && (red > blue)){
                    //        currColor = RED;
                    //        printf("RED\n\n");
                    //        return RED;
                    //    }
                    //    else if((green > red) && (green > blue)){
                    //       if(blue < .9*green){ 
                    //            currColor = YELLOW;
                    //            printf("YELLOW\n\n");
                    //            return YELLOW;
                    //        }
                    //        currColor = GREEN;
                    //        printf("GREEN\n\n");
                    //        return GREEN;
                    //    }
                    //    else if((blue > green) && (blue > red)){
                    //        currColor = BLUE;
                    //        printf("BLUE\n\n");
                    //        return BLUE;
                    //    }
                    
                    
    if( (blue > green) && (blue > red) )
    {
            currColor = BLUE;
            printf("BLUE\n\n");
            return BLUE;
    } 
    else if( (red > green) )
    {
        if( green > .95*red)
        {
            currColor = YELLOW;
            printf("YELLOW\n\n");
            return YELLOW;
        }
        else
        {
            currColor = RED;
            printf("RED\n\n");
            return RED;           
        }
    }
    else if( (red < green) )
    {
        if( .95*green < red)
        {
            currColor = YELLOW;
            printf("YELLOW\n\n");
            return YELLOW;
        }
        else
        {
            currColor = GREEN;
            printf("GREEN\n\n");
            return GREEN;           
        }
    }
    
    printf("NO COLOR DETECTED\n\n");
    return NONE;
}



bool cupFound(){
    //printf("checking tag numbers...\n"); 
    bool found = false;
    int cupNum = 1;
    while(!found && (cupNum <= totalCupNum)){

        found = RFIDtag(cupNum);
        cupNum++;
        //printf("\twrong cup!\n");
    }
    return found;
}


//State machine
void stateMachine(State curr){
    switch(curr){
        case(INIT):
            HomePosition();
            currState = ID_COLOR;
            break;
        case(ID_COLOR):
            Color temp = idColor();
            if(temp != NONE){
                // COLOR DETECTION OF BALL 
                currColor = temp;
                currState = PICK_UP;
            }
            break;
        case(PICK_UP):
            printf("picking up the ball\n\n");
            OpenClaw();
            wait(1);
            BendDown();
            wait(1);
            CloseClaw();
            currState = FIND_CUP;
            break;
        case(FIND_CUP):
            printf("looking for the right cup...\n");
            if(cupFound()){
                currState = DROP;
            }
            break;
        case(DROP):
                printf("found cup. dropping ball...\n\n");
                DropBall();
                OpenClaw();
                currState = INIT;
            break;
        }
            
}


void initializeCup()
{
    Cup[NONE]   = 0;
    Cup[RED]    = 5477137;
    Cup[GREEN]  = 5477138;
    Cup[BLUE]   = 5453363;
    Cup[YELLOW] = 5453460;
}

/*
void control()
{
       switch(pc.getc()) 
       {
            case '1': shoulder.decreasePosition(); break;
            case '2': shoulder.increasePosition(); break;
            case '-': shoulder.decreaseRange(); break;
            case '=': shoulder.increaseRange(); break;

            case 'q': joint1.decreasePosition(); break;
            case 'w': joint1.increasePosition(); break;
            case '[': joint1.decreaseRange(); break;
            case ']': joint1.increaseRange(); break;

            case 'a': joint2.decreasePosition(); break;
            case 's': joint2.increasePosition(); break;
            case ';': joint2.decreaseRange(); break;
            case '\'': joint2.increaseRange(); break;

            case 'z': claw.decreasePosition(); break;
            case 'x': claw.increasePosition(); break;
            case ',': claw.decreaseRange(); break;
            case '.': claw.increaseRange(); break;
        }
    
    printf("#########\n\t");
    printf("claw: \n\
        \t position = %.2f, range = +/-%0.4f\n\
        \n\n\
        ", claw.getPosition(), claw.getRange()  );

    printf("joint2: \n\
        \t position = %.2f, range = +/-%0.4f\n\
        \n\n\
        ", joint2.getPosition(), joint2.getRange()  ); 

    printf("joint1: \n\
        \t position = %.2f, range = +/-%0.4f\n\
        \n\n\
        ", joint1.getPosition(), joint1.getRange()  );

    printf("shoulder: \n\
        \t position = %.2f, range = +/-%0.4f\n\
        \n\n\
        ", shoulder.getPosition(), shoulder.getRange()  );
}
*/

int main() 
{
    colorLED = 1;
    initializeCup();
    defaultRange();
    HomePosition();
    calibrateColorSensor();
    
    while(1){

        stateMachine(currState);
//        idColor();
    }
}