Richmond Sua / Mbed 2 deprecated 4180proj_ServoProgram_v11_DONE

color sorting robot

Dependencies:   ID12RFID ColorSortingRobot mbed

Fork of ServoProgram by Simon Ford

main.cpp@1:4c80434e0d26, 2015-12-07 (annotated)

Committer:
richsua
Date:
Mon Dec 07 19:56:00 2015 +0000
Revision:
1:4c80434e0d26
Parent:
0:7b3eabfa1a0f 
color sorting robot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:7b3eabfa1a0f 1 #include "mbed.h"
simon 0:7b3eabfa1a0f 2 #include "Servo.h"
richsua 1:4c80434e0d26 3 #include "ID12RFID.h"
simon 0:7b3eabfa1a0f 4
richsua 1:4c80434e0d26 5 Serial pc(USBTX, USBRX); //used for debugging
richsua 1:4c80434e0d26 6 ID12RFID rfid(p14); // uart rx
richsua 1:4c80434e0d26 7 //Color sensor pins:
richsua 1:4c80434e0d26 8 AnalogIn redIn(p15);
richsua 1:4c80434e0d26 9 AnalogIn greenIn(p16);
richsua 1:4c80434e0d26 10 AnalogIn blueIn(p17);
richsua 1:4c80434e0d26 11 DigitalOut colorLED(p8);
richsua 1:4c80434e0d26 12
richsua 1:4c80434e0d26 13 //Enums:
richsua 1:4c80434e0d26 14 enum State { INIT, ID_COLOR, PICK_UP, FIND_CUP, DROP };
richsua 1:4c80434e0d26 15 enum Color { NONE, RED, GREEN, BLUE, YELLOW };
richsua 1:4c80434e0d26 16
richsua 1:4c80434e0d26 17 //Global variables:
richsua 1:4c80434e0d26 18 State currState = INIT; //current state
richsua 1:4c80434e0d26 19 Color currColor = NONE; //current ball color
richsua 1:4c80434e0d26 20 int currRFID = 0;
richsua 1:4c80434e0d26 21 bool ballPickedUp = false; //tracks whether ball has been picked up yet
richsua 1:4c80434e0d26 22 int totalCupNum = 4; //number of cups
richsua 1:4c80434e0d26 23 int Cup[5] = { 0, 0, 0, 0, 0}; //color to rfid association
richsua 1:4c80434e0d26 24 //Note: the color enum is being ussed as the index.
richsua 1:4c80434e0d26 25 // Cup[NONE] = 0;
richsua 1:4c80434e0d26 26 // Cup[RED] = 5465397;
richsua 1:4c80434e0d26 27 // Cup[GREEN] = 5465138;
richsua 1:4c80434e0d26 28 // Cup[BLUE] = 999;
richsua 1:4c80434e0d26 29 // Cup[YELLOW] = 999;
richsua 1:4c80434e0d26 30 double defaultRed;
richsua 1:4c80434e0d26 31 double defaultGreen;
richsua 1:4c80434e0d26 32 double defaultBlue;
richsua 1:4c80434e0d26 33 double fixRed; // for color calibration
richsua 1:4c80434e0d26 34 double fixGreen;
richsua 1:4c80434e0d26 35 double fixBlue;
richsua 1:4c80434e0d26 36 float minDiff = 5;
richsua 1:4c80434e0d26 37 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.
richsua 1:4c80434e0d26 38
richsua 1:4c80434e0d26 39 {
richsua 1:4c80434e0d26 40
richsua 1:4c80434e0d26 41 public:
richsua 1:4c80434e0d26 42 float range;
richsua 1:4c80434e0d26 43 float position;
richsua 1:4c80434e0d26 44 float moveIncrement;
richsua 1:4c80434e0d26 45 float moveWaitSpeed;
richsua 1:4c80434e0d26 46
richsua 1:4c80434e0d26 47 myservoClass(PinName pin) : Servo(pin)
richsua 1:4c80434e0d26 48 {
richsua 1:4c80434e0d26 49 range = 0.0005;
richsua 1:4c80434e0d26 50 position = 0.5;
richsua 1:4c80434e0d26 51 moveIncrement = 0.01;
richsua 1:4c80434e0d26 52 moveWaitSpeed = 0.01;
richsua 1:4c80434e0d26 53 }
richsua 1:4c80434e0d26 54
richsua 1:4c80434e0d26 55 float getPosition()
richsua 1:4c80434e0d26 56 {
richsua 1:4c80434e0d26 57 return position;
richsua 1:4c80434e0d26 58 }
richsua 1:4c80434e0d26 59
richsua 1:4c80434e0d26 60 float getRange()
richsua 1:4c80434e0d26 61 {
richsua 1:4c80434e0d26 62 return range;
richsua 1:4c80434e0d26 63 }
richsua 1:4c80434e0d26 64
richsua 1:4c80434e0d26 65 void correctPositionBoundary(float &pos)
richsua 1:4c80434e0d26 66 {
richsua 1:4c80434e0d26 67 if ( pos < 0 )
richsua 1:4c80434e0d26 68 pos = 0;
richsua 1:4c80434e0d26 69 if ( pos > 1 )
richsua 1:4c80434e0d26 70 pos = 1;
richsua 1:4c80434e0d26 71 }
richsua 1:4c80434e0d26 72
richsua 1:4c80434e0d26 73 void decreasePosition()
richsua 1:4c80434e0d26 74 {
richsua 1:4c80434e0d26 75 position = position - moveIncrement;
richsua 1:4c80434e0d26 76 correctPositionBoundary(position);
richsua 1:4c80434e0d26 77 write(position);
richsua 1:4c80434e0d26 78
richsua 1:4c80434e0d26 79 }
richsua 1:4c80434e0d26 80
richsua 1:4c80434e0d26 81 void increasePosition()
richsua 1:4c80434e0d26 82 {
richsua 1:4c80434e0d26 83 position = position + moveIncrement;
richsua 1:4c80434e0d26 84 correctPositionBoundary(position);
richsua 1:4c80434e0d26 85 write(position);
richsua 1:4c80434e0d26 86
richsua 1:4c80434e0d26 87 }
richsua 1:4c80434e0d26 88
richsua 1:4c80434e0d26 89 void decreaseRange()
richsua 1:4c80434e0d26 90 {
richsua 1:4c80434e0d26 91 range -= 0.0001;
richsua 1:4c80434e0d26 92 calibrate(range, 45.0);
richsua 1:4c80434e0d26 93 }
richsua 1:4c80434e0d26 94
richsua 1:4c80434e0d26 95 void increaseRange()
richsua 1:4c80434e0d26 96 {
richsua 1:4c80434e0d26 97 range += 0.0001;
richsua 1:4c80434e0d26 98 calibrate(range, 45.0);
richsua 1:4c80434e0d26 99 }
richsua 1:4c80434e0d26 100
richsua 1:4c80434e0d26 101 void setPosition(float targetPosition)
richsua 1:4c80434e0d26 102 {
richsua 1:4c80434e0d26 103 correctPositionBoundary(targetPosition);
richsua 1:4c80434e0d26 104
richsua 1:4c80434e0d26 105 //for slower movement
richsua 1:4c80434e0d26 106 if (position < targetPosition)
richsua 1:4c80434e0d26 107 {
richsua 1:4c80434e0d26 108 for(; position<targetPosition; position += moveIncrement)
richsua 1:4c80434e0d26 109 {
richsua 1:4c80434e0d26 110 write(position);
richsua 1:4c80434e0d26 111 wait(moveWaitSpeed);
richsua 1:4c80434e0d26 112 }
richsua 1:4c80434e0d26 113 }
richsua 1:4c80434e0d26 114 else if(position > targetPosition)
richsua 1:4c80434e0d26 115 {
richsua 1:4c80434e0d26 116 for(; position>targetPosition; position -= moveIncrement)
richsua 1:4c80434e0d26 117 {
richsua 1:4c80434e0d26 118 write(position);
richsua 1:4c80434e0d26 119 wait(moveWaitSpeed);
richsua 1:4c80434e0d26 120 }
richsua 1:4c80434e0d26 121 }
richsua 1:4c80434e0d26 122 }
simon 0:7b3eabfa1a0f 123
richsua 1:4c80434e0d26 124 void setRange(float r)
richsua 1:4c80434e0d26 125 {
richsua 1:4c80434e0d26 126 range = r;
richsua 1:4c80434e0d26 127 calibrate(range, 45.0);
richsua 1:4c80434e0d26 128 }
richsua 1:4c80434e0d26 129
richsua 1:4c80434e0d26 130 };
richsua 1:4c80434e0d26 131
richsua 1:4c80434e0d26 132 myservoClass shoulder(p21);
richsua 1:4c80434e0d26 133 myservoClass joint1(p22);
richsua 1:4c80434e0d26 134 myservoClass joint2(p23);
richsua 1:4c80434e0d26 135 myservoClass claw(p24);
richsua 1:4c80434e0d26 136
richsua 1:4c80434e0d26 137 void defaultRange()
richsua 1:4c80434e0d26 138 {
richsua 1:4c80434e0d26 139 joint1.setRange(0.0010);
richsua 1:4c80434e0d26 140 joint2.setRange(0.0010);
richsua 1:4c80434e0d26 141 shoulder.setRange(0.0010);
richsua 1:4c80434e0d26 142 }
richsua 1:4c80434e0d26 143
richsua 1:4c80434e0d26 144 void HomePosition()
richsua 1:4c80434e0d26 145 {
richsua 1:4c80434e0d26 146 // arm sticks straight up
richsua 1:4c80434e0d26 147 claw.setPosition(.07);
richsua 1:4c80434e0d26 148 joint2.setPosition(0.65);
richsua 1:4c80434e0d26 149 joint1.setPosition(0.3);
richsua 1:4c80434e0d26 150 shoulder.setPosition(0.5);
richsua 1:4c80434e0d26 151 }
richsua 1:4c80434e0d26 152
richsua 1:4c80434e0d26 153 void BendDown()
richsua 1:4c80434e0d26 154 {
richsua 1:4c80434e0d26 155
richsua 1:4c80434e0d26 156 joint2.setPosition(0.23);
richsua 1:4c80434e0d26 157 joint1.setPosition(0.47);
richsua 1:4c80434e0d26 158 shoulder.setPosition(0.5);
richsua 1:4c80434e0d26 159
richsua 1:4c80434e0d26 160
richsua 1:4c80434e0d26 161 }
richsua 1:4c80434e0d26 162 void OpenClaw()
richsua 1:4c80434e0d26 163 {
richsua 1:4c80434e0d26 164 // claw opens
richsua 1:4c80434e0d26 165 claw.setPosition(0.02);
richsua 1:4c80434e0d26 166 }
richsua 1:4c80434e0d26 167 void CloseClaw()
richsua 1:4c80434e0d26 168 {
richsua 1:4c80434e0d26 169 if(currColor == BLUE){
richsua 1:4c80434e0d26 170 // claw closes to ball size
richsua 1:4c80434e0d26 171 claw.setPosition(0.35);
richsua 1:4c80434e0d26 172 }
richsua 1:4c80434e0d26 173 else {
richsua 1:4c80434e0d26 174 claw.setPosition(0.4);
richsua 1:4c80434e0d26 175 }
richsua 1:4c80434e0d26 176 }
simon 0:7b3eabfa1a0f 177
richsua 1:4c80434e0d26 178 void RFIDpos(int cupNum)
richsua 1:4c80434e0d26 179 {
richsua 1:4c80434e0d26 180 // gets in position for scanning
richsua 1:4c80434e0d26 181 joint2.setPosition(.77);
richsua 1:4c80434e0d26 182 // joint1.setPosition(.4);
richsua 1:4c80434e0d26 183 joint1.setPosition(.2);
richsua 1:4c80434e0d26 184 wait(0.2);
richsua 1:4c80434e0d26 185 switch(cupNum){
richsua 1:4c80434e0d26 186 // pick up location
richsua 1:4c80434e0d26 187 case(0):
richsua 1:4c80434e0d26 188 shoulder.setPosition(0.5);
richsua 1:4c80434e0d26 189 break;
richsua 1:4c80434e0d26 190 //cup numbers counting clockwise from left of arm
richsua 1:4c80434e0d26 191 case(1):
richsua 1:4c80434e0d26 192 shoulder.setPosition(0.0);
richsua 1:4c80434e0d26 193 break;
richsua 1:4c80434e0d26 194 case(2):
richsua 1:4c80434e0d26 195 shoulder.setPosition(0.35);
richsua 1:4c80434e0d26 196 break;
richsua 1:4c80434e0d26 197 case(3):
richsua 1:4c80434e0d26 198 shoulder.setPosition(0.65);
richsua 1:4c80434e0d26 199 break;
richsua 1:4c80434e0d26 200 case(4):
richsua 1:4c80434e0d26 201 shoulder.setPosition(0.9);
richsua 1:4c80434e0d26 202 break;
richsua 1:4c80434e0d26 203 }
richsua 1:4c80434e0d26 204 joint1.setPosition(.4);
richsua 1:4c80434e0d26 205 }
richsua 1:4c80434e0d26 206
richsua 1:4c80434e0d26 207 bool RFIDtag(int cupNum){
richsua 1:4c80434e0d26 208 RFIDpos(cupNum); //move into posion
richsua 1:4c80434e0d26 209 // wait(2);
richsua 1:4c80434e0d26 210 while(!rfid.readable()){
richsua 1:4c80434e0d26 211 // will not move past until tag is readable
richsua 1:4c80434e0d26 212 }
richsua 1:4c80434e0d26 213
richsua 1:4c80434e0d26 214 //read for tag
richsua 1:4c80434e0d26 215 //if tag matches correct color return true
richsua 1:4c80434e0d26 216 //else return false
richsua 1:4c80434e0d26 217 if(rfid.readable())
richsua 1:4c80434e0d26 218 {
richsua 1:4c80434e0d26 219
richsua 1:4c80434e0d26 220 // while(rfid.read() == currRFID)
richsua 1:4c80434e0d26 221 // {
richsua 1:4c80434e0d26 222 // }
richsua 1:4c80434e0d26 223 currRFID = rfid.read();
richsua 1:4c80434e0d26 224
richsua 1:4c80434e0d26 225
richsua 1:4c80434e0d26 226 printf("\tLooking for: %d --------> This one : %d\r\n", Cup[currColor], currRFID);
richsua 1:4c80434e0d26 227 if( Cup[currColor] == currRFID )
richsua 1:4c80434e0d26 228 {
richsua 1:4c80434e0d26 229 return true;
richsua 1:4c80434e0d26 230 }
richsua 1:4c80434e0d26 231 else
richsua 1:4c80434e0d26 232 {
richsua 1:4c80434e0d26 233 return false;
richsua 1:4c80434e0d26 234 }
richsua 1:4c80434e0d26 235 }
richsua 1:4c80434e0d26 236 return false;
richsua 1:4c80434e0d26 237 }
richsua 1:4c80434e0d26 238
richsua 1:4c80434e0d26 239
richsua 1:4c80434e0d26 240
richsua 1:4c80434e0d26 241 void DropBall()
richsua 1:4c80434e0d26 242 {
richsua 1:4c80434e0d26 243 joint1.setPosition(0.25);
richsua 1:4c80434e0d26 244 joint2.setPosition(0.15);
richsua 1:4c80434e0d26 245 }
richsua 1:4c80434e0d26 246
richsua 1:4c80434e0d26 247
richsua 1:4c80434e0d26 248
richsua 1:4c80434e0d26 249 void calibrateColorSensor(){
richsua 1:4c80434e0d26 250
richsua 1:4c80434e0d26 251 printf("remove any balls from color sensor\n");
richsua 1:4c80434e0d26 252 wait(2);
richsua 1:4c80434e0d26 253 printf("calibrating color sensor...\n");
simon 0:7b3eabfa1a0f 254
richsua 1:4c80434e0d26 255 // initialize sum number of iterations for precision
richsua 1:4c80434e0d26 256 int numIterations = 30000;
richsua 1:4c80434e0d26 257 double redInSum = 0;
richsua 1:4c80434e0d26 258 double greenInSum = 0;
richsua 1:4c80434e0d26 259 double blueInSum = 0;
richsua 1:4c80434e0d26 260 // sum all current color readings
richsua 1:4c80434e0d26 261 for(int i = 0; i < numIterations; i++){ // first iteration starts at 0, so can't take away one from i < numIterations
richsua 1:4c80434e0d26 262 redInSum += redIn;
richsua 1:4c80434e0d26 263 greenInSum += greenIn;
richsua 1:4c80434e0d26 264 blueInSum += blueIn;
richsua 1:4c80434e0d26 265 }
richsua 1:4c80434e0d26 266
richsua 1:4c80434e0d26 267 // average the color readings
richsua 1:4c80434e0d26 268 double redInAvg = redInSum/numIterations;
richsua 1:4c80434e0d26 269 double greenInAvg = greenInSum/numIterations;
richsua 1:4c80434e0d26 270 double blueInAvg = blueInSum/numIterations;
richsua 1:4c80434e0d26 271
richsua 1:4c80434e0d26 272 // calculate the calibration constant to normalize the "NO COLOR" reading to around 50
richsua 1:4c80434e0d26 273 fixRed = 50/redInAvg;
richsua 1:4c80434e0d26 274 fixGreen = 50/greenInAvg;
richsua 1:4c80434e0d26 275 fixBlue = 50/blueInAvg;
richsua 1:4c80434e0d26 276
richsua 1:4c80434e0d26 277 // adjust new color readings so that the readings will differ when colored ball is present
richsua 1:4c80434e0d26 278 defaultRed = redInAvg*fixRed;
richsua 1:4c80434e0d26 279 defaultGreen = greenInAvg*fixGreen;
richsua 1:4c80434e0d26 280 defaultBlue = blueInAvg*fixBlue;
richsua 1:4c80434e0d26 281 printf("Old Red: %.2f\n", redInAvg);
richsua 1:4c80434e0d26 282 printf("Old Green: %.2f\n", greenInAvg);
richsua 1:4c80434e0d26 283 printf("Old Blue: %.2f\n", blueInAvg);
richsua 1:4c80434e0d26 284 printf("---------------\n");
richsua 1:4c80434e0d26 285 printf("fixRed: %.2f\n", fixRed);
richsua 1:4c80434e0d26 286 printf("fixGreen: %.2f\n", fixGreen);
richsua 1:4c80434e0d26 287 printf("fixBlue: %.2f\n", fixBlue);
richsua 1:4c80434e0d26 288 printf("---------------\n");
richsua 1:4c80434e0d26 289 printf("default red: %.2f \n", defaultRed);
richsua 1:4c80434e0d26 290 printf("default green: %.2f \n", defaultGreen);
richsua 1:4c80434e0d26 291 printf("default blue: %.2f \n\n", defaultBlue);
richsua 1:4c80434e0d26 292 }
richsua 1:4c80434e0d26 293
richsua 1:4c80434e0d26 294 Color idColor(){
richsua 1:4c80434e0d26 295 printf("place ball...\n");
richsua 1:4c80434e0d26 296 // need wait function because some readings are taken when the ball isn't even on the color sensor
richsua 1:4c80434e0d26 297 // wait assures all readings are taken from the ball and not empty space or previous ball
richsua 1:4c80434e0d26 298 wait(2.5);
richsua 1:4c80434e0d26 299
richsua 1:4c80434e0d26 300 printf("reading ball color...\n");
richsua 1:4c80434e0d26 301 // take average of 50 readings
richsua 1:4c80434e0d26 302 double redSum = 0;
richsua 1:4c80434e0d26 303 double greenSum = 0;
richsua 1:4c80434e0d26 304 double blueSum = 0;
richsua 1:4c80434e0d26 305 int numReadings = 10000;
richsua 1:4c80434e0d26 306 for(int i = 0; i < numReadings; i++){
richsua 1:4c80434e0d26 307 redSum += redIn*fixRed;
richsua 1:4c80434e0d26 308 greenSum += greenIn*fixGreen;
richsua 1:4c80434e0d26 309 blueSum += blueIn*fixBlue;
richsua 1:4c80434e0d26 310 }
richsua 1:4c80434e0d26 311 double red = redSum/numReadings;
richsua 1:4c80434e0d26 312 double green = greenSum/numReadings;
richsua 1:4c80434e0d26 313 double blue = blueSum/numReadings;
richsua 1:4c80434e0d26 314
richsua 1:4c80434e0d26 315 printf("red: %.2f \n", red);
richsua 1:4c80434e0d26 316 printf("green: %.2f \n", green);
richsua 1:4c80434e0d26 317 printf("blue: %.2f \n\n", blue);
richsua 1:4c80434e0d26 318
richsua 1:4c80434e0d26 319 //modified color values read in from color sensor:
richsua 1:4c80434e0d26 320 float redDiff = abs(red - defaultRed);
richsua 1:4c80434e0d26 321 float blueDiff = abs(blue - defaultBlue);
richsua 1:4c80434e0d26 322 float greenDiff = abs(green - defaultGreen);
richsua 1:4c80434e0d26 323 if((redDiff < minDiff) && (blueDiff < minDiff) && (greenDiff < minDiff)){
richsua 1:4c80434e0d26 324 currColor = NONE;
richsua 1:4c80434e0d26 325 printf("NO COLOR DETECTED\n\n");
richsua 1:4c80434e0d26 326 return NONE;
richsua 1:4c80434e0d26 327 }
richsua 1:4c80434e0d26 328
richsua 1:4c80434e0d26 329
richsua 1:4c80434e0d26 330 //if((red > green) && (red > blue)){
richsua 1:4c80434e0d26 331 // currColor = RED;
richsua 1:4c80434e0d26 332 // printf("RED\n\n");
richsua 1:4c80434e0d26 333 // return RED;
richsua 1:4c80434e0d26 334 // }
richsua 1:4c80434e0d26 335 // else if((green > red) && (green > blue)){
richsua 1:4c80434e0d26 336 // if(blue < .9*green){
richsua 1:4c80434e0d26 337 // currColor = YELLOW;
richsua 1:4c80434e0d26 338 // printf("YELLOW\n\n");
richsua 1:4c80434e0d26 339 // return YELLOW;
richsua 1:4c80434e0d26 340 // }
richsua 1:4c80434e0d26 341 // currColor = GREEN;
richsua 1:4c80434e0d26 342 // printf("GREEN\n\n");
richsua 1:4c80434e0d26 343 // return GREEN;
richsua 1:4c80434e0d26 344 // }
richsua 1:4c80434e0d26 345 // else if((blue > green) && (blue > red)){
richsua 1:4c80434e0d26 346 // currColor = BLUE;
richsua 1:4c80434e0d26 347 // printf("BLUE\n\n");
richsua 1:4c80434e0d26 348 // return BLUE;
richsua 1:4c80434e0d26 349 // }
richsua 1:4c80434e0d26 350
richsua 1:4c80434e0d26 351
richsua 1:4c80434e0d26 352 if( (blue > green) && (blue > red) )
richsua 1:4c80434e0d26 353 {
richsua 1:4c80434e0d26 354 currColor = BLUE;
richsua 1:4c80434e0d26 355 printf("BLUE\n\n");
richsua 1:4c80434e0d26 356 return BLUE;
richsua 1:4c80434e0d26 357 }
richsua 1:4c80434e0d26 358 else if( (red > green) )
richsua 1:4c80434e0d26 359 {
richsua 1:4c80434e0d26 360 if( green > .95*red)
richsua 1:4c80434e0d26 361 {
richsua 1:4c80434e0d26 362 currColor = YELLOW;
richsua 1:4c80434e0d26 363 printf("YELLOW\n\n");
richsua 1:4c80434e0d26 364 return YELLOW;
richsua 1:4c80434e0d26 365 }
richsua 1:4c80434e0d26 366 else
richsua 1:4c80434e0d26 367 {
richsua 1:4c80434e0d26 368 currColor = RED;
richsua 1:4c80434e0d26 369 printf("RED\n\n");
richsua 1:4c80434e0d26 370 return RED;
richsua 1:4c80434e0d26 371 }
richsua 1:4c80434e0d26 372 }
richsua 1:4c80434e0d26 373 else if( (red < green) )
richsua 1:4c80434e0d26 374 {
richsua 1:4c80434e0d26 375 if( .95*green < red)
richsua 1:4c80434e0d26 376 {
richsua 1:4c80434e0d26 377 currColor = YELLOW;
richsua 1:4c80434e0d26 378 printf("YELLOW\n\n");
richsua 1:4c80434e0d26 379 return YELLOW;
richsua 1:4c80434e0d26 380 }
richsua 1:4c80434e0d26 381 else
richsua 1:4c80434e0d26 382 {
richsua 1:4c80434e0d26 383 currColor = GREEN;
richsua 1:4c80434e0d26 384 printf("GREEN\n\n");
richsua 1:4c80434e0d26 385 return GREEN;
simon 0:7b3eabfa1a0f 386 }
richsua 1:4c80434e0d26 387 }
richsua 1:4c80434e0d26 388
richsua 1:4c80434e0d26 389 printf("NO COLOR DETECTED\n\n");
richsua 1:4c80434e0d26 390 return NONE;
richsua 1:4c80434e0d26 391 }
richsua 1:4c80434e0d26 392
richsua 1:4c80434e0d26 393
richsua 1:4c80434e0d26 394
richsua 1:4c80434e0d26 395 bool cupFound(){
richsua 1:4c80434e0d26 396 //printf("checking tag numbers...\n");
richsua 1:4c80434e0d26 397 bool found = false;
richsua 1:4c80434e0d26 398 int cupNum = 1;
richsua 1:4c80434e0d26 399 while(!found && (cupNum <= totalCupNum)){
richsua 1:4c80434e0d26 400
richsua 1:4c80434e0d26 401 found = RFIDtag(cupNum);
richsua 1:4c80434e0d26 402 cupNum++;
richsua 1:4c80434e0d26 403 //printf("\twrong cup!\n");
simon 0:7b3eabfa1a0f 404 }
richsua 1:4c80434e0d26 405 return found;
simon 0:7b3eabfa1a0f 406 }
richsua 1:4c80434e0d26 407
richsua 1:4c80434e0d26 408
richsua 1:4c80434e0d26 409 //State machine
richsua 1:4c80434e0d26 410 void stateMachine(State curr){
richsua 1:4c80434e0d26 411 switch(curr){
richsua 1:4c80434e0d26 412 case(INIT):
richsua 1:4c80434e0d26 413 HomePosition();
richsua 1:4c80434e0d26 414 currState = ID_COLOR;
richsua 1:4c80434e0d26 415 break;
richsua 1:4c80434e0d26 416 case(ID_COLOR):
richsua 1:4c80434e0d26 417 Color temp = idColor();
richsua 1:4c80434e0d26 418 if(temp != NONE){
richsua 1:4c80434e0d26 419 // COLOR DETECTION OF BALL
richsua 1:4c80434e0d26 420 currColor = temp;
richsua 1:4c80434e0d26 421 currState = PICK_UP;
richsua 1:4c80434e0d26 422 }
richsua 1:4c80434e0d26 423 break;
richsua 1:4c80434e0d26 424 case(PICK_UP):
richsua 1:4c80434e0d26 425 printf("picking up the ball\n\n");
richsua 1:4c80434e0d26 426 OpenClaw();
richsua 1:4c80434e0d26 427 wait(1);
richsua 1:4c80434e0d26 428 BendDown();
richsua 1:4c80434e0d26 429 wait(1);
richsua 1:4c80434e0d26 430 CloseClaw();
richsua 1:4c80434e0d26 431 currState = FIND_CUP;
richsua 1:4c80434e0d26 432 break;
richsua 1:4c80434e0d26 433 case(FIND_CUP):
richsua 1:4c80434e0d26 434 printf("looking for the right cup...\n");
richsua 1:4c80434e0d26 435 if(cupFound()){
richsua 1:4c80434e0d26 436 currState = DROP;
richsua 1:4c80434e0d26 437 }
richsua 1:4c80434e0d26 438 break;
richsua 1:4c80434e0d26 439 case(DROP):
richsua 1:4c80434e0d26 440 printf("found cup. dropping ball...\n\n");
richsua 1:4c80434e0d26 441 DropBall();
richsua 1:4c80434e0d26 442 OpenClaw();
richsua 1:4c80434e0d26 443 currState = INIT;
richsua 1:4c80434e0d26 444 break;
richsua 1:4c80434e0d26 445 }
richsua 1:4c80434e0d26 446
richsua 1:4c80434e0d26 447 }
richsua 1:4c80434e0d26 448
richsua 1:4c80434e0d26 449
richsua 1:4c80434e0d26 450 void initializeCup()
richsua 1:4c80434e0d26 451 {
richsua 1:4c80434e0d26 452 Cup[NONE] = 0;
richsua 1:4c80434e0d26 453 Cup[RED] = 5477137;
richsua 1:4c80434e0d26 454 Cup[GREEN] = 5477138;
richsua 1:4c80434e0d26 455 Cup[BLUE] = 5453363;
richsua 1:4c80434e0d26 456 Cup[YELLOW] = 5453460;
richsua 1:4c80434e0d26 457 }
richsua 1:4c80434e0d26 458
richsua 1:4c80434e0d26 459 /*
richsua 1:4c80434e0d26 460 void control()
richsua 1:4c80434e0d26 461 {
richsua 1:4c80434e0d26 462 switch(pc.getc())
richsua 1:4c80434e0d26 463 {
richsua 1:4c80434e0d26 464 case '1': shoulder.decreasePosition(); break;
richsua 1:4c80434e0d26 465 case '2': shoulder.increasePosition(); break;
richsua 1:4c80434e0d26 466 case '-': shoulder.decreaseRange(); break;
richsua 1:4c80434e0d26 467 case '=': shoulder.increaseRange(); break;
richsua 1:4c80434e0d26 468
richsua 1:4c80434e0d26 469 case 'q': joint1.decreasePosition(); break;
richsua 1:4c80434e0d26 470 case 'w': joint1.increasePosition(); break;
richsua 1:4c80434e0d26 471 case '[': joint1.decreaseRange(); break;
richsua 1:4c80434e0d26 472 case ']': joint1.increaseRange(); break;
richsua 1:4c80434e0d26 473
richsua 1:4c80434e0d26 474 case 'a': joint2.decreasePosition(); break;
richsua 1:4c80434e0d26 475 case 's': joint2.increasePosition(); break;
richsua 1:4c80434e0d26 476 case ';': joint2.decreaseRange(); break;
richsua 1:4c80434e0d26 477 case '\'': joint2.increaseRange(); break;
richsua 1:4c80434e0d26 478
richsua 1:4c80434e0d26 479 case 'z': claw.decreasePosition(); break;
richsua 1:4c80434e0d26 480 case 'x': claw.increasePosition(); break;
richsua 1:4c80434e0d26 481 case ',': claw.decreaseRange(); break;
richsua 1:4c80434e0d26 482 case '.': claw.increaseRange(); break;
richsua 1:4c80434e0d26 483 }
richsua 1:4c80434e0d26 484
richsua 1:4c80434e0d26 485 printf("#########\n\t");
richsua 1:4c80434e0d26 486 printf("claw: \n\
richsua 1:4c80434e0d26 487 \t position = %.2f, range = +/-%0.4f\n\
richsua 1:4c80434e0d26 488 \n\n\
richsua 1:4c80434e0d26 489 ", claw.getPosition(), claw.getRange() );
richsua 1:4c80434e0d26 490
richsua 1:4c80434e0d26 491 printf("joint2: \n\
richsua 1:4c80434e0d26 492 \t position = %.2f, range = +/-%0.4f\n\
richsua 1:4c80434e0d26 493 \n\n\
richsua 1:4c80434e0d26 494 ", joint2.getPosition(), joint2.getRange() );
richsua 1:4c80434e0d26 495
richsua 1:4c80434e0d26 496 printf("joint1: \n\
richsua 1:4c80434e0d26 497 \t position = %.2f, range = +/-%0.4f\n\
richsua 1:4c80434e0d26 498 \n\n\
richsua 1:4c80434e0d26 499 ", joint1.getPosition(), joint1.getRange() );
richsua 1:4c80434e0d26 500
richsua 1:4c80434e0d26 501 printf("shoulder: \n\
richsua 1:4c80434e0d26 502 \t position = %.2f, range = +/-%0.4f\n\
richsua 1:4c80434e0d26 503 \n\n\
richsua 1:4c80434e0d26 504 ", shoulder.getPosition(), shoulder.getRange() );
richsua 1:4c80434e0d26 505 }
richsua 1:4c80434e0d26 506 */
richsua 1:4c80434e0d26 507
richsua 1:4c80434e0d26 508 int main()
richsua 1:4c80434e0d26 509 {
richsua 1:4c80434e0d26 510 colorLED = 1;
richsua 1:4c80434e0d26 511 initializeCup();
richsua 1:4c80434e0d26 512 defaultRange();
richsua 1:4c80434e0d26 513 HomePosition();
richsua 1:4c80434e0d26 514 calibrateColorSensor();
richsua 1:4c80434e0d26 515
richsua 1:4c80434e0d26 516 while(1){
richsua 1:4c80434e0d26 517
richsua 1:4c80434e0d26 518 stateMachine(currState);
richsua 1:4c80434e0d26 519 // idColor();
richsua 1:4c80434e0d26 520 }
richsua 1:4c80434e0d26 521 }