Revision 0:d4141fa5c9bd, committed 2018-05-15

Comitter:

jmckneel

Date:

Tue May 15 22:52:32 2018 +0000

Commit message:

Testing sequence for the Industrial End Node (not just the stepper motor)

Changed in this revision

diff -r 000000000000 -r d4141fa5c9bd main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 15 22:52:32 2018 +0000
@@ -0,0 +1,363 @@
+#include "mbed.h"
+
+// Macros ----------------------------------------------------------------------
+#define SLIDE_PERIOD    (20)     // Period of PWM for slide servo (ms)
+#define SLIDE_PIN       (D3)     // Slide servo PWM pin
+
+#define R_DISPENSE      (PB_2)   // Red skittle solenoid
+#define O_DISPENSE      (PB_12)  // Orange skittle solenoid
+#define Y_DISPENSE      (PA_4)   // Yellow skittle solenoid
+#define G_DISPENSE      (PB_0)   // Green skittle solenoid
+#define V_DISPENSE      (PC_1)   // Violet skittle solenoid
+
+#define PC_BAUD         (115200) // Baud rate for PC debug
+
+#define KITTY_CAT_EN    (D7)     // Kitty cat enable/power up pin
+#define KITTY_CAT_PWM   (PA_13)  // Kitty cat PWM pin
+
+// PWMS ------------------------------------------------------------------------
+PwmOut geneva(D9);
+PwmOut slide(D3);
+
+// Solenoids -------------------------------------------------------------------
+DigitalOut red(R_DISPENSE);
+DigitalOut orange(O_DISPENSE);
+DigitalOut yellow(Y_DISPENSE);
+DigitalOut green(G_DISPENSE);
+DigitalOut violet(V_DISPENSE);
+
+DigitalOut kitty_cat_en(KITTY_CAT_EN);   // Enabling pin for the unclogging motor
+DigitalOut kitty_cat_pwm(KITTY_CAT_PWM); // Software PWM pin for the unclogging motor
+
+// RGB Sensor ------------------------------------------------------------------
+I2C i2c(I2C_SDA, I2C_SCL);       // Pins for I2C communication (SDA, SCL)
+Serial pc(SERIAL_TX, SERIAL_RX); // Used for debugging
+int sensor_addr = 41 << 1;       // RGB sensor I2C address
+
+// Button ----------------------------------------------------------------------
+DigitalIn button(USER_BUTTON);
+
+// RGB Measurement Variables ---------------------------------------------------
+int C = 0;                   // Current level of clear
+int R = 0;                     // Current level of red
+int G = 0;                   // Current level of green
+int B = 0;                    // Current level of blue
+
+// Optimal RGBC Values ---------------------------------------------------------
+uint16_t red_R = 3104;
+uint16_t red_G = 1376;
+uint16_t red_B = 1299;
+uint16_t red_C = 5441;
+
+uint16_t orange_R = 6586;
+uint16_t orange_G = 2810;
+uint16_t orange_B = 2014;
+uint16_t orange_C = 11056;
+
+uint16_t yellow_R = 7994;
+uint16_t yellow_G = 6247;
+uint16_t yellow_B = 2836;
+uint16_t yellow_C = 16767;
+
+uint16_t green_R = 2702;
+uint16_t green_G = 4098;
+uint16_t green_B = 2029;
+uint16_t green_C = 8623;
+
+uint16_t violet_R = 1331;
+uint16_t violet_G = 1024;
+uint16_t violet_B = 922;
+uint16_t violet_C = 3148;
+
+uint16_t empty_R = 648;
+uint16_t empty_G = 652;
+uint16_t empty_B = 572;
+uint16_t empty_C = 1831;
+
+uint16_t empty_dist = 0;
+uint16_t red_dist = 0;
+uint16_t orange_dist = 0;
+uint16_t yellow_dist = 0;
+uint16_t green_dist = 0;
+uint16_t violet_dist = 0;
+//uint16_t red_R_dist = 0;
+//uint16_t violet_R_dist = 0;
+//uint16_t orange_G_dist = 0;
+//uint16_t yellow_G_dist = 0;
+
+// Initialize RGB Sensor -------------------------------------------------------
+void init_RGB(void){
+    // 1.) Connect to the color sensor and verify
+    i2c.frequency(100000);
+    char id_regval[1] = {146};
+    char data[1] = {0};
+    i2c.write(sensor_addr,id_regval,1, true);
+    i2c.read(sensor_addr,data,1,false);
+    
+    // 2.) Initialize color sensor
+    char timing_register[2] = {129,0};
+    i2c.write(sensor_addr,timing_register,2,false);
+    char control_register[2] = {143,0};
+    i2c.write(sensor_addr,control_register,2,false);
+    char enable_register[2] = {128,3};
+    i2c.write(sensor_addr,enable_register,2,false);
+}
+
+// read_RGB method -------------------------------------------------------------
+void read_RGB(void){
+    // 1.) Read clear
+    char clear_reg[1] = {148};
+    char clear_data[2] = {0,0};
+    i2c.write(sensor_addr,clear_reg,1, true);
+    i2c.read(sensor_addr,clear_data,2, false);
+    C = ((int)clear_data[1] << 8) | clear_data[0];
+    
+    // 2.) Read red
+    char red_reg[1] = {150};
+    char red_data[2] = {0,0};
+    i2c.write(sensor_addr,red_reg,1, true);
+    i2c.read(sensor_addr,red_data,2, false);
+    R = ((int)red_data[1] << 8) | red_data[0];
+    
+    // 3.) Read green
+    char green_reg[1] = {152};
+    char green_data[2] = {0,0};
+    i2c.write(sensor_addr,green_reg,1, true);
+    i2c.read(sensor_addr,green_data,2, false);
+    G = ((int)green_data[1] << 8) | green_data[0];
+    
+    // 4.) Read blue
+    char blue_reg[1] = {154};
+    char blue_data[2] = {0,0};
+    i2c.write(sensor_addr,blue_reg,1, true);
+    i2c.read(sensor_addr,blue_data,2, false);
+    B = ((int)blue_data[1] << 8) | blue_data[0];
+}
+
+// Stir ------------------------------------------------------------------------
+/* Calls on the kitty cat to scratch around and stir the skittle reservoir,
+   which unclogs the reservoir and vaguely resembles a feline-like action. */
+void stir(void){    
+    // 1.) Enable the kitty cat
+    kitty_cat_en = 1;
+    
+    // 2.) Create software PWM for 10 cycles
+    kitty_cat_pwm = 1;
+    wait(0.05);
+    kitty_cat_pwm = 0;
+    wait(0.05);
+    kitty_cat_pwm = 1;
+    wait(0.05);
+    kitty_cat_pwm = 0;
+    wait(0.05);
+    kitty_cat_pwm = 1;
+    wait(0.05);
+    kitty_cat_pwm = 0;
+    wait(0.05);
+    kitty_cat_pwm = 1;
+    wait(0.05);
+    kitty_cat_pwm = 0;
+    wait(0.05);
+    kitty_cat_pwm = 1;
+    wait(0.05);
+    kitty_cat_pwm = 0;
+    wait(0.05);
+    
+    // 3.) Disable the kitty cat
+    kitty_cat_en = 0;
+}
+
+// Set Slide Servo -------------------------------------------------------------
+void set_slide_servo(uint8_t color){
+    if (color == 1){
+        printf("R\r\n");
+        slide.pulsewidth_us(1200);
+    }
+    else if (color == 2){
+        printf("O\r\n");
+        slide.pulsewidth_us(1475);
+    }
+    else if (color == 3){
+        printf("Y\r\n");
+        slide.pulsewidth_us(1750);
+    }
+    else if (color == 4){
+        printf("G\r\n");
+        slide.pulsewidth_us(2025);
+    }
+    else if (color == 5){
+        printf("V\r\n");
+        slide.pulsewidth_us(2300);
+    }
+    else {
+        printf("E\r\n");
+    }
+}
+
+// identify_color --------------------------------------------------------------
+int identify_color(void){
+    // 1.) Compute distances from each color using the clear computation
+    empty_dist = abs((empty_R) - (R)) + abs((empty_G) - (G)) + abs((empty_B) - (B)) + abs((empty_C) - (C));
+    red_dist = abs((red_R) - (R)) + abs((red_G) - (G)) + abs((red_B) - (B)) + abs((red_C) - (C));
+    orange_dist = abs((orange_R) - (R)) + abs((orange_G) - (G)) + abs((orange_B) - (B)) + abs((orange_C) - (C));
+    yellow_dist = abs((yellow_R) - (R)) + abs((yellow_G) - (G)) + abs((yellow_B) - (B)) + abs((yellow_C) - (C));
+    green_dist = abs((green_R) - (R)) + abs((green_G) - (G)) + abs((green_B) - (B)) + abs((green_C) - (C));
+    violet_dist = abs((violet_R) - (R)) + abs((violet_G) - (G)) + abs((violet_B) - (B)) + abs((violet_C) - (C));;
+    int min_dist = 65535;
+    uint8_t min_dist_index = 0;
+    
+    // 2.) Preliminary distance check
+    if (empty_dist < min_dist) {
+        min_dist = empty_dist;
+        min_dist_index = 0;
+    }
+    if (red_dist < min_dist) {
+        min_dist = red_dist;
+        min_dist_index = 1;
+    }
+    if (orange_dist < min_dist) {
+        min_dist = orange_dist;
+        min_dist_index = 2;
+    }
+    if (yellow_dist < min_dist) {
+        min_dist = yellow_dist;
+        min_dist_index = 3;
+    }
+    if (green_dist < min_dist) {
+        min_dist = green_dist;
+        min_dist_index = 4;
+    }
+    if (violet_dist < min_dist) {
+        min_dist = violet_dist;
+        min_dist_index = 5;
+    }
+    
+    // 3.) If orange, yellow, red, or violet was found, do a secondary check
+    /*if ((min_dist_index == 1) || (min_dist_index == 5)) {
+        // a.) Find the minimum red distance
+        violet_dist = abs(abs((violet_R) - (R)) + abs((violet_G) - (G)) + abs((violet_B) - (B)));
+        violet_dist = abs(abs((violet_R) - (R)) + abs((violet_G) - (G)) + abs((violet_B) - (B)));
+        
+        // b.) Decide if it's a violet or red skittle
+        if (violet_R_dist < red_R_dist) {
+            return 5;
+        }
+        else {
+            return 1;
+        }
+    }
+    else if ((min_dist_index == 2) || (min_dist_index == 3) || (min_dist_index == 4)) {
+        // a.) Find the minimum green distance
+        orange_G_dist = abs((orange_G) - (G));
+        yellow_G_dist = abs((yellow_G) - (G));
+        
+        // b.) Decide if it's an orange or yellow skittle
+        if (orange_G_dist < yellow_G_dist) {
+            return 2;
+        }
+        else {
+            return 3;
+        }
+    }*/
+    
+    // 4.) Return minimum distance index
+    return min_dist_index;
+}
+
+// main ------------------------------------------------------------------------
+int main() {
+    // Set all solenoids to 0
+    red = 0;
+    orange = 0;
+    yellow = 0;
+    green = 0;
+    violet = 0;
+    
+    // 1.) Intialize RGB Sensor
+    init_RGB();
+    pc.baud(PC_BAUD);
+    
+    // Test Red Solenoid
+    /*red = 1;
+    wait(0.5);
+    red = 0;
+    wait(0.5);*/
+    
+    // Test Orange Solenoid
+    /*orange = 1;
+    wait(0.5);
+    orange = 0;
+    wait(0.5);*/
+    
+    // Test Yellow Solenoid
+    /*yellow = 1;
+    wait(0.5);
+    yellow = 0;
+    wait(0.5);*/
+    
+    // Test Green Solenoid
+    /*green = 1;
+    wait(0.5);
+    green = 0;
+    wait(0.5);*/
+    
+    // Test Violet Solenoid
+    /*violet = 1;
+    wait(0.5);
+    violet = 0;
+    wait(0.5);*/
+    
+    // Test Slide Servo
+    slide.period_ms(20);
+    slide.pulsewidth_us(1200);
+    wait(1);
+    slide.pulsewidth_us(1475);
+    wait(1);
+    slide.pulsewidth_us(1750);
+    wait(1);
+    slide.pulsewidth_us(2025);
+    wait(1);
+    slide.pulsewidth_us(2300);
+    
+    // Test Kitty Cat
+    stir();
+    stir();
+    stir();
+    
+    // Test Geneva Wheel Stepper Motor
+    geneva.period_ms(10);
+    geneva.pulsewidth_ms(1);
+    
+    uint8_t buff[10];
+    
+    // Identify colors in endless loop
+    while (1) {
+        // A.) Read the RGB sensor 5 times
+        read_RGB();
+        buff[0] = identify_color();
+        read_RGB();
+        buff[1] = identify_color();
+        read_RGB();
+        buff[2] = identify_color();
+        read_RGB();
+        buff[3] = identify_color();
+        read_RGB();
+        buff[4] = identify_color();
+        read_RGB();
+        buff[5] = identify_color();
+        read_RGB();
+        buff[6] = identify_color();
+        read_RGB();
+        buff[7] = identify_color();
+        read_RGB();
+        buff[8] = identify_color();
+        read_RGB();
+        buff[9] = identify_color();
+        
+        // B.) Decide if a skittle was scanned
+        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])) {
+            set_slide_servo(buff[0]);
+            wait(1.5);
+        }
+    }
+}

diff -r 000000000000 -r d4141fa5c9bd mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 15 22:52:32 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e7ca05fa8600
\ No newline at end of file