color sorting robot
Dependencies: ID12RFID ColorSortingRobot mbed
Fork of ServoProgram by
main.cpp
- Committer:
- richsua
- Date:
- 2015-12-07
- Revision:
- 1:4c80434e0d26
- Parent:
- 0:7b3eabfa1a0f
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(); } }