Revision 1:4c80434e0d26, committed 2015-12-07

Comitter:

richsua

Date:

Mon Dec 07 19:56:00 2015 +0000

Parent:

0:7b3eabfa1a0f

Commit message:

color sorting robot

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ID12RFID.lib	Mon Dec 07 19:56:00 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/ID12RFID/#f04afa911cf5

--- a/Servo.lib	Tue May 18 19:40:18 2010 +0000
+++ b/Servo.lib	Mon Dec 07 19:56:00 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/simon/code/Servo/#36b69a7ced07
+https://developer.mbed.org/users/richsua/code/ColorSortingRobot/#018f2f69b895

--- a/main.cpp	Tue May 18 19:40:18 2010 +0000
+++ b/main.cpp	Mon Dec 07 19:56:00 2015 +0000
@@ -1,27 +1,521 @@
 #include "mbed.h"
 #include "Servo.h"
+#include "ID12RFID.h"
 
-Servo myservo(p21);
-Serial pc(USBTX, USBRX);
+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);
+                }
+            }
+        }
 
-int main() {
-    printf("Servo Calibration Controls:\n");
-    printf("1,2,3 - Position Servo (full left, middle, full right)\n");
-    printf("4,5 - Decrease or Increase range\n");
+        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);
+        }
+}
 
-    float range = 0.0005;
-    float position = 0.5;
+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");
     
-    while(1) {                   
-        switch(pc.getc()) {
-            case '1': position = 0.0; break;
-            case '2': position = 0.5; break;
-            case '3': position = 1.0; break;
-            case '4': range += 0.0001; break; 
-            case '5': range -= 0.0001; break; 
+    // 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("position = %.1f, range = +/-%0.4f\n", position, range);
-        myservo.calibrate(range, 45.0); 
-        myservo = position;
+    }
+    
+    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();
+    }
+}
\ No newline at end of file