File content as of revision 1:13d222b50e8e:

/* -----------------------------------------------------------------------------
    Authors: Jared McKneely, Kyle Gong
    Title: Industrial End Node Software v. 2.0.0
    Date: April 23rd, 2018
    
    Description:
    Controls a Skittle-sorting machine. Gathers status of Skittle receptacles
    from gateway, and updates the count upon Skittle detection. Ceases movement
    if a Skittle receptacle is found to be full.
    
----------------------------------------------------------------------------- */

// Libraries -------------------------------------------------------------------
#include "mbed.h"

// Macros ----------------------------------------------------------------------
#define PC_BAUD         (115200) // Baud rate for PC debug
#define XB_BAUD         (57600)  // Baud rate for XBee PRO 900HP communications
#define SCAN_TIME_MS    (10)     // Wait time for skittle scanning
#define TX              (PA_9)   // XBee transmit pin
#define RX              (PA_10)  // XBee receive pin
#define NODE_ID         (99)     // Industrial node ID
#define BUFFER_SIZE     (32)     // Size of XBee buffer

#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 GENEVA_STEP     (D9)     // Geneva wheel stepper motor PWM pin (STEP)
#define GENEVA_PERIOD   (10)     // Period of PWM for geneva wheel stepper motor (ms)
#define GENEVA_PULSE    (1000)   // Pulse width of geneva wheel PWM for rotation (us)
#define GENEVA_STOP     (0)      // Stop pulse width

#define SLIDE_PERIOD    (20)     // Period of PWM for slide servo (ms)
#define SLIDE_PIN       (D3)     // Slide servo PWM pin

#define AMP_PIN         (PC_4)   // Ammeter pin
#define KITTY_CAT_EN    (D7)     // Kitty cat enable/power up pin
#define KITTY_CAT_PWM   (PA_13)  // Kitty cat PWM pin

// Hardware Parameters ---------------------------------------------------------
I2C i2c(I2C_SDA, I2C_SCL);               // Pins for I2C communication (SDA, SCL)
Serial pc(SERIAL_TX, SERIAL_RX);         // Used for debugging
Serial xb(TX, RX);                       // XBee PRO 900HP for communicating with gateway
PwmOut geneva_pwm(GENEVA_STEP);          // PWM for controlling the geneva wheel
PwmOut slide_pwm(SLIDE_PIN);             // PWM for controlling the slide servo
int sensor_addr = 41 << 1;               // RGB sensor I2C address
AnalogIn analog_value(AMP_PIN);          // Current measuring input

DigitalOut r_solenoid(R_DISPENSE);       // Red solenoid
DigitalOut o_solenoid(O_DISPENSE);       // Orange solenoid
DigitalOut y_solenoid(Y_DISPENSE);       // Yellow solenoid
DigitalOut g_solenoid(G_DISPENSE);       // Green solenoid
DigitalOut v_solenoid(V_DISPENSE);       // Violet solenoid

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
Ticker kitty_cat_timer;                  // Creates even intervals

// Globals ---------------------------------------------------------------------
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

bool r_full = true;           // Is the red receptacle full?
bool o_full = true;           // Is the orange receptacle full?
bool y_full = true;           // Is the yellow receptacle full?
bool g_full = true;           // Is the green receptacle full?
bool v_full = true;           // Is the violet receptacle full?

int r_dispense = 0;           // Number of red skittles needing dispensing
int o_dispense = 0;           // Number of orange skittles needing dispensing
int y_dispense = 0;           // Number of yellow skittles needing dispensing
int g_dispense = 0;           // Number of green skittles needing dispensing
int v_dispense = 0;           // Number of violet skittles needing dispensing

bool rx_flag = false;         // Receive flag
bool geneva_spinning = false; // Geneva wheel flag
char xb_buffer[BUFFER_SIZE];  // Contains the XBee's receive buffer
int xb_index = 0;             // Position in the XBee buffer

// Optimal Skittle Color Parameters --------------------------------------------
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;

// Get Number (Helper) ---------------------------------------------------------
/* Returns the number corresponding to an ASCII character. */
int get_number(char c){
    if (c == '1'){
        return 1;    
    }
    if (c == '2'){
        return 2;    
    }
    if (c == '3'){
        return 3;    
    }
    if (c == '4'){
        return 4;    
    }
    if (c == '5'){
        return 5;    
    }
    if (c == '6'){
        return 6;    
    }
    if (c == '7'){
        return 7;    
    }
    if (c == '8'){
        return 8;    
    }
    if (c == '9'){
        return 9;    
    }
    return 0;
}

// Initialize RGB Sensor -------------------------------------------------------
/* Initializes the I2C comm registers on the Adafruit TCS34725. */
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 --------------------------------------------------------------------
/* Reads the current color values from the RGB sensor, stores the values in the
   global variables clear, red, green, and blue. */
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];
}

// 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.) Return minimum distance index
    return min_dist_index;
}

// Set Slide Servo -------------------------------------------------------------
/* Given a character corresponding to a color, repositions the slide to the position
   corresponding to that color's skittle receptacle. */
void set_slide_servo(uint8_t color){
    if (color == 1){
        slide_pwm.pulsewidth_us(1200);
    }
    else if (color == 2){
        slide_pwm.pulsewidth_us(1475);
    }
    else if (color == 3){
        slide_pwm.pulsewidth_us(1750);
    }
    else if (color == 4){
        slide_pwm.pulsewidth_us(2025);
    }
    else if (color == 5){
        slide_pwm.pulsewidth_us(2300);
    }
}

// Buffer Handler --------------------------------------------------------------
/* Extracts information from a message received by the gateway and placed into
   the XBee buffer. */
void parse_buffer(void){
    // 1.) Check red data
    if (xb_buffer[9] == 'n'){
        r_full = false;
    }
    else if (xb_buffer[9] == 'i'){
        r_full = true;
    }
    else{
        r_dispense = r_dispense + get_number(xb_buffer[9]);
    }
    
    // 2.) Check orange receptacle
    if (xb_buffer[14] == 'n'){
        o_full = false;
    }
    else if (xb_buffer[14] == 'i'){
        o_full = true;
    }
    else{
        o_dispense = o_dispense + get_number(xb_buffer[14]);
    }
    
    // 3.) Check yellow receptacle
    if (xb_buffer[19] == 'n'){
        y_full = false;
    }
    else if (xb_buffer[19] == 'i'){
        y_full = true;
    }
    else{
        y_dispense = y_dispense + get_number(xb_buffer[19]);
    }
    
    // 4.) Check green receptacle
    if (xb_buffer[24] == 'n'){
        g_full = false;
    }
    else if (xb_buffer[24] == 'i'){
        g_full = true;
    }
    else{
        g_dispense = g_dispense + get_number(xb_buffer[24]);
    }
    
    // 5.) Check violet receptacle
    if (xb_buffer[29] == 'n'){
        v_full = false;
    }
    else if (xb_buffer[29] == 'i'){
        v_full = true;
    }
    else{
        v_dispense = v_dispense + get_number(xb_buffer[29]);
    }
    
    // 6.) Clear buffer
    memset(xb_buffer, '\0', BUFFER_SIZE);
    rx_flag = false;
    if (geneva_spinning){
        geneva_pwm.pulsewidth_us(GENEVA_PULSE);
    }
}

// Is the Receptacle Full? -----------------------------------------------------
/* Returns the current value of the full receptacle boolean. */
bool is_full(uint8_t color){
    if (color == 1){
        return r_full;
    }
    else if (color == 2){
        return o_full;
    }
    else if (color == 3){
        return y_full;
    }
    else if (color == 4){
        return g_full;
    }
    else if (color == 5){
        return v_full;
    }
    return false;
}

// Increment Count -------------------------------------------------------------
/* Sends a message to the gateway indicating a change in the count of the
   skittle color corresponding to the character sent. */
void incr_count(uint8_t color, int count){
    if (color == 1){
        xb.printf("ni:%d,re:%d,or:0,ye:0,gr:0,pu:0\n", NODE_ID, count);
    }
    else if (color == 2){
        xb.printf("ni:%d,re:0,or:%d,ye:0,gr:0,pu:0\n", NODE_ID, count);
    }
    else if (color == 3){
        xb.printf("ni:%d,re:0,or:0,ye:%d,gr:0,pu:0\n", NODE_ID, count);
    }
    else if (color == 4){
        xb.printf("ni:%d,re:0,or:0,ye:0,gr:%d,pu:0\n", NODE_ID, count);
    }
    else if (color == 5){
        xb.printf("ni:%d,re:0,or:0,ye:0,gr:0,pu:%d\n", NODE_ID, count);
    }
}

// Dispense Skittles -----------------------------------------------------------
/* Controls the solenoid placed at the bottom of each skittle receptacle in order
   to dispense a number of skittles equal to the number currently in its count. */
void dispense_skittles(void){
    if (r_dispense > 0){
        while (r_dispense > 0){
            printf("Red skittle requested. Dispensing.\r\n");
            r_solenoid = 1;
            wait(0.25);
            r_solenoid = 0;
            r_dispense = r_dispense - 1;
        }
        r_full = false;
    }
    if (o_dispense > 0){
        while (o_dispense > 0){
            printf("Orange skittle requested. Dispensing.\r\n");
            o_solenoid = 1;
            wait(0.25);
            o_solenoid = 0;
            o_dispense = o_dispense - 1;
        }
        o_full = false;
    }
    if (y_dispense > 0){
        while (y_dispense > 0){
            printf("Yellow skittle requested. Dispensing.\r\n");
            y_solenoid = 1;
            wait(0.25);
            y_solenoid = 0;
            y_dispense = y_dispense - 1;
        }
        y_full = false;
    }
    if (g_dispense > 0){
        while (g_dispense > 0){
            printf("Green skittle requested. Dispensing.\r\n");
            g_solenoid = 1;
            wait(0.25);
            g_solenoid = 0;
            g_dispense = g_dispense - 1;
        }
        g_full = false;
    }
    if (v_dispense > 0){
        while (v_dispense > 0){
            printf("Violet skittle requested. Dispensing.\r\n");
            v_solenoid = 1;
            wait(0.25);
            v_solenoid = 0;
            v_dispense = v_dispense - 1;
        }
        v_full = false;
    }
}

// Is Jammed -------------------------------------------------------------------
/* Reads the Adafruit INA169 for high current and returns a boolean that
   indicates whether or not the Geneva drive is jammed. */
bool is_jammed() {
    float meas;
    meas = analog_value.read(); //Reads the voltage from ammeter (representatitive of curerent 1:1)
    meas = meas * 5000;         // Change the value to be in the 0 to 5000 range
    //printf("measure = %.2f mA\r\n", meas);
    
    if (meas > 9000) {           // If the value is greater than 600mA oscillate
        printf("Is jammed, attempting to fix...");
        geneva_pwm.pulsewidth_us(GENEVA_STOP);
        wait(10);
        geneva_pwm.pulsewidth_us(GENEVA_PULSE);
        wait(.1);
        return true;
    }
    else {
        return false;
    }
}

// Scan Delay ------------------------------------------------------------------
/* Used to halt skittle counting until a different color reading is confirmed as
   the Geneva drive rotates in between skittles. */
void scan_delay(uint8_t color){
    uint8_t c;
    while (true){
        read_RGB();
        c = identify_color();
        if ((c == 0) || (c != color)){
            return;
        }
        printf("scanning delay..........\r\n");
        c = 0;
        //is_jammed();
    }
}

// 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;
}

// RX Handler ------------------------------------------------------------------
/* Handles receives from the XBee. Copies XBee characters over to xb_buffer. */
void rx_handler(void){
    char c = xb.getc();
    if (xb_index > 31){
        xb_index = 0;
    }
    if (c == '\n'){
        xb_index = 0;
        if ((xb_buffer[3] == '9') && (xb_buffer[4] == '9')){
            rx_flag = true;
        }
    }
    else{
        xb_buffer[xb_index] = c;
        xb_index = xb_index + 1;
    }
}

// Kitty Cat Flip --------------------------------------------------------------
void kitty_cat_flip(void){
    kitty_cat_pwm = !kitty_cat_pwm;
}

// Main ------------------------------------------------------------------------
int main(void) {
// INITIALIZATION --------------------------------------------------------------
    // 1.) Intialize baud rates
    pc.baud(PC_BAUD);
    xb.baud(XB_BAUD);
    printf("IoT Project Industrial Node: Skittle Sorter v. 2.0.0\r\n");
    printf("Authors: Jared McKneely, Kyle Gong\r\n");
    printf("Date of revision: January 12th, 2018\r\n\n");
    printf("Initialized comm baud rates: %d baud debug, %d baud XBee PRO 900HP.\r\n", PC_BAUD, XB_BAUD);
    wait(1);
    
    // 2.) Initialize external hardware
    printf("Initializing external hardware parameters.\r\n");
    slide_pwm.period_ms(SLIDE_PERIOD);
    geneva_pwm.period_ms(GENEVA_PERIOD);
    init_RGB();
    xb.attach(&rx_handler, Serial::RxIrq);
    r_solenoid = 0;
    o_solenoid = 0;
    y_solenoid = 0;
    g_solenoid = 0;
    v_solenoid = 0;
    kitty_cat_en = 0;
    kitty_cat_pwm = 0;
    
    // 3.) Check the receptacles with the gateway
    printf("Contacting gateway, checking receptacles.\r\n");
    while(!rx_flag){
        xb.printf("ni:%d,st:st\n", NODE_ID);
        wait(1);
    }
    parse_buffer();
    rx_flag = false;
    
    // 4.) Create color buffer, booleans identifying color objects
    printf("Initializing software parameters.\r\n");
    uint8_t c_buff[10];
    bool skittle = false;
    int jamCount = 0;     // Stops the motor if checks and is jammed twice consecutive
    int stirCount = 0;    // When it reaches 50, the machine stirs the reservoir
    int stirredCount = 0; // When it reaches 5, the machine stops
    
    // 5.) Test the slide PWM
    slide_pwm.pulsewidth_us(1200);
    wait(1);
    slide_pwm.pulsewidth_us(1475);
    wait(1);
    slide_pwm.pulsewidth_us(1750);
    wait(1);
    slide_pwm.pulsewidth_us(2025);
    wait(1);
    slide_pwm.pulsewidth_us(2300);
    wait(1);
    slide_pwm.pulsewidth_us(1750);

// PROCESS CONTROL -------------------------------------------------------------
    // 5.) Begin scanning cycle
    printf("Beginning scanning routine.\r\n");
    geneva_pwm.pulsewidth_us(GENEVA_PULSE);
    geneva_spinning = true;
    while (true){
        // a.) Check for a jam (ammeter control)
        if (is_jammed()){
            jamCount = jamCount + 1;
        }
        else {
            jamCount = 0;
        }
        if (jamCount == 2){
            geneva_pwm.pulsewidth_us(GENEVA_STOP);
            wait(10);
            geneva_pwm.pulsewidth_us(GENEVA_PULSE);
        }
        
        // b.) Scan for Skittle
        skittle = false;                      // Reset the skittle bool
        while (skittle == false){

            // i.) Scan 10 times for a skittle
            c_buff[0] = identify_color();
            read_RGB();
            c_buff[1] = identify_color();
            read_RGB();
            c_buff[2] = identify_color();
            read_RGB();
            c_buff[3] = identify_color();
            read_RGB();
            c_buff[4] = identify_color();
            read_RGB();
            c_buff[5] = identify_color();
            read_RGB();
            c_buff[6] = identify_color();
            read_RGB();
            c_buff[7] = identify_color();
            read_RGB();
            c_buff[8] = identify_color();
            read_RGB();
            c_buff[9] = identify_color();
            
            // ii.) Determine if all five measurements were the same color, and if it was a skittle
            if ((c_buff[0] == c_buff[1]) && (c_buff[1] == c_buff[2]) && (c_buff[2] == c_buff[3]) && (c_buff[3] == c_buff[4]) && (c_buff[4] == c_buff[5]) && (c_buff[5] == c_buff[6]) && (c_buff[6] == c_buff[7]) && (c_buff[7] == c_buff[8]) && (c_buff[8] == c_buff[9]) && (c_buff[0] != 0)) {
                skittle = true;
                set_slide_servo(c_buff[0]);
                stirCount = 0;
                stirredCount = 0;
            }
            
            // iii.) Check the XBee buffer for parsing
            if (rx_flag){              // If a block of information was received
                parse_buffer();        // Extract the information
            }
            
            // iv.) Handle stirring!
            stirCount = stirCount + 1; // Add to the stir count
            printf("Stir count is %d\r\n", stirCount);
            if ((stirCount >= 50) && (geneva_spinning == true)){ // If the stir count is too high
                stir();                // Stir the 'V' three times
                stir();
                stir();
                stirCount = 0;         // Reset the stir count
                stirredCount = stirredCount + 1;
            }
            else if(stirCount >= 50){
                stirCount = 0;
            }
            dispense_skittles();       // Dispense any skittles
            
            // v.) Handle stirred
            if (stirredCount >= 5){
                geneva_pwm.pulsewidth_us(GENEVA_STOP);
                set_slide_servo(5);
                return 0;
            }
        }
        
        // d.) Increment/full receptacle control
        if (is_full(c_buff[0])){                    // If the receptacle is full, halt the machine temporarily
            printf("%c is full.\r\n", c_buff[0]);     // Print the full receptacle
            geneva_pwm.pulsewidth_us(GENEVA_STOP);  // Stop the geneva wheel
            geneva_spinning = false;                // Set the flag to false
            kitty_cat_en = 0;
        }
        else{                                       // Otherwise, it's not full!
            set_slide_servo(c_buff[0]);             // Set the slide servo for the colored receptacle
            incr_count(c_buff[0], 1);               // Increment the count of that skittle color
            geneva_pwm.pulsewidth_us(GENEVA_PULSE); // Start the geneva wheel
            geneva_spinning = true;                 // Set the flag to true
            kitty_cat_en = 1;
        }
        
        // e.) Dispense any skittles with flags marked, delay until this Skittle is removed from the scanning bay
        if (geneva_spinning){
            scan_delay(c_buff[0]);                    // Delay scan until open air is detected
        }
    }
}