Arrow Intern IoT / Mbed 2 deprecated Industrial_End_Node1

Final industrial end node software

Dependencies:   mbed

main.cpp@1:13d222b50e8e, 2018-05-15 (annotated)

Committer:
jmckneel
Date:
Tue May 15 22:48:20 2018 +0000
Revision:
1:13d222b50e8e
Parent:
0:3845f08fcd02 
Final industrial end node software

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jmckneel 0:3845f08fcd02 1 /* -----------------------------------------------------------------------------
jmckneel 0:3845f08fcd02 2 Authors: Jared McKneely, Kyle Gong
jmckneel 0:3845f08fcd02 3 Title: Industrial End Node Software v. 2.0.0
jmckneel 0:3845f08fcd02 4 Date: April 23rd, 2018
jmckneel 0:3845f08fcd02 5
jmckneel 0:3845f08fcd02 6 Description:
jmckneel 0:3845f08fcd02 7 Controls a Skittle-sorting machine. Gathers status of Skittle receptacles
jmckneel 0:3845f08fcd02 8 from gateway, and updates the count upon Skittle detection. Ceases movement
jmckneel 0:3845f08fcd02 9 if a Skittle receptacle is found to be full.
jmckneel 0:3845f08fcd02 10
jmckneel 0:3845f08fcd02 11 ----------------------------------------------------------------------------- */
jmckneel 0:3845f08fcd02 12
jmckneel 0:3845f08fcd02 13 // Libraries -------------------------------------------------------------------
jmckneel 0:3845f08fcd02 14 #include "mbed.h"
jmckneel 0:3845f08fcd02 15
jmckneel 0:3845f08fcd02 16 // Macros ----------------------------------------------------------------------
jmckneel 0:3845f08fcd02 17 #define PC_BAUD (115200) // Baud rate for PC debug
jmckneel 0:3845f08fcd02 18 #define XB_BAUD (57600) // Baud rate for XBee PRO 900HP communications
jmckneel 0:3845f08fcd02 19 #define SCAN_TIME_MS (10) // Wait time for skittle scanning
jmckneel 0:3845f08fcd02 20 #define TX (PA_9) // XBee transmit pin
jmckneel 0:3845f08fcd02 21 #define RX (PA_10) // XBee receive pin
jmckneel 0:3845f08fcd02 22 #define NODE_ID (99) // Industrial node ID
jmckneel 0:3845f08fcd02 23 #define BUFFER_SIZE (32) // Size of XBee buffer
jmckneel 0:3845f08fcd02 24
jmckneel 0:3845f08fcd02 25 #define R_DISPENSE (PB_2) // Red skittle solenoid
jmckneel 0:3845f08fcd02 26 #define O_DISPENSE (PB_12) // Orange skittle solenoid
jmckneel 0:3845f08fcd02 27 #define Y_DISPENSE (PA_4) // Yellow skittle solenoid
jmckneel 0:3845f08fcd02 28 #define G_DISPENSE (PB_0) // Green skittle solenoid
jmckneel 0:3845f08fcd02 29 #define V_DISPENSE (PC_1) // Violet skittle solenoid
jmckneel 0:3845f08fcd02 30
jmckneel 0:3845f08fcd02 31 #define GENEVA_STEP (D9) // Geneva wheel stepper motor PWM pin (STEP)
jmckneel 0:3845f08fcd02 32 #define GENEVA_PERIOD (10) // Period of PWM for geneva wheel stepper motor (ms)
jmckneel 0:3845f08fcd02 33 #define GENEVA_PULSE (1000) // Pulse width of geneva wheel PWM for rotation (us)
jmckneel 0:3845f08fcd02 34 #define GENEVA_STOP (0) // Stop pulse width
jmckneel 0:3845f08fcd02 35
jmckneel 0:3845f08fcd02 36 #define SLIDE_PERIOD (20) // Period of PWM for slide servo (ms)
jmckneel 0:3845f08fcd02 37 #define SLIDE_PIN (D3) // Slide servo PWM pin
jmckneel 0:3845f08fcd02 38
jmckneel 0:3845f08fcd02 39 #define AMP_PIN (PC_4) // Ammeter pin
jmckneel 0:3845f08fcd02 40 #define KITTY_CAT_EN (D7) // Kitty cat enable/power up pin
jmckneel 0:3845f08fcd02 41 #define KITTY_CAT_PWM (PA_13) // Kitty cat PWM pin
jmckneel 0:3845f08fcd02 42
jmckneel 0:3845f08fcd02 43 // Hardware Parameters ---------------------------------------------------------
jmckneel 0:3845f08fcd02 44 I2C i2c(I2C_SDA, I2C_SCL); // Pins for I2C communication (SDA, SCL)
jmckneel 0:3845f08fcd02 45 Serial pc(SERIAL_TX, SERIAL_RX); // Used for debugging
jmckneel 0:3845f08fcd02 46 Serial xb(TX, RX); // XBee PRO 900HP for communicating with gateway
jmckneel 0:3845f08fcd02 47 PwmOut geneva_pwm(GENEVA_STEP); // PWM for controlling the geneva wheel
jmckneel 0:3845f08fcd02 48 PwmOut slide_pwm(SLIDE_PIN); // PWM for controlling the slide servo
jmckneel 0:3845f08fcd02 49 int sensor_addr = 41 << 1; // RGB sensor I2C address
jmckneel 0:3845f08fcd02 50 AnalogIn analog_value(AMP_PIN); // Current measuring input
jmckneel 0:3845f08fcd02 51
jmckneel 0:3845f08fcd02 52 DigitalOut r_solenoid(R_DISPENSE); // Red solenoid
jmckneel 0:3845f08fcd02 53 DigitalOut o_solenoid(O_DISPENSE); // Orange solenoid
jmckneel 0:3845f08fcd02 54 DigitalOut y_solenoid(Y_DISPENSE); // Yellow solenoid
jmckneel 0:3845f08fcd02 55 DigitalOut g_solenoid(G_DISPENSE); // Green solenoid
jmckneel 0:3845f08fcd02 56 DigitalOut v_solenoid(V_DISPENSE); // Violet solenoid
jmckneel 0:3845f08fcd02 57
jmckneel 0:3845f08fcd02 58 DigitalOut kitty_cat_en(KITTY_CAT_EN); // Enabling pin for the unclogging motor
jmckneel 0:3845f08fcd02 59 DigitalOut kitty_cat_pwm(KITTY_CAT_PWM); // Software PWM pin for the unclogging motor
jmckneel 0:3845f08fcd02 60 Ticker kitty_cat_timer; // Creates even intervals
jmckneel 0:3845f08fcd02 61
jmckneel 0:3845f08fcd02 62 // Globals ---------------------------------------------------------------------
jmckneel 0:3845f08fcd02 63 int C = 0; // Current level of clear
jmckneel 0:3845f08fcd02 64 int R = 0; // Current level of red
jmckneel 0:3845f08fcd02 65 int G = 0; // Current level of green
jmckneel 0:3845f08fcd02 66 int B = 0; // Current level of blue
jmckneel 0:3845f08fcd02 67
jmckneel 0:3845f08fcd02 68 bool r_full = true; // Is the red receptacle full?
jmckneel 0:3845f08fcd02 69 bool o_full = true; // Is the orange receptacle full?
jmckneel 0:3845f08fcd02 70 bool y_full = true; // Is the yellow receptacle full?
jmckneel 0:3845f08fcd02 71 bool g_full = true; // Is the green receptacle full?
jmckneel 0:3845f08fcd02 72 bool v_full = true; // Is the violet receptacle full?
jmckneel 0:3845f08fcd02 73
jmckneel 0:3845f08fcd02 74 int r_dispense = 0; // Number of red skittles needing dispensing
jmckneel 0:3845f08fcd02 75 int o_dispense = 0; // Number of orange skittles needing dispensing
jmckneel 0:3845f08fcd02 76 int y_dispense = 0; // Number of yellow skittles needing dispensing
jmckneel 0:3845f08fcd02 77 int g_dispense = 0; // Number of green skittles needing dispensing
jmckneel 0:3845f08fcd02 78 int v_dispense = 0; // Number of violet skittles needing dispensing
jmckneel 0:3845f08fcd02 79
jmckneel 0:3845f08fcd02 80 bool rx_flag = false; // Receive flag
jmckneel 0:3845f08fcd02 81 bool geneva_spinning = false; // Geneva wheel flag
jmckneel 0:3845f08fcd02 82 char xb_buffer[BUFFER_SIZE]; // Contains the XBee's receive buffer
jmckneel 0:3845f08fcd02 83 int xb_index = 0; // Position in the XBee buffer
jmckneel 0:3845f08fcd02 84
jmckneel 0:3845f08fcd02 85 // Optimal Skittle Color Parameters --------------------------------------------
jmckneel 0:3845f08fcd02 86 uint16_t red_R = 3104;
jmckneel 0:3845f08fcd02 87 uint16_t red_G = 1376;
jmckneel 0:3845f08fcd02 88 uint16_t red_B = 1299;
jmckneel 0:3845f08fcd02 89 uint16_t red_C = 5441;
jmckneel 0:3845f08fcd02 90
jmckneel 0:3845f08fcd02 91 uint16_t orange_R = 6586;
jmckneel 0:3845f08fcd02 92 uint16_t orange_G = 2810;
jmckneel 0:3845f08fcd02 93 uint16_t orange_B = 2014;
jmckneel 0:3845f08fcd02 94 uint16_t orange_C = 11056;
jmckneel 0:3845f08fcd02 95
jmckneel 0:3845f08fcd02 96 uint16_t yellow_R = 7994;
jmckneel 0:3845f08fcd02 97 uint16_t yellow_G = 6247;
jmckneel 0:3845f08fcd02 98 uint16_t yellow_B = 2836;
jmckneel 0:3845f08fcd02 99 uint16_t yellow_C = 16767;
jmckneel 0:3845f08fcd02 100
jmckneel 0:3845f08fcd02 101 uint16_t green_R = 2702;
jmckneel 0:3845f08fcd02 102 uint16_t green_G = 4098;
jmckneel 0:3845f08fcd02 103 uint16_t green_B = 2029;
jmckneel 0:3845f08fcd02 104 uint16_t green_C = 8623;
jmckneel 0:3845f08fcd02 105
jmckneel 0:3845f08fcd02 106 uint16_t violet_R = 1331;
jmckneel 0:3845f08fcd02 107 uint16_t violet_G = 1024;
jmckneel 0:3845f08fcd02 108 uint16_t violet_B = 922;
jmckneel 0:3845f08fcd02 109 uint16_t violet_C = 3148;
jmckneel 0:3845f08fcd02 110
jmckneel 0:3845f08fcd02 111 uint16_t empty_R = 648;
jmckneel 0:3845f08fcd02 112 uint16_t empty_G = 652;
jmckneel 0:3845f08fcd02 113 uint16_t empty_B = 572;
jmckneel 0:3845f08fcd02 114 uint16_t empty_C = 1831;
jmckneel 0:3845f08fcd02 115
jmckneel 0:3845f08fcd02 116 uint16_t empty_dist = 0;
jmckneel 0:3845f08fcd02 117 uint16_t red_dist = 0;
jmckneel 0:3845f08fcd02 118 uint16_t orange_dist = 0;
jmckneel 0:3845f08fcd02 119 uint16_t yellow_dist = 0;
jmckneel 0:3845f08fcd02 120 uint16_t green_dist = 0;
jmckneel 0:3845f08fcd02 121 uint16_t violet_dist = 0;
jmckneel 0:3845f08fcd02 122
jmckneel 0:3845f08fcd02 123 // Get Number (Helper) ---------------------------------------------------------
jmckneel 0:3845f08fcd02 124 /* Returns the number corresponding to an ASCII character. */
jmckneel 0:3845f08fcd02 125 int get_number(char c){
jmckneel 0:3845f08fcd02 126 if (c == '1'){
jmckneel 0:3845f08fcd02 127 return 1;
jmckneel 0:3845f08fcd02 128 }
jmckneel 0:3845f08fcd02 129 if (c == '2'){
jmckneel 0:3845f08fcd02 130 return 2;
jmckneel 0:3845f08fcd02 131 }
jmckneel 0:3845f08fcd02 132 if (c == '3'){
jmckneel 0:3845f08fcd02 133 return 3;
jmckneel 0:3845f08fcd02 134 }
jmckneel 0:3845f08fcd02 135 if (c == '4'){
jmckneel 0:3845f08fcd02 136 return 4;
jmckneel 0:3845f08fcd02 137 }
jmckneel 0:3845f08fcd02 138 if (c == '5'){
jmckneel 0:3845f08fcd02 139 return 5;
jmckneel 0:3845f08fcd02 140 }
jmckneel 0:3845f08fcd02 141 if (c == '6'){
jmckneel 0:3845f08fcd02 142 return 6;
jmckneel 0:3845f08fcd02 143 }
jmckneel 0:3845f08fcd02 144 if (c == '7'){
jmckneel 0:3845f08fcd02 145 return 7;
jmckneel 0:3845f08fcd02 146 }
jmckneel 0:3845f08fcd02 147 if (c == '8'){
jmckneel 0:3845f08fcd02 148 return 8;
jmckneel 0:3845f08fcd02 149 }
jmckneel 0:3845f08fcd02 150 if (c == '9'){
jmckneel 0:3845f08fcd02 151 return 9;
jmckneel 0:3845f08fcd02 152 }
jmckneel 0:3845f08fcd02 153 return 0;
jmckneel 0:3845f08fcd02 154 }
jmckneel 0:3845f08fcd02 155
jmckneel 0:3845f08fcd02 156 // Initialize RGB Sensor -------------------------------------------------------
jmckneel 0:3845f08fcd02 157 /* Initializes the I2C comm registers on the Adafruit TCS34725. */
jmckneel 0:3845f08fcd02 158 void init_RGB(void){
jmckneel 0:3845f08fcd02 159 // 1.) Connect to the color sensor and verify
jmckneel 0:3845f08fcd02 160 i2c.frequency(100000);
jmckneel 0:3845f08fcd02 161 char id_regval[1] = {146};
jmckneel 0:3845f08fcd02 162 char data[1] = {0};
jmckneel 0:3845f08fcd02 163 i2c.write(sensor_addr,id_regval,1, true);
jmckneel 0:3845f08fcd02 164 i2c.read(sensor_addr,data,1,false);
jmckneel 0:3845f08fcd02 165
jmckneel 0:3845f08fcd02 166 // 2.) Initialize color sensor
jmckneel 0:3845f08fcd02 167 char timing_register[2] = {129,0};
jmckneel 0:3845f08fcd02 168 i2c.write(sensor_addr,timing_register,2,false);
jmckneel 0:3845f08fcd02 169 char control_register[2] = {143,0};
jmckneel 0:3845f08fcd02 170 i2c.write(sensor_addr,control_register,2,false);
jmckneel 0:3845f08fcd02 171 char enable_register[2] = {128,3};
jmckneel 0:3845f08fcd02 172 i2c.write(sensor_addr,enable_register,2,false);
jmckneel 0:3845f08fcd02 173 }
jmckneel 0:3845f08fcd02 174
jmckneel 0:3845f08fcd02 175 // Read RGB --------------------------------------------------------------------
jmckneel 0:3845f08fcd02 176 /* Reads the current color values from the RGB sensor, stores the values in the
jmckneel 0:3845f08fcd02 177 global variables clear, red, green, and blue. */
jmckneel 0:3845f08fcd02 178 void read_RGB(void){
jmckneel 0:3845f08fcd02 179 // 1.) Read clear
jmckneel 0:3845f08fcd02 180 char clear_reg[1] = {148};
jmckneel 0:3845f08fcd02 181 char clear_data[2] = {0,0};
jmckneel 0:3845f08fcd02 182 i2c.write(sensor_addr,clear_reg,1, true);
jmckneel 0:3845f08fcd02 183 i2c.read(sensor_addr,clear_data,2, false);
jmckneel 0:3845f08fcd02 184 C = ((int)clear_data[1] << 8) | clear_data[0];
jmckneel 0:3845f08fcd02 185
jmckneel 0:3845f08fcd02 186 // 2.) Read red
jmckneel 0:3845f08fcd02 187 char red_reg[1] = {150};
jmckneel 0:3845f08fcd02 188 char red_data[2] = {0,0};
jmckneel 0:3845f08fcd02 189 i2c.write(sensor_addr,red_reg,1, true);
jmckneel 0:3845f08fcd02 190 i2c.read(sensor_addr,red_data,2, false);
jmckneel 0:3845f08fcd02 191 R = ((int)red_data[1] << 8) | red_data[0];
jmckneel 0:3845f08fcd02 192
jmckneel 0:3845f08fcd02 193 // 3.) Read green
jmckneel 0:3845f08fcd02 194 char green_reg[1] = {152};
jmckneel 0:3845f08fcd02 195 char green_data[2] = {0,0};
jmckneel 0:3845f08fcd02 196 i2c.write(sensor_addr,green_reg,1, true);
jmckneel 0:3845f08fcd02 197 i2c.read(sensor_addr,green_data,2, false);
jmckneel 0:3845f08fcd02 198 G = ((int)green_data[1] << 8) | green_data[0];
jmckneel 0:3845f08fcd02 199
jmckneel 0:3845f08fcd02 200 // 4.) Read blue
jmckneel 0:3845f08fcd02 201 char blue_reg[1] = {154};
jmckneel 0:3845f08fcd02 202 char blue_data[2] = {0,0};
jmckneel 0:3845f08fcd02 203 i2c.write(sensor_addr,blue_reg,1, true);
jmckneel 0:3845f08fcd02 204 i2c.read(sensor_addr,blue_data,2, false);
jmckneel 0:3845f08fcd02 205 B = ((int)blue_data[1] << 8) | blue_data[0];
jmckneel 0:3845f08fcd02 206 }
jmckneel 0:3845f08fcd02 207
jmckneel 0:3845f08fcd02 208 // Identify Color --------------------------------------------------------------
jmckneel 0:3845f08fcd02 209 int identify_color(void){
jmckneel 0:3845f08fcd02 210 // 1.) Compute distances from each color using the clear computation
jmckneel 0:3845f08fcd02 211 empty_dist = abs((empty_R) - (R)) + abs((empty_G) - (G)) + abs((empty_B) - (B)) + abs((empty_C) - (C));
jmckneel 0:3845f08fcd02 212 red_dist = abs((red_R) - (R)) + abs((red_G) - (G)) + abs((red_B) - (B)) + abs((red_C) - (C));
jmckneel 0:3845f08fcd02 213 orange_dist = abs((orange_R) - (R)) + abs((orange_G) - (G)) + abs((orange_B) - (B)) + abs((orange_C) - (C));
jmckneel 0:3845f08fcd02 214 yellow_dist = abs((yellow_R) - (R)) + abs((yellow_G) - (G)) + abs((yellow_B) - (B)) + abs((yellow_C) - (C));
jmckneel 0:3845f08fcd02 215 green_dist = abs((green_R) - (R)) + abs((green_G) - (G)) + abs((green_B) - (B)) + abs((green_C) - (C));
jmckneel 0:3845f08fcd02 216 violet_dist = abs((violet_R) - (R)) + abs((violet_G) - (G)) + abs((violet_B) - (B)) + abs((violet_C) - (C));;
jmckneel 0:3845f08fcd02 217 int min_dist = 65535;
jmckneel 0:3845f08fcd02 218 uint8_t min_dist_index = 0;
jmckneel 0:3845f08fcd02 219
jmckneel 0:3845f08fcd02 220 // 2.) Preliminary distance check
jmckneel 0:3845f08fcd02 221 if (empty_dist < min_dist) {
jmckneel 0:3845f08fcd02 222 min_dist = empty_dist;
jmckneel 0:3845f08fcd02 223 min_dist_index = 0;
jmckneel 0:3845f08fcd02 224 }
jmckneel 0:3845f08fcd02 225 if (red_dist < min_dist) {
jmckneel 0:3845f08fcd02 226 min_dist = red_dist;
jmckneel 0:3845f08fcd02 227 min_dist_index = 1;
jmckneel 0:3845f08fcd02 228 }
jmckneel 0:3845f08fcd02 229 if (orange_dist < min_dist) {
jmckneel 0:3845f08fcd02 230 min_dist = orange_dist;
jmckneel 0:3845f08fcd02 231 min_dist_index = 2;
jmckneel 0:3845f08fcd02 232 }
jmckneel 0:3845f08fcd02 233 if (yellow_dist < min_dist) {
jmckneel 0:3845f08fcd02 234 min_dist = yellow_dist;
jmckneel 0:3845f08fcd02 235 min_dist_index = 3;
jmckneel 0:3845f08fcd02 236 }
jmckneel 0:3845f08fcd02 237 if (green_dist < min_dist) {
jmckneel 0:3845f08fcd02 238 min_dist = green_dist;
jmckneel 0:3845f08fcd02 239 min_dist_index = 4;
jmckneel 0:3845f08fcd02 240 }
jmckneel 0:3845f08fcd02 241 if (violet_dist < min_dist) {
jmckneel 0:3845f08fcd02 242 min_dist = violet_dist;
jmckneel 0:3845f08fcd02 243 min_dist_index = 5;
jmckneel 0:3845f08fcd02 244 }
jmckneel 0:3845f08fcd02 245
jmckneel 0:3845f08fcd02 246 // 3.) Return minimum distance index
jmckneel 0:3845f08fcd02 247 return min_dist_index;
jmckneel 0:3845f08fcd02 248 }
jmckneel 0:3845f08fcd02 249
jmckneel 0:3845f08fcd02 250 // Set Slide Servo -------------------------------------------------------------
jmckneel 0:3845f08fcd02 251 /* Given a character corresponding to a color, repositions the slide to the position
jmckneel 0:3845f08fcd02 252 corresponding to that color's skittle receptacle. */
jmckneel 0:3845f08fcd02 253 void set_slide_servo(uint8_t color){
jmckneel 0:3845f08fcd02 254 if (color == 1){
jmckneel 0:3845f08fcd02 255 slide_pwm.pulsewidth_us(1200);
jmckneel 0:3845f08fcd02 256 }
jmckneel 0:3845f08fcd02 257 else if (color == 2){
jmckneel 0:3845f08fcd02 258 slide_pwm.pulsewidth_us(1475);
jmckneel 0:3845f08fcd02 259 }
jmckneel 0:3845f08fcd02 260 else if (color == 3){
jmckneel 0:3845f08fcd02 261 slide_pwm.pulsewidth_us(1750);
jmckneel 0:3845f08fcd02 262 }
jmckneel 0:3845f08fcd02 263 else if (color == 4){
jmckneel 0:3845f08fcd02 264 slide_pwm.pulsewidth_us(2025);
jmckneel 0:3845f08fcd02 265 }
jmckneel 0:3845f08fcd02 266 else if (color == 5){
jmckneel 0:3845f08fcd02 267 slide_pwm.pulsewidth_us(2300);
jmckneel 0:3845f08fcd02 268 }
jmckneel 0:3845f08fcd02 269 }
jmckneel 0:3845f08fcd02 270
jmckneel 0:3845f08fcd02 271 // Buffer Handler --------------------------------------------------------------
jmckneel 0:3845f08fcd02 272 /* Extracts information from a message received by the gateway and placed into
jmckneel 0:3845f08fcd02 273 the XBee buffer. */
jmckneel 0:3845f08fcd02 274 void parse_buffer(void){
jmckneel 0:3845f08fcd02 275 // 1.) Check red data
jmckneel 0:3845f08fcd02 276 if (xb_buffer[9] == 'n'){
jmckneel 0:3845f08fcd02 277 r_full = false;
jmckneel 0:3845f08fcd02 278 }
jmckneel 0:3845f08fcd02 279 else if (xb_buffer[9] == 'i'){
jmckneel 0:3845f08fcd02 280 r_full = true;
jmckneel 0:3845f08fcd02 281 }
jmckneel 0:3845f08fcd02 282 else{
jmckneel 0:3845f08fcd02 283 r_dispense = r_dispense + get_number(xb_buffer[9]);
jmckneel 0:3845f08fcd02 284 }
jmckneel 0:3845f08fcd02 285
jmckneel 0:3845f08fcd02 286 // 2.) Check orange receptacle
jmckneel 0:3845f08fcd02 287 if (xb_buffer[14] == 'n'){
jmckneel 0:3845f08fcd02 288 o_full = false;
jmckneel 0:3845f08fcd02 289 }
jmckneel 0:3845f08fcd02 290 else if (xb_buffer[14] == 'i'){
jmckneel 0:3845f08fcd02 291 o_full = true;
jmckneel 0:3845f08fcd02 292 }
jmckneel 0:3845f08fcd02 293 else{
jmckneel 0:3845f08fcd02 294 o_dispense = o_dispense + get_number(xb_buffer[14]);
jmckneel 0:3845f08fcd02 295 }
jmckneel 0:3845f08fcd02 296
jmckneel 0:3845f08fcd02 297 // 3.) Check yellow receptacle
jmckneel 0:3845f08fcd02 298 if (xb_buffer[19] == 'n'){
jmckneel 0:3845f08fcd02 299 y_full = false;
jmckneel 0:3845f08fcd02 300 }
jmckneel 0:3845f08fcd02 301 else if (xb_buffer[19] == 'i'){
jmckneel 0:3845f08fcd02 302 y_full = true;
jmckneel 0:3845f08fcd02 303 }
jmckneel 0:3845f08fcd02 304 else{
jmckneel 0:3845f08fcd02 305 y_dispense = y_dispense + get_number(xb_buffer[19]);
jmckneel 0:3845f08fcd02 306 }
jmckneel 0:3845f08fcd02 307
jmckneel 0:3845f08fcd02 308 // 4.) Check green receptacle
jmckneel 0:3845f08fcd02 309 if (xb_buffer[24] == 'n'){
jmckneel 0:3845f08fcd02 310 g_full = false;
jmckneel 0:3845f08fcd02 311 }
jmckneel 0:3845f08fcd02 312 else if (xb_buffer[24] == 'i'){
jmckneel 0:3845f08fcd02 313 g_full = true;
jmckneel 0:3845f08fcd02 314 }
jmckneel 0:3845f08fcd02 315 else{
jmckneel 0:3845f08fcd02 316 g_dispense = g_dispense + get_number(xb_buffer[24]);
jmckneel 0:3845f08fcd02 317 }
jmckneel 0:3845f08fcd02 318
jmckneel 0:3845f08fcd02 319 // 5.) Check violet receptacle
jmckneel 0:3845f08fcd02 320 if (xb_buffer[29] == 'n'){
jmckneel 0:3845f08fcd02 321 v_full = false;
jmckneel 0:3845f08fcd02 322 }
jmckneel 0:3845f08fcd02 323 else if (xb_buffer[29] == 'i'){
jmckneel 0:3845f08fcd02 324 v_full = true;
jmckneel 0:3845f08fcd02 325 }
jmckneel 0:3845f08fcd02 326 else{
jmckneel 0:3845f08fcd02 327 v_dispense = v_dispense + get_number(xb_buffer[29]);
jmckneel 0:3845f08fcd02 328 }
jmckneel 0:3845f08fcd02 329
jmckneel 0:3845f08fcd02 330 // 6.) Clear buffer
jmckneel 0:3845f08fcd02 331 memset(xb_buffer, '\0', BUFFER_SIZE);
jmckneel 0:3845f08fcd02 332 rx_flag = false;
jmckneel 0:3845f08fcd02 333 if (geneva_spinning){
jmckneel 0:3845f08fcd02 334 geneva_pwm.pulsewidth_us(GENEVA_PULSE);
jmckneel 0:3845f08fcd02 335 }
jmckneel 0:3845f08fcd02 336 }
jmckneel 0:3845f08fcd02 337
jmckneel 0:3845f08fcd02 338 // Is the Receptacle Full? -----------------------------------------------------
jmckneel 0:3845f08fcd02 339 /* Returns the current value of the full receptacle boolean. */
jmckneel 0:3845f08fcd02 340 bool is_full(uint8_t color){
jmckneel 0:3845f08fcd02 341 if (color == 1){
jmckneel 0:3845f08fcd02 342 return r_full;
jmckneel 0:3845f08fcd02 343 }
jmckneel 0:3845f08fcd02 344 else if (color == 2){
jmckneel 0:3845f08fcd02 345 return o_full;
jmckneel 0:3845f08fcd02 346 }
jmckneel 0:3845f08fcd02 347 else if (color == 3){
jmckneel 0:3845f08fcd02 348 return y_full;
jmckneel 0:3845f08fcd02 349 }
jmckneel 0:3845f08fcd02 350 else if (color == 4){
jmckneel 0:3845f08fcd02 351 return g_full;
jmckneel 0:3845f08fcd02 352 }
jmckneel 0:3845f08fcd02 353 else if (color == 5){
jmckneel 0:3845f08fcd02 354 return v_full;
jmckneel 0:3845f08fcd02 355 }
jmckneel 0:3845f08fcd02 356 return false;
jmckneel 0:3845f08fcd02 357 }
jmckneel 0:3845f08fcd02 358
jmckneel 0:3845f08fcd02 359 // Increment Count -------------------------------------------------------------
jmckneel 0:3845f08fcd02 360 /* Sends a message to the gateway indicating a change in the count of the
jmckneel 0:3845f08fcd02 361 skittle color corresponding to the character sent. */
jmckneel 0:3845f08fcd02 362 void incr_count(uint8_t color, int count){
jmckneel 0:3845f08fcd02 363 if (color == 1){
jmckneel 0:3845f08fcd02 364 xb.printf("ni:%d,re:%d,or:0,ye:0,gr:0,pu:0\n", NODE_ID, count);
jmckneel 0:3845f08fcd02 365 }
jmckneel 0:3845f08fcd02 366 else if (color == 2){
jmckneel 0:3845f08fcd02 367 xb.printf("ni:%d,re:0,or:%d,ye:0,gr:0,pu:0\n", NODE_ID, count);
jmckneel 0:3845f08fcd02 368 }
jmckneel 0:3845f08fcd02 369 else if (color == 3){
jmckneel 0:3845f08fcd02 370 xb.printf("ni:%d,re:0,or:0,ye:%d,gr:0,pu:0\n", NODE_ID, count);
jmckneel 0:3845f08fcd02 371 }
jmckneel 0:3845f08fcd02 372 else if (color == 4){
jmckneel 0:3845f08fcd02 373 xb.printf("ni:%d,re:0,or:0,ye:0,gr:%d,pu:0\n", NODE_ID, count);
jmckneel 0:3845f08fcd02 374 }
jmckneel 0:3845f08fcd02 375 else if (color == 5){
jmckneel 0:3845f08fcd02 376 xb.printf("ni:%d,re:0,or:0,ye:0,gr:0,pu:%d\n", NODE_ID, count);
jmckneel 0:3845f08fcd02 377 }
jmckneel 0:3845f08fcd02 378 }
jmckneel 0:3845f08fcd02 379
jmckneel 0:3845f08fcd02 380 // Dispense Skittles -----------------------------------------------------------
jmckneel 0:3845f08fcd02 381 /* Controls the solenoid placed at the bottom of each skittle receptacle in order
jmckneel 0:3845f08fcd02 382 to dispense a number of skittles equal to the number currently in its count. */
jmckneel 0:3845f08fcd02 383 void dispense_skittles(void){
jmckneel 0:3845f08fcd02 384 if (r_dispense > 0){
jmckneel 0:3845f08fcd02 385 while (r_dispense > 0){
jmckneel 0:3845f08fcd02 386 printf("Red skittle requested. Dispensing.\r\n");
jmckneel 0:3845f08fcd02 387 r_solenoid = 1;
jmckneel 0:3845f08fcd02 388 wait(0.25);
jmckneel 0:3845f08fcd02 389 r_solenoid = 0;
jmckneel 0:3845f08fcd02 390 r_dispense = r_dispense - 1;
jmckneel 0:3845f08fcd02 391 }
jmckneel 0:3845f08fcd02 392 r_full = false;
jmckneel 0:3845f08fcd02 393 }
jmckneel 0:3845f08fcd02 394 if (o_dispense > 0){
jmckneel 0:3845f08fcd02 395 while (o_dispense > 0){
jmckneel 0:3845f08fcd02 396 printf("Orange skittle requested. Dispensing.\r\n");
jmckneel 0:3845f08fcd02 397 o_solenoid = 1;
jmckneel 0:3845f08fcd02 398 wait(0.25);
jmckneel 0:3845f08fcd02 399 o_solenoid = 0;
jmckneel 0:3845f08fcd02 400 o_dispense = o_dispense - 1;
jmckneel 0:3845f08fcd02 401 }
jmckneel 0:3845f08fcd02 402 o_full = false;
jmckneel 0:3845f08fcd02 403 }
jmckneel 0:3845f08fcd02 404 if (y_dispense > 0){
jmckneel 0:3845f08fcd02 405 while (y_dispense > 0){
jmckneel 0:3845f08fcd02 406 printf("Yellow skittle requested. Dispensing.\r\n");
jmckneel 0:3845f08fcd02 407 y_solenoid = 1;
jmckneel 0:3845f08fcd02 408 wait(0.25);
jmckneel 0:3845f08fcd02 409 y_solenoid = 0;
jmckneel 0:3845f08fcd02 410 y_dispense = y_dispense - 1;
jmckneel 0:3845f08fcd02 411 }
jmckneel 0:3845f08fcd02 412 y_full = false;
jmckneel 0:3845f08fcd02 413 }
jmckneel 0:3845f08fcd02 414 if (g_dispense > 0){
jmckneel 0:3845f08fcd02 415 while (g_dispense > 0){
jmckneel 0:3845f08fcd02 416 printf("Green skittle requested. Dispensing.\r\n");
jmckneel 0:3845f08fcd02 417 g_solenoid = 1;
jmckneel 0:3845f08fcd02 418 wait(0.25);
jmckneel 0:3845f08fcd02 419 g_solenoid = 0;
jmckneel 0:3845f08fcd02 420 g_dispense = g_dispense - 1;
jmckneel 0:3845f08fcd02 421 }
jmckneel 0:3845f08fcd02 422 g_full = false;
jmckneel 0:3845f08fcd02 423 }
jmckneel 0:3845f08fcd02 424 if (v_dispense > 0){
jmckneel 0:3845f08fcd02 425 while (v_dispense > 0){
jmckneel 0:3845f08fcd02 426 printf("Violet skittle requested. Dispensing.\r\n");
jmckneel 0:3845f08fcd02 427 v_solenoid = 1;
jmckneel 0:3845f08fcd02 428 wait(0.25);
jmckneel 0:3845f08fcd02 429 v_solenoid = 0;
jmckneel 0:3845f08fcd02 430 v_dispense = v_dispense - 1;
jmckneel 0:3845f08fcd02 431 }
jmckneel 0:3845f08fcd02 432 v_full = false;
jmckneel 0:3845f08fcd02 433 }
jmckneel 0:3845f08fcd02 434 }
jmckneel 0:3845f08fcd02 435
jmckneel 0:3845f08fcd02 436 // Is Jammed -------------------------------------------------------------------
jmckneel 0:3845f08fcd02 437 /* Reads the Adafruit INA169 for high current and returns a boolean that
jmckneel 0:3845f08fcd02 438 indicates whether or not the Geneva drive is jammed. */
jmckneel 0:3845f08fcd02 439 bool is_jammed() {
jmckneel 0:3845f08fcd02 440 float meas;
jmckneel 0:3845f08fcd02 441 meas = analog_value.read(); //Reads the voltage from ammeter (representatitive of curerent 1:1)
jmckneel 0:3845f08fcd02 442 meas = meas * 5000; // Change the value to be in the 0 to 5000 range
jmckneel 0:3845f08fcd02 443 //printf("measure = %.2f mA\r\n", meas);
jmckneel 0:3845f08fcd02 444
jmckneel 0:3845f08fcd02 445 if (meas > 9000) { // If the value is greater than 600mA oscillate
jmckneel 0:3845f08fcd02 446 printf("Is jammed, attempting to fix...");
jmckneel 0:3845f08fcd02 447 geneva_pwm.pulsewidth_us(GENEVA_STOP);
jmckneel 0:3845f08fcd02 448 wait(10);
jmckneel 0:3845f08fcd02 449 geneva_pwm.pulsewidth_us(GENEVA_PULSE);
jmckneel 0:3845f08fcd02 450 wait(.1);
jmckneel 0:3845f08fcd02 451 return true;
jmckneel 0:3845f08fcd02 452 }
jmckneel 0:3845f08fcd02 453 else {
jmckneel 0:3845f08fcd02 454 return false;
jmckneel 0:3845f08fcd02 455 }
jmckneel 0:3845f08fcd02 456 }
jmckneel 0:3845f08fcd02 457
jmckneel 0:3845f08fcd02 458 // Scan Delay ------------------------------------------------------------------
jmckneel 0:3845f08fcd02 459 /* Used to halt skittle counting until a different color reading is confirmed as
jmckneel 0:3845f08fcd02 460 the Geneva drive rotates in between skittles. */
jmckneel 0:3845f08fcd02 461 void scan_delay(uint8_t color){
jmckneel 0:3845f08fcd02 462 uint8_t c;
jmckneel 0:3845f08fcd02 463 while (true){
jmckneel 0:3845f08fcd02 464 read_RGB();
jmckneel 0:3845f08fcd02 465 c = identify_color();
jmckneel 0:3845f08fcd02 466 if ((c == 0) || (c != color)){
jmckneel 0:3845f08fcd02 467 return;
jmckneel 0:3845f08fcd02 468 }
jmckneel 0:3845f08fcd02 469 printf("scanning delay..........\r\n");
jmckneel 0:3845f08fcd02 470 c = 0;
jmckneel 0:3845f08fcd02 471 //is_jammed();
jmckneel 0:3845f08fcd02 472 }
jmckneel 0:3845f08fcd02 473 }
jmckneel 0:3845f08fcd02 474
jmckneel 0:3845f08fcd02 475 // Stir ------------------------------------------------------------------------
jmckneel 0:3845f08fcd02 476 /* Calls on the kitty cat to scratch around and stir the skittle reservoir,
jmckneel 0:3845f08fcd02 477 which unclogs the reservoir and vaguely resembles a feline-like action. */
jmckneel 0:3845f08fcd02 478 void stir(void){
jmckneel 0:3845f08fcd02 479 // 1.) Enable the kitty cat
jmckneel 0:3845f08fcd02 480 kitty_cat_en = 1;
jmckneel 0:3845f08fcd02 481
jmckneel 0:3845f08fcd02 482 // 2.) Create software PWM for 10 cycles
jmckneel 0:3845f08fcd02 483 kitty_cat_pwm = 1;
jmckneel 0:3845f08fcd02 484 wait(0.05);
jmckneel 0:3845f08fcd02 485 kitty_cat_pwm = 0;
jmckneel 0:3845f08fcd02 486 wait(0.05);
jmckneel 0:3845f08fcd02 487 kitty_cat_pwm = 1;
jmckneel 0:3845f08fcd02 488 wait(0.05);
jmckneel 0:3845f08fcd02 489 kitty_cat_pwm = 0;
jmckneel 0:3845f08fcd02 490 wait(0.05);
jmckneel 0:3845f08fcd02 491 kitty_cat_pwm = 1;
jmckneel 0:3845f08fcd02 492 wait(0.05);
jmckneel 0:3845f08fcd02 493 kitty_cat_pwm = 0;
jmckneel 0:3845f08fcd02 494 wait(0.05);
jmckneel 0:3845f08fcd02 495 kitty_cat_pwm = 1;
jmckneel 0:3845f08fcd02 496 wait(0.05);
jmckneel 0:3845f08fcd02 497 kitty_cat_pwm = 0;
jmckneel 0:3845f08fcd02 498 wait(0.05);
jmckneel 0:3845f08fcd02 499 kitty_cat_pwm = 1;
jmckneel 0:3845f08fcd02 500 wait(0.05);
jmckneel 0:3845f08fcd02 501 kitty_cat_pwm = 0;
jmckneel 0:3845f08fcd02 502 wait(0.05);
jmckneel 0:3845f08fcd02 503
jmckneel 0:3845f08fcd02 504 // 3.) Disable the kitty cat
jmckneel 0:3845f08fcd02 505 kitty_cat_en = 0;
jmckneel 0:3845f08fcd02 506 }
jmckneel 0:3845f08fcd02 507
jmckneel 0:3845f08fcd02 508 // RX Handler ------------------------------------------------------------------
jmckneel 0:3845f08fcd02 509 /* Handles receives from the XBee. Copies XBee characters over to xb_buffer. */
jmckneel 0:3845f08fcd02 510 void rx_handler(void){
jmckneel 0:3845f08fcd02 511 char c = xb.getc();
jmckneel 0:3845f08fcd02 512 if (xb_index > 31){
jmckneel 0:3845f08fcd02 513 xb_index = 0;
jmckneel 0:3845f08fcd02 514 }
jmckneel 0:3845f08fcd02 515 if (c == '\n'){
jmckneel 0:3845f08fcd02 516 xb_index = 0;
jmckneel 0:3845f08fcd02 517 if ((xb_buffer[3] == '9') && (xb_buffer[4] == '9')){
jmckneel 0:3845f08fcd02 518 rx_flag = true;
jmckneel 0:3845f08fcd02 519 }
jmckneel 0:3845f08fcd02 520 }
jmckneel 0:3845f08fcd02 521 else{
jmckneel 0:3845f08fcd02 522 xb_buffer[xb_index] = c;
jmckneel 0:3845f08fcd02 523 xb_index = xb_index + 1;
jmckneel 0:3845f08fcd02 524 }
jmckneel 0:3845f08fcd02 525 }
jmckneel 0:3845f08fcd02 526
jmckneel 0:3845f08fcd02 527 // Kitty Cat Flip --------------------------------------------------------------
jmckneel 0:3845f08fcd02 528 void kitty_cat_flip(void){
jmckneel 0:3845f08fcd02 529 kitty_cat_pwm = !kitty_cat_pwm;
jmckneel 0:3845f08fcd02 530 }
jmckneel 0:3845f08fcd02 531
jmckneel 0:3845f08fcd02 532 // Main ------------------------------------------------------------------------
jmckneel 0:3845f08fcd02 533 int main(void) {
jmckneel 0:3845f08fcd02 534 // INITIALIZATION --------------------------------------------------------------
jmckneel 0:3845f08fcd02 535 // 1.) Intialize baud rates
jmckneel 0:3845f08fcd02 536 pc.baud(PC_BAUD);
jmckneel 0:3845f08fcd02 537 xb.baud(XB_BAUD);
jmckneel 0:3845f08fcd02 538 printf("IoT Project Industrial Node: Skittle Sorter v. 2.0.0\r\n");
jmckneel 0:3845f08fcd02 539 printf("Authors: Jared McKneely, Kyle Gong\r\n");
jmckneel 0:3845f08fcd02 540 printf("Date of revision: January 12th, 2018\r\n\n");
jmckneel 0:3845f08fcd02 541 printf("Initialized comm baud rates: %d baud debug, %d baud XBee PRO 900HP.\r\n", PC_BAUD, XB_BAUD);
jmckneel 0:3845f08fcd02 542 wait(1);
jmckneel 0:3845f08fcd02 543
jmckneel 0:3845f08fcd02 544 // 2.) Initialize external hardware
jmckneel 0:3845f08fcd02 545 printf("Initializing external hardware parameters.\r\n");
jmckneel 0:3845f08fcd02 546 slide_pwm.period_ms(SLIDE_PERIOD);
jmckneel 0:3845f08fcd02 547 geneva_pwm.period_ms(GENEVA_PERIOD);
jmckneel 0:3845f08fcd02 548 init_RGB();
jmckneel 0:3845f08fcd02 549 xb.attach(&rx_handler, Serial::RxIrq);
jmckneel 0:3845f08fcd02 550 r_solenoid = 0;
jmckneel 0:3845f08fcd02 551 o_solenoid = 0;
jmckneel 0:3845f08fcd02 552 y_solenoid = 0;
jmckneel 0:3845f08fcd02 553 g_solenoid = 0;
jmckneel 0:3845f08fcd02 554 v_solenoid = 0;
jmckneel 0:3845f08fcd02 555 kitty_cat_en = 0;
jmckneel 0:3845f08fcd02 556 kitty_cat_pwm = 0;
jmckneel 0:3845f08fcd02 557
jmckneel 0:3845f08fcd02 558 // 3.) Check the receptacles with the gateway
jmckneel 0:3845f08fcd02 559 printf("Contacting gateway, checking receptacles.\r\n");
jmckneel 0:3845f08fcd02 560 while(!rx_flag){
jmckneel 0:3845f08fcd02 561 xb.printf("ni:%d,st:st\n", NODE_ID);
jmckneel 0:3845f08fcd02 562 wait(1);
jmckneel 0:3845f08fcd02 563 }
jmckneel 0:3845f08fcd02 564 parse_buffer();
jmckneel 0:3845f08fcd02 565 rx_flag = false;
jmckneel 0:3845f08fcd02 566
jmckneel 0:3845f08fcd02 567 // 4.) Create color buffer, booleans identifying color objects
jmckneel 0:3845f08fcd02 568 printf("Initializing software parameters.\r\n");
jmckneel 0:3845f08fcd02 569 uint8_t c_buff[10];
jmckneel 0:3845f08fcd02 570 bool skittle = false;
jmckneel 1:13d222b50e8e 571 int jamCount = 0; // Stops the motor if checks and is jammed twice consecutive
jmckneel 1:13d222b50e8e 572 int stirCount = 0; // When it reaches 50, the machine stirs the reservoir
jmckneel 1:13d222b50e8e 573 int stirredCount = 0; // When it reaches 5, the machine stops
jmckneel 0:3845f08fcd02 574
jmckneel 0:3845f08fcd02 575 // 5.) Test the slide PWM
jmckneel 0:3845f08fcd02 576 slide_pwm.pulsewidth_us(1200);
jmckneel 0:3845f08fcd02 577 wait(1);
jmckneel 0:3845f08fcd02 578 slide_pwm.pulsewidth_us(1475);
jmckneel 0:3845f08fcd02 579 wait(1);
jmckneel 0:3845f08fcd02 580 slide_pwm.pulsewidth_us(1750);
jmckneel 0:3845f08fcd02 581 wait(1);
jmckneel 0:3845f08fcd02 582 slide_pwm.pulsewidth_us(2025);
jmckneel 0:3845f08fcd02 583 wait(1);
jmckneel 0:3845f08fcd02 584 slide_pwm.pulsewidth_us(2300);
jmckneel 0:3845f08fcd02 585 wait(1);
jmckneel 0:3845f08fcd02 586 slide_pwm.pulsewidth_us(1750);
jmckneel 0:3845f08fcd02 587
jmckneel 0:3845f08fcd02 588 // PROCESS CONTROL -------------------------------------------------------------
jmckneel 0:3845f08fcd02 589 // 5.) Begin scanning cycle
jmckneel 0:3845f08fcd02 590 printf("Beginning scanning routine.\r\n");
jmckneel 0:3845f08fcd02 591 geneva_pwm.pulsewidth_us(GENEVA_PULSE);
jmckneel 0:3845f08fcd02 592 geneva_spinning = true;
jmckneel 0:3845f08fcd02 593 while (true){
jmckneel 0:3845f08fcd02 594 // a.) Check for a jam (ammeter control)
jmckneel 0:3845f08fcd02 595 if (is_jammed()){
jmckneel 0:3845f08fcd02 596 jamCount = jamCount + 1;
jmckneel 0:3845f08fcd02 597 }
jmckneel 0:3845f08fcd02 598 else {
jmckneel 0:3845f08fcd02 599 jamCount = 0;
jmckneel 0:3845f08fcd02 600 }
jmckneel 0:3845f08fcd02 601 if (jamCount == 2){
jmckneel 0:3845f08fcd02 602 geneva_pwm.pulsewidth_us(GENEVA_STOP);
jmckneel 0:3845f08fcd02 603 wait(10);
jmckneel 0:3845f08fcd02 604 geneva_pwm.pulsewidth_us(GENEVA_PULSE);
jmckneel 0:3845f08fcd02 605 }
jmckneel 0:3845f08fcd02 606
jmckneel 0:3845f08fcd02 607 // b.) Scan for Skittle
jmckneel 0:3845f08fcd02 608 skittle = false; // Reset the skittle bool
jmckneel 0:3845f08fcd02 609 while (skittle == false){
jmckneel 0:3845f08fcd02 610
jmckneel 0:3845f08fcd02 611 // i.) Scan 10 times for a skittle
jmckneel 0:3845f08fcd02 612 c_buff[0] = identify_color();
jmckneel 0:3845f08fcd02 613 read_RGB();
jmckneel 0:3845f08fcd02 614 c_buff[1] = identify_color();
jmckneel 0:3845f08fcd02 615 read_RGB();
jmckneel 0:3845f08fcd02 616 c_buff[2] = identify_color();
jmckneel 0:3845f08fcd02 617 read_RGB();
jmckneel 0:3845f08fcd02 618 c_buff[3] = identify_color();
jmckneel 0:3845f08fcd02 619 read_RGB();
jmckneel 0:3845f08fcd02 620 c_buff[4] = identify_color();
jmckneel 0:3845f08fcd02 621 read_RGB();
jmckneel 0:3845f08fcd02 622 c_buff[5] = identify_color();
jmckneel 0:3845f08fcd02 623 read_RGB();
jmckneel 0:3845f08fcd02 624 c_buff[6] = identify_color();
jmckneel 0:3845f08fcd02 625 read_RGB();
jmckneel 0:3845f08fcd02 626 c_buff[7] = identify_color();
jmckneel 0:3845f08fcd02 627 read_RGB();
jmckneel 0:3845f08fcd02 628 c_buff[8] = identify_color();
jmckneel 0:3845f08fcd02 629 read_RGB();
jmckneel 0:3845f08fcd02 630 c_buff[9] = identify_color();
jmckneel 0:3845f08fcd02 631
jmckneel 0:3845f08fcd02 632 // ii.) Determine if all five measurements were the same color, and if it was a skittle
jmckneel 0:3845f08fcd02 633 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)) {
jmckneel 0:3845f08fcd02 634 skittle = true;
jmckneel 0:3845f08fcd02 635 set_slide_servo(c_buff[0]);
jmckneel 0:3845f08fcd02 636 stirCount = 0;
jmckneel 1:13d222b50e8e 637 stirredCount = 0;
jmckneel 0:3845f08fcd02 638 }
jmckneel 0:3845f08fcd02 639
jmckneel 0:3845f08fcd02 640 // iii.) Check the XBee buffer for parsing
jmckneel 0:3845f08fcd02 641 if (rx_flag){ // If a block of information was received
jmckneel 0:3845f08fcd02 642 parse_buffer(); // Extract the information
jmckneel 0:3845f08fcd02 643 }
jmckneel 0:3845f08fcd02 644
jmckneel 0:3845f08fcd02 645 // iv.) Handle stirring!
jmckneel 0:3845f08fcd02 646 stirCount = stirCount + 1; // Add to the stir count
jmckneel 0:3845f08fcd02 647 printf("Stir count is %d\r\n", stirCount);
jmckneel 0:3845f08fcd02 648 if ((stirCount >= 50) && (geneva_spinning == true)){ // If the stir count is too high
jmckneel 0:3845f08fcd02 649 stir(); // Stir the 'V' three times
jmckneel 0:3845f08fcd02 650 stir();
jmckneel 0:3845f08fcd02 651 stir();
jmckneel 0:3845f08fcd02 652 stirCount = 0; // Reset the stir count
jmckneel 1:13d222b50e8e 653 stirredCount = stirredCount + 1;
jmckneel 0:3845f08fcd02 654 }
jmckneel 0:3845f08fcd02 655 else if(stirCount >= 50){
jmckneel 0:3845f08fcd02 656 stirCount = 0;
jmckneel 0:3845f08fcd02 657 }
jmckneel 0:3845f08fcd02 658 dispense_skittles(); // Dispense any skittles
jmckneel 1:13d222b50e8e 659
jmckneel 1:13d222b50e8e 660 // v.) Handle stirred
jmckneel 1:13d222b50e8e 661 if (stirredCount >= 5){
jmckneel 1:13d222b50e8e 662 geneva_pwm.pulsewidth_us(GENEVA_STOP);
jmckneel 1:13d222b50e8e 663 set_slide_servo(5);
jmckneel 1:13d222b50e8e 664 return 0;
jmckneel 1:13d222b50e8e 665 }
jmckneel 0:3845f08fcd02 666 }
jmckneel 0:3845f08fcd02 667
jmckneel 0:3845f08fcd02 668 // d.) Increment/full receptacle control
jmckneel 0:3845f08fcd02 669 if (is_full(c_buff[0])){ // If the receptacle is full, halt the machine temporarily
jmckneel 0:3845f08fcd02 670 printf("%c is full.\r\n", c_buff[0]); // Print the full receptacle
jmckneel 0:3845f08fcd02 671 geneva_pwm.pulsewidth_us(GENEVA_STOP); // Stop the geneva wheel
jmckneel 0:3845f08fcd02 672 geneva_spinning = false; // Set the flag to false
jmckneel 0:3845f08fcd02 673 kitty_cat_en = 0;
jmckneel 0:3845f08fcd02 674 }
jmckneel 0:3845f08fcd02 675 else{ // Otherwise, it's not full!
jmckneel 0:3845f08fcd02 676 set_slide_servo(c_buff[0]); // Set the slide servo for the colored receptacle
jmckneel 0:3845f08fcd02 677 incr_count(c_buff[0], 1); // Increment the count of that skittle color
jmckneel 0:3845f08fcd02 678 geneva_pwm.pulsewidth_us(GENEVA_PULSE); // Start the geneva wheel
jmckneel 0:3845f08fcd02 679 geneva_spinning = true; // Set the flag to true
jmckneel 0:3845f08fcd02 680 kitty_cat_en = 1;
jmckneel 0:3845f08fcd02 681 }
jmckneel 0:3845f08fcd02 682
jmckneel 0:3845f08fcd02 683 // e.) Dispense any skittles with flags marked, delay until this Skittle is removed from the scanning bay
jmckneel 0:3845f08fcd02 684 if (geneva_spinning){
jmckneel 0:3845f08fcd02 685 scan_delay(c_buff[0]); // Delay scan until open air is detected
jmckneel 0:3845f08fcd02 686 }
jmckneel 0:3845f08fcd02 687 }
jmckneel 0:3845f08fcd02 688 }