main.cpp

00001 #include "mbed.h"
00002 
00003 // Macros ----------------------------------------------------------------------
00004 #define SLIDE_PERIOD    (20)     // Period of PWM for slide servo (ms)
00005 #define SLIDE_PIN       (D3)     // Slide servo PWM pin
00006 
00007 #define R_DISPENSE      (PB_2)   // Red skittle solenoid
00008 #define O_DISPENSE      (PB_12)  // Orange skittle solenoid
00009 #define Y_DISPENSE      (PA_4)   // Yellow skittle solenoid
00010 #define G_DISPENSE      (PB_0)   // Green skittle solenoid
00011 #define V_DISPENSE      (PC_1)   // Violet skittle solenoid
00012 
00013 #define PC_BAUD         (115200) // Baud rate for PC debug
00014 
00015 #define KITTY_CAT_EN    (D7)     // Kitty cat enable/power up pin
00016 #define KITTY_CAT_PWM   (PA_13)  // Kitty cat PWM pin
00017 
00018 // PWMS ------------------------------------------------------------------------
00019 PwmOut geneva(D9);
00020 PwmOut slide(D3);
00021 
00022 // Solenoids -------------------------------------------------------------------
00023 DigitalOut red(R_DISPENSE);
00024 DigitalOut orange(O_DISPENSE);
00025 DigitalOut yellow(Y_DISPENSE);
00026 DigitalOut green(G_DISPENSE);
00027 DigitalOut violet(V_DISPENSE);
00028 
00029 DigitalOut kitty_cat_en(KITTY_CAT_EN);   // Enabling pin for the unclogging motor
00030 DigitalOut kitty_cat_pwm(KITTY_CAT_PWM); // Software PWM pin for the unclogging motor
00031 
00032 // RGB Sensor ------------------------------------------------------------------
00033 I2C i2c(I2C_SDA, I2C_SCL);       // Pins for I2C communication (SDA, SCL)
00034 Serial pc(SERIAL_TX, SERIAL_RX); // Used for debugging
00035 int sensor_addr = 41 << 1;       // RGB sensor I2C address
00036 
00037 // Button ----------------------------------------------------------------------
00038 DigitalIn button(USER_BUTTON);
00039 
00040 // RGB Measurement Variables ---------------------------------------------------
00041 int C = 0;                   // Current level of clear
00042 int R = 0;                     // Current level of red
00043 int G = 0;                   // Current level of green
00044 int B = 0;                    // Current level of blue
00045 
00046 // Optimal RGBC Values ---------------------------------------------------------
00047 uint16_t red_R = 3104;
00048 uint16_t red_G = 1376;
00049 uint16_t red_B = 1299;
00050 uint16_t red_C = 5441;
00051 
00052 uint16_t orange_R = 6586;
00053 uint16_t orange_G = 2810;
00054 uint16_t orange_B = 2014;
00055 uint16_t orange_C = 11056;
00056 
00057 uint16_t yellow_R = 7994;
00058 uint16_t yellow_G = 6247;
00059 uint16_t yellow_B = 2836;
00060 uint16_t yellow_C = 16767;
00061 
00062 uint16_t green_R = 2702;
00063 uint16_t green_G = 4098;
00064 uint16_t green_B = 2029;
00065 uint16_t green_C = 8623;
00066 
00067 uint16_t violet_R = 1331;
00068 uint16_t violet_G = 1024;
00069 uint16_t violet_B = 922;
00070 uint16_t violet_C = 3148;
00071 
00072 uint16_t empty_R = 648;
00073 uint16_t empty_G = 652;
00074 uint16_t empty_B = 572;
00075 uint16_t empty_C = 1831;
00076 
00077 uint16_t empty_dist = 0;
00078 uint16_t red_dist = 0;
00079 uint16_t orange_dist = 0;
00080 uint16_t yellow_dist = 0;
00081 uint16_t green_dist = 0;
00082 uint16_t violet_dist = 0;
00083 //uint16_t red_R_dist = 0;
00084 //uint16_t violet_R_dist = 0;
00085 //uint16_t orange_G_dist = 0;
00086 //uint16_t yellow_G_dist = 0;
00087 
00088 // Initialize RGB Sensor -------------------------------------------------------
00089 void init_RGB(void){
00090     // 1.) Connect to the color sensor and verify
00091     i2c.frequency(100000);
00092     char id_regval[1] = {146};
00093     char data[1] = {0};
00094     i2c.write(sensor_addr,id_regval,1, true);
00095     i2c.read(sensor_addr,data,1,false);
00096     
00097     // 2.) Initialize color sensor
00098     char timing_register[2] = {129,0};
00099     i2c.write(sensor_addr,timing_register,2,false);
00100     char control_register[2] = {143,0};
00101     i2c.write(sensor_addr,control_register,2,false);
00102     char enable_register[2] = {128,3};
00103     i2c.write(sensor_addr,enable_register,2,false);
00104 }
00105 
00106 // read_RGB method -------------------------------------------------------------
00107 void read_RGB(void){
00108     // 1.) Read clear
00109     char clear_reg[1] = {148};
00110     char clear_data[2] = {0,0};
00111     i2c.write(sensor_addr,clear_reg,1, true);
00112     i2c.read(sensor_addr,clear_data,2, false);
00113     C = ((int)clear_data[1] << 8) | clear_data[0];
00114     
00115     // 2.) Read red
00116     char red_reg[1] = {150};
00117     char red_data[2] = {0,0};
00118     i2c.write(sensor_addr,red_reg,1, true);
00119     i2c.read(sensor_addr,red_data,2, false);
00120     R = ((int)red_data[1] << 8) | red_data[0];
00121     
00122     // 3.) Read green
00123     char green_reg[1] = {152};
00124     char green_data[2] = {0,0};
00125     i2c.write(sensor_addr,green_reg,1, true);
00126     i2c.read(sensor_addr,green_data,2, false);
00127     G = ((int)green_data[1] << 8) | green_data[0];
00128     
00129     // 4.) Read blue
00130     char blue_reg[1] = {154};
00131     char blue_data[2] = {0,0};
00132     i2c.write(sensor_addr,blue_reg,1, true);
00133     i2c.read(sensor_addr,blue_data,2, false);
00134     B = ((int)blue_data[1] << 8) | blue_data[0];
00135 }
00136 
00137 // Stir ------------------------------------------------------------------------
00138 /* Calls on the kitty cat to scratch around and stir the skittle reservoir,
00139    which unclogs the reservoir and vaguely resembles a feline-like action. */
00140 void stir(void){    
00141     // 1.) Enable the kitty cat
00142     kitty_cat_en = 1;
00143     
00144     // 2.) Create software PWM for 10 cycles
00145     kitty_cat_pwm = 1;
00146     wait(0.05);
00147     kitty_cat_pwm = 0;
00148     wait(0.05);
00149     kitty_cat_pwm = 1;
00150     wait(0.05);
00151     kitty_cat_pwm = 0;
00152     wait(0.05);
00153     kitty_cat_pwm = 1;
00154     wait(0.05);
00155     kitty_cat_pwm = 0;
00156     wait(0.05);
00157     kitty_cat_pwm = 1;
00158     wait(0.05);
00159     kitty_cat_pwm = 0;
00160     wait(0.05);
00161     kitty_cat_pwm = 1;
00162     wait(0.05);
00163     kitty_cat_pwm = 0;
00164     wait(0.05);
00165     
00166     // 3.) Disable the kitty cat
00167     kitty_cat_en = 0;
00168 }
00169 
00170 // Set Slide Servo -------------------------------------------------------------
00171 void set_slide_servo(uint8_t color){
00172     if (color == 1){
00173         printf("R\r\n");
00174         slide.pulsewidth_us(1200);
00175     }
00176     else if (color == 2){
00177         printf("O\r\n");
00178         slide.pulsewidth_us(1475);
00179     }
00180     else if (color == 3){
00181         printf("Y\r\n");
00182         slide.pulsewidth_us(1750);
00183     }
00184     else if (color == 4){
00185         printf("G\r\n");
00186         slide.pulsewidth_us(2025);
00187     }
00188     else if (color == 5){
00189         printf("V\r\n");
00190         slide.pulsewidth_us(2300);
00191     }
00192     else {
00193         printf("E\r\n");
00194     }
00195 }
00196 
00197 // identify_color --------------------------------------------------------------
00198 int identify_color(void){
00199     // 1.) Compute distances from each color using the clear computation
00200     empty_dist = abs((empty_R) - (R)) + abs((empty_G) - (G)) + abs((empty_B) - (B)) + abs((empty_C) - (C));
00201     red_dist = abs((red_R) - (R)) + abs((red_G) - (G)) + abs((red_B) - (B)) + abs((red_C) - (C));
00202     orange_dist = abs((orange_R) - (R)) + abs((orange_G) - (G)) + abs((orange_B) - (B)) + abs((orange_C) - (C));
00203     yellow_dist = abs((yellow_R) - (R)) + abs((yellow_G) - (G)) + abs((yellow_B) - (B)) + abs((yellow_C) - (C));
00204     green_dist = abs((green_R) - (R)) + abs((green_G) - (G)) + abs((green_B) - (B)) + abs((green_C) - (C));
00205     violet_dist = abs((violet_R) - (R)) + abs((violet_G) - (G)) + abs((violet_B) - (B)) + abs((violet_C) - (C));;
00206     int min_dist = 65535;
00207     uint8_t min_dist_index = 0;
00208     
00209     // 2.) Preliminary distance check
00210     if (empty_dist < min_dist) {
00211         min_dist = empty_dist;
00212         min_dist_index = 0;
00213     }
00214     if (red_dist < min_dist) {
00215         min_dist = red_dist;
00216         min_dist_index = 1;
00217     }
00218     if (orange_dist < min_dist) {
00219         min_dist = orange_dist;
00220         min_dist_index = 2;
00221     }
00222     if (yellow_dist < min_dist) {
00223         min_dist = yellow_dist;
00224         min_dist_index = 3;
00225     }
00226     if (green_dist < min_dist) {
00227         min_dist = green_dist;
00228         min_dist_index = 4;
00229     }
00230     if (violet_dist < min_dist) {
00231         min_dist = violet_dist;
00232         min_dist_index = 5;
00233     }
00234     
00235     // 3.) If orange, yellow, red, or violet was found, do a secondary check
00236     /*if ((min_dist_index == 1) || (min_dist_index == 5)) {
00237         // a.) Find the minimum red distance
00238         violet_dist = abs(abs((violet_R) - (R)) + abs((violet_G) - (G)) + abs((violet_B) - (B)));
00239         violet_dist = abs(abs((violet_R) - (R)) + abs((violet_G) - (G)) + abs((violet_B) - (B)));
00240         
00241         // b.) Decide if it's a violet or red skittle
00242         if (violet_R_dist < red_R_dist) {
00243             return 5;
00244         }
00245         else {
00246             return 1;
00247         }
00248     }
00249     else if ((min_dist_index == 2) || (min_dist_index == 3) || (min_dist_index == 4)) {
00250         // a.) Find the minimum green distance
00251         orange_G_dist = abs((orange_G) - (G));
00252         yellow_G_dist = abs((yellow_G) - (G));
00253         
00254         // b.) Decide if it's an orange or yellow skittle
00255         if (orange_G_dist < yellow_G_dist) {
00256             return 2;
00257         }
00258         else {
00259             return 3;
00260         }
00261     }*/
00262     
00263     // 4.) Return minimum distance index
00264     return min_dist_index;
00265 }
00266 
00267 // main ------------------------------------------------------------------------
00268 int main() {
00269     // Set all solenoids to 0
00270     red = 0;
00271     orange = 0;
00272     yellow = 0;
00273     green = 0;
00274     violet = 0;
00275     
00276     // 1.) Intialize RGB Sensor
00277     init_RGB();
00278     pc.baud(PC_BAUD);
00279     
00280     // Test Red Solenoid
00281     /*red = 1;
00282     wait(0.5);
00283     red = 0;
00284     wait(0.5);*/
00285     
00286     // Test Orange Solenoid
00287     /*orange = 1;
00288     wait(0.5);
00289     orange = 0;
00290     wait(0.5);*/
00291     
00292     // Test Yellow Solenoid
00293     /*yellow = 1;
00294     wait(0.5);
00295     yellow = 0;
00296     wait(0.5);*/
00297     
00298     // Test Green Solenoid
00299     /*green = 1;
00300     wait(0.5);
00301     green = 0;
00302     wait(0.5);*/
00303     
00304     // Test Violet Solenoid
00305     /*violet = 1;
00306     wait(0.5);
00307     violet = 0;
00308     wait(0.5);*/
00309     
00310     // Test Slide Servo
00311     slide.period_ms(20);
00312     slide.pulsewidth_us(1200);
00313     wait(1);
00314     slide.pulsewidth_us(1475);
00315     wait(1);
00316     slide.pulsewidth_us(1750);
00317     wait(1);
00318     slide.pulsewidth_us(2025);
00319     wait(1);
00320     slide.pulsewidth_us(2300);
00321     
00322     // Test Kitty Cat
00323     stir();
00324     stir();
00325     stir();
00326     
00327     // Test Geneva Wheel Stepper Motor
00328     geneva.period_ms(10);
00329     geneva.pulsewidth_ms(1);
00330     
00331     uint8_t buff[10];
00332     
00333     // Identify colors in endless loop
00334     while (1) {
00335         // A.) Read the RGB sensor 5 times
00336         read_RGB();
00337         buff[0] = identify_color();
00338         read_RGB();
00339         buff[1] = identify_color();
00340         read_RGB();
00341         buff[2] = identify_color();
00342         read_RGB();
00343         buff[3] = identify_color();
00344         read_RGB();
00345         buff[4] = identify_color();
00346         read_RGB();
00347         buff[5] = identify_color();
00348         read_RGB();
00349         buff[6] = identify_color();
00350         read_RGB();
00351         buff[7] = identify_color();
00352         read_RGB();
00353         buff[8] = identify_color();
00354         read_RGB();
00355         buff[9] = identify_color();
00356         
00357         // B.) Decide if a skittle was scanned
00358         if ((buff[0] == buff[1]) && (buff[1] == buff[2]) && (buff[2] == buff[3]) && (buff[3] == buff[4]) && (buff[4] == buff[5]) && (buff[5] == buff[6]) && (buff[6] == buff[7]) && (buff[7] == buff[8]) && (buff[8] == buff[9])) {
00359             set_slide_servo(buff[0]);
00360             wait(1.5);
00361         }
00362     }
00363 }