Ion Systems / Mbed 2 deprecated Chipin_Main

Dependencies:   MCP23017 TCS3472_I2C WattBob_TextLCD mbed-rtos mbed

Committer:
IonSystems
Date:
Tue Nov 25 12:06:26 2014 +0000
Revision:
19:78d4b78fa736
Parent:
17:6a0bb0ad5bb4
Child:
20:6de191ac7ff3
Added lift operation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IonSystems 6:e64796f1f384 1 #include "mbed.h"
IonSystems 11:06f6e82b40a8 2 //#include "Colour.h"
IonSystems 11:06f6e82b40a8 3 #include "colourProcessing.h"
IonSystems 6:e64796f1f384 4 //Setup pins to FPGA
IonSystems 6:e64796f1f384 5 DigitalOut startSort(p5); //A positive edge tells the FPGA to start sorting.
IonSystems 10:8c0696b99692 6 DigitalIn sortComplete(p16);
IonSystems 6:e64796f1f384 7 /*
IonSystems 6:e64796f1f384 8 The command codes for the three sort select bits(ColourBit1 - ColourBit2)
IonSystems 17:6a0bb0ad5bb4 9 See datasheet for furter information
IonSystems 6:e64796f1f384 10 000 Sort red chip
IonSystems 6:e64796f1f384 11 001 Sort green chip
IonSystems 6:e64796f1f384 12 010 Sort blue chip
IonSystems 6:e64796f1f384 13 011 Bin the chip
IonSystems 6:e64796f1f384 14 100 Recycle the chip
IonSystems 6:e64796f1f384 15 101 Nothing
IonSystems 6:e64796f1f384 16 110 Nothing
IonSystems 6:e64796f1f384 17 111 Nothing
IonSystems 6:e64796f1f384 18 */
IonSystems 6:e64796f1f384 19
IonSystems 6:e64796f1f384 20 DigitalOut colourBit1(p6); //The 3 bits below are select bits for the sorter.
IonSystems 6:e64796f1f384 21 DigitalOut colourBit2(p7);
IonSystems 6:e64796f1f384 22 DigitalOut colourBit3(p8);
IonSystems 9:8d78eb55ad5e 23 DigitalOut startDispense(p18); //A positive edge tells the FPGA to start dispensing. p9
IonSystems 6:e64796f1f384 24 /*
IonSystems 6:e64796f1f384 25 00 Dispense red chip
IonSystems 6:e64796f1f384 26 01 Dispense green chip
IonSystems 6:e64796f1f384 27 10 Dispense blue chip
IonSystems 6:e64796f1f384 28 11 Nothing
IonSystems 6:e64796f1f384 29 */
IonSystems 6:e64796f1f384 30
IonSystems 9:8d78eb55ad5e 31 DigitalOut dispenseBit1(p19); //The 2 bits below are select bits for the dispenser.p10
IonSystems 9:8d78eb55ad5e 32 DigitalOut dispenseBit2(p20); //p11
IonSystems 8:b7771391adc9 33
IonSystems 12:814a8fdbb6f7 34 DigitalIn dispenseComplete(p11); //FPGA sets to 1 once complete until another signal is recieved.
IonSystems 10:8c0696b99692 35 DigitalOut select(p15); //0 for control, 1 for maintenance.
IonSystems 10:8c0696b99692 36
IonSystems 12:814a8fdbb6f7 37
IonSystems 19:78d4b78fa736 38 void setMaintenanceSelect(bool bit6, bool bit5, bool bit4, bool bit3, bool bit2, bool bit1)
IonSystems 19:78d4b78fa736 39 {
IonSystems 12:814a8fdbb6f7 40 startSort = bit1;
IonSystems 12:814a8fdbb6f7 41 colourBit1 = bit2;
IonSystems 12:814a8fdbb6f7 42 colourBit2 = bit3;
IonSystems 12:814a8fdbb6f7 43 colourBit3 = bit4;
IonSystems 12:814a8fdbb6f7 44 startDispense = bit5;
IonSystems 12:814a8fdbb6f7 45 dispenseBit1 = bit6;
IonSystems 19:78d4b78fa736 46 }
IonSystems 19:78d4b78fa736 47
IonSystems 19:78d4b78fa736 48 void setMaintenanceStart(bool value)
IonSystems 19:78d4b78fa736 49 {
IonSystems 12:814a8fdbb6f7 50 dispenseBit2 = value;
IonSystems 19:78d4b78fa736 51 }
IonSystems 19:78d4b78fa736 52 void waitForMaintenanceComplete()
IonSystems 19:78d4b78fa736 53 {
IonSystems 19:78d4b78fa736 54 while(!sortComplete) {
IonSystems 14:31ba3e56c788 55 printLCD("Waiting for REC");
IonSystems 12:814a8fdbb6f7 56 wait(0.1);
IonSystems 19:78d4b78fa736 57 }
IonSystems 19:78d4b78fa736 58 }
IonSystems 19:78d4b78fa736 59
IonSystems 19:78d4b78fa736 60 void setDispenseSelect(bool dBit1, bool dBit2)
IonSystems 19:78d4b78fa736 61 {
IonSystems 19:78d4b78fa736 62 dispenseBit1 = dBit1;
IonSystems 19:78d4b78fa736 63 dispenseBit2 = dBit2;
IonSystems 12:814a8fdbb6f7 64 }
IonSystems 19:78d4b78fa736 65
IonSystems 19:78d4b78fa736 66 void setSortSelect(bool sBit1, bool sBit2, bool sBit3)
IonSystems 19:78d4b78fa736 67 {
IonSystems 19:78d4b78fa736 68 colourBit1 = sBit1;
IonSystems 19:78d4b78fa736 69 colourBit2 = sBit2;
IonSystems 19:78d4b78fa736 70 colourBit3 = sBit3;
IonSystems 19:78d4b78fa736 71 }
IonSystems 19:78d4b78fa736 72
IonSystems 19:78d4b78fa736 73 void maintain(StateMachine maintainState)
IonSystems 19:78d4b78fa736 74 {
IonSystems 12:814a8fdbb6f7 75 select = 1; //setting FPGA to maintenance mode.
IonSystems 19:78d4b78fa736 76 setMaintenanceSelect(true,true,true,true,true,true);
IonSystems 19:78d4b78fa736 77 switch(maintainState) {
IonSystems 12:814a8fdbb6f7 78 case RB_LEFT:
IonSystems 12:814a8fdbb6f7 79 setMaintenanceSelect(false,false,false,true,false,true);
IonSystems 19:78d4b78fa736 80 break;
IonSystems 12:814a8fdbb6f7 81 case RB_CENTRE:
IonSystems 12:814a8fdbb6f7 82 setMaintenanceSelect(false,false,false,true,true,false);
IonSystems 19:78d4b78fa736 83 break;
IonSystems 12:814a8fdbb6f7 84 case RB_RIGHT:
IonSystems 12:814a8fdbb6f7 85 setMaintenanceSelect(false,false,false,true,true,true);
IonSystems 19:78d4b78fa736 86 break;
IonSystems 12:814a8fdbb6f7 87 case GO_UP:
IonSystems 12:814a8fdbb6f7 88 setMaintenanceSelect(false,false,true,false,false,false);
IonSystems 19:78d4b78fa736 89 break;
IonSystems 12:814a8fdbb6f7 90 case GO_CENTRE:
IonSystems 12:814a8fdbb6f7 91 setMaintenanceSelect(false,false,true,false,false,true);
IonSystems 19:78d4b78fa736 92 break;
IonSystems 12:814a8fdbb6f7 93 case GO_DOWN:
IonSystems 12:814a8fdbb6f7 94 setMaintenanceSelect(false,false,true,false,true,false);
IonSystems 19:78d4b78fa736 95 break;
IonSystems 12:814a8fdbb6f7 96 case BR_LEFT:
IonSystems 12:814a8fdbb6f7 97 setMaintenanceSelect(false,false,true,false,true,true);
IonSystems 19:78d4b78fa736 98 break;
IonSystems 12:814a8fdbb6f7 99 case BR_RIGHT:
IonSystems 12:814a8fdbb6f7 100 setMaintenanceSelect(false,false,true,true,false,false);
IonSystems 19:78d4b78fa736 101 break;
IonSystems 12:814a8fdbb6f7 102 case R_PUSH:
IonSystems 12:814a8fdbb6f7 103 setMaintenanceSelect(false,false,true,true,false,true);
IonSystems 19:78d4b78fa736 104 break;
IonSystems 14:31ba3e56c788 105 default:
IonSystems 14:31ba3e56c788 106 log("Incorrect maintenance command recieved");
IonSystems 14:31ba3e56c788 107 printLCD("Incorrect maintenance command recieved");
IonSystems 19:78d4b78fa736 108 break;
IonSystems 19:78d4b78fa736 109 }
IonSystems 12:814a8fdbb6f7 110 setMaintenanceStart(true);//Bit 7
IonSystems 12:814a8fdbb6f7 111 waitForMaintenanceComplete();
IonSystems 15:0c5f20e15b6a 112 wait(0.5);
IonSystems 12:814a8fdbb6f7 113 setMaintenanceStart(false);
IonSystems 19:78d4b78fa736 114 }
IonSystems 19:78d4b78fa736 115
IonSystems 19:78d4b78fa736 116 void waitForDispenseComplete()
IonSystems 19:78d4b78fa736 117 {
IonSystems 19:78d4b78fa736 118 //wait(1); //temporary delay
IonSystems 19:78d4b78fa736 119 while(!dispenseComplete) {
IonSystems 19:78d4b78fa736 120 wait(0.2);
IonSystems 19:78d4b78fa736 121 log("Waiting for dispense complete");
IonSystems 19:78d4b78fa736 122 printLCD("Waiting for dispense complete");
IonSystems 12:814a8fdbb6f7 123 }
IonSystems 19:78d4b78fa736 124 wait(0.6);
IonSystems 19:78d4b78fa736 125 log("Complete");
IonSystems 19:78d4b78fa736 126 }
IonSystems 19:78d4b78fa736 127
IonSystems 19:78d4b78fa736 128 void dispense(Colour colour)
IonSystems 19:78d4b78fa736 129 {
IonSystems 12:814a8fdbb6f7 130
IonSystems 19:78d4b78fa736 131 bool validDispense = false;
IonSystems 19:78d4b78fa736 132 select = 0; //Setting to operation mode just in case it has not been set.
IonSystems 19:78d4b78fa736 133 setDispenseSelect(true,true);
IonSystems 19:78d4b78fa736 134 /* A dispense operation can only take place if there are more than zero chips
IonSystems 19:78d4b78fa736 135 */
IonSystems 19:78d4b78fa736 136 switch(colour) {
IonSystems 19:78d4b78fa736 137 case RED:
IonSystems 19:78d4b78fa736 138 if(redAmount > 0 || operationMode == false) {
IonSystems 19:78d4b78fa736 139 log("RED");
IonSystems 19:78d4b78fa736 140 log("Setting dispense bits to 00");
IonSystems 19:78d4b78fa736 141 setDispenseSelect(false,false);
IonSystems 19:78d4b78fa736 142 redAmount--;
IonSystems 19:78d4b78fa736 143 validDispense = true;
IonSystems 19:78d4b78fa736 144 }
IonSystems 10:8c0696b99692 145 break;
IonSystems 19:78d4b78fa736 146 case GREEN:
IonSystems 19:78d4b78fa736 147 if(greenAmount > 0 || operationMode == false) {
IonSystems 19:78d4b78fa736 148 log("GREEN");
IonSystems 19:78d4b78fa736 149 log("Setting dispense bits to 10");
IonSystems 19:78d4b78fa736 150 setDispenseSelect(true,false);
IonSystems 19:78d4b78fa736 151 greenAmount--;
IonSystems 19:78d4b78fa736 152 validDispense = true;
IonSystems 19:78d4b78fa736 153 }
IonSystems 10:8c0696b99692 154 break;
IonSystems 19:78d4b78fa736 155 case BLUE:
IonSystems 19:78d4b78fa736 156 if(blueAmount > 0 || operationMode == false) {
IonSystems 10:8c0696b99692 157 log("BLUE");
IonSystems 10:8c0696b99692 158 log("Setting dispense bits to 01");
IonSystems 19:78d4b78fa736 159 setDispenseSelect(false,true);
IonSystems 12:814a8fdbb6f7 160 blueAmount--;
IonSystems 12:814a8fdbb6f7 161 validDispense = true;
IonSystems 19:78d4b78fa736 162 }
IonSystems 10:8c0696b99692 163 break;
IonSystems 19:78d4b78fa736 164
IonSystems 19:78d4b78fa736 165 }
IonSystems 19:78d4b78fa736 166 writeFile(redAmount,greenAmount,blueAmount);
IonSystems 19:78d4b78fa736 167
IonSystems 19:78d4b78fa736 168 if(validDispense || operationMode == false) {
IonSystems 10:8c0696b99692 169 log("Setting start dispense to true");
IonSystems 10:8c0696b99692 170 startDispense = true; //set the startDispense line high.
IonSystems 19:78d4b78fa736 171
IonSystems 19:78d4b78fa736 172 waitForDispenseComplete();
IonSystems 19:78d4b78fa736 173
IonSystems 10:8c0696b99692 174 log("Setting start dispense to false");
IonSystems 10:8c0696b99692 175 startDispense = false;
IonSystems 19:78d4b78fa736 176
IonSystems 19:78d4b78fa736 177 log("Resetting dispense bits to 11");
IonSystems 19:78d4b78fa736 178 setDispenseSelect(true,true);
IonSystems 19:78d4b78fa736 179
IonSystems 19:78d4b78fa736 180
IonSystems 19:78d4b78fa736 181
IonSystems 10:8c0696b99692 182 printLCD("DISPENSE COMPLETE");
IonSystems 19:78d4b78fa736 183
IonSystems 10:8c0696b99692 184 }
IonSystems 19:78d4b78fa736 185 }
IonSystems 19:78d4b78fa736 186 void dispenseOrder(int r, int g, int b)
IonSystems 19:78d4b78fa736 187 {
IonSystems 19:78d4b78fa736 188 for(int i = r; r >= 0; i--) {
IonSystems 14:31ba3e56c788 189 dispense(RED);
IonSystems 19:78d4b78fa736 190 }
IonSystems 19:78d4b78fa736 191 for(int i = g; i >= 0; i--) {
IonSystems 15:0c5f20e15b6a 192 dispense(GREEN);
IonSystems 19:78d4b78fa736 193 }
IonSystems 19:78d4b78fa736 194 for(int i = g; i >= 0; i--) {
IonSystems 15:0c5f20e15b6a 195 dispense(BLUE);
IonSystems 17:6a0bb0ad5bb4 196 }
IonSystems 19:78d4b78fa736 197 }
IonSystems 19:78d4b78fa736 198
IonSystems 19:78d4b78fa736 199 void recycle()
IonSystems 19:78d4b78fa736 200 {
IonSystems 19:78d4b78fa736 201
IonSystems 19:78d4b78fa736 202 select = 0; //Setting to operation mode just in case it has not been set.
IonSystems 19:78d4b78fa736 203 setSortSelect(true,true,true);
IonSystems 19:78d4b78fa736 204
IonSystems 19:78d4b78fa736 205 log("Recycle");
IonSystems 19:78d4b78fa736 206 log("Setting sort bits to 100");
IonSystems 19:78d4b78fa736 207 setSortSelect(false,false,true);
IonSystems 19:78d4b78fa736 208
IonSystems 19:78d4b78fa736 209 log("Setting start sort to true");
IonSystems 19:78d4b78fa736 210 startSort = true; //set the startDispense line high.
IonSystems 19:78d4b78fa736 211
IonSystems 19:78d4b78fa736 212 //wait(1); //temporary delay
IonSystems 19:78d4b78fa736 213 while(!sortComplete) {
IonSystems 17:6a0bb0ad5bb4 214 wait(0.5);
IonSystems 19:78d4b78fa736 215 log("Waiting for sort complete");
IonSystems 19:78d4b78fa736 216 printLCD("Waiting for sort complete");
IonSystems 19:78d4b78fa736 217 }
IonSystems 19:78d4b78fa736 218
IonSystems 19:78d4b78fa736 219
IonSystems 19:78d4b78fa736 220 log("Complete");
IonSystems 19:78d4b78fa736 221 wait(0.5);
IonSystems 19:78d4b78fa736 222 log("Setting start sort to false");
IonSystems 19:78d4b78fa736 223 startSort = false;
IonSystems 19:78d4b78fa736 224
IonSystems 19:78d4b78fa736 225 log("Resetting dispense bits to 11");
IonSystems 19:78d4b78fa736 226 setSortSelect(true,true,true);
IonSystems 19:78d4b78fa736 227 printLCD("RECYCLE COMPLETE");
IonSystems 19:78d4b78fa736 228
IonSystems 19:78d4b78fa736 229 }
IonSystems 19:78d4b78fa736 230
IonSystems 19:78d4b78fa736 231 void waitForSortComplete()
IonSystems 19:78d4b78fa736 232 {
IonSystems 19:78d4b78fa736 233 while(!sortComplete) {
IonSystems 19:78d4b78fa736 234 wait(0.5);
IonSystems 19:78d4b78fa736 235 log("Waiting for sort complete");
IonSystems 19:78d4b78fa736 236 printLCD("Waiting for sort complete");
IonSystems 19:78d4b78fa736 237 }
IonSystems 19:78d4b78fa736 238 log("sortComplete signal recieved from FPGA");
IonSystems 19:78d4b78fa736 239 wait(0.6);
IonSystems 19:78d4b78fa736 240
IonSystems 19:78d4b78fa736 241 }
IonSystems 19:78d4b78fa736 242
IonSystems 19:78d4b78fa736 243 void sort(Colour colour)
IonSystems 19:78d4b78fa736 244 {
IonSystems 19:78d4b78fa736 245 if(colour == NONE) {
IonSystems 19:78d4b78fa736 246 return;
IonSystems 19:78d4b78fa736 247 } else {
IonSystems 19:78d4b78fa736 248
IonSystems 10:8c0696b99692 249 select = 0; //Setting to operation mode just in case it has not been set.
IonSystems 19:78d4b78fa736 250 setSortSelect(true,true,true);
IonSystems 19:78d4b78fa736 251 switch(colour) {
IonSystems 10:8c0696b99692 252 case RED:
IonSystems 19:78d4b78fa736 253 if(redAmount >= tubeSize) {
IonSystems 17:6a0bb0ad5bb4 254 recycle();
IonSystems 17:6a0bb0ad5bb4 255 return;
IonSystems 19:78d4b78fa736 256 }
IonSystems 10:8c0696b99692 257 log("Sort RED");
IonSystems 10:8c0696b99692 258 log("Setting sort bits to 000");
IonSystems 19:78d4b78fa736 259 setSortSelect(false,false,false);
IonSystems 12:814a8fdbb6f7 260 redAmount++;
IonSystems 19:78d4b78fa736 261 break;
IonSystems 10:8c0696b99692 262 case GREEN:
IonSystems 19:78d4b78fa736 263 if(greenAmount >= tubeSize) {
IonSystems 17:6a0bb0ad5bb4 264 recycle();
IonSystems 17:6a0bb0ad5bb4 265 return;
IonSystems 19:78d4b78fa736 266 }
IonSystems 10:8c0696b99692 267 log("Sort GREEN");
IonSystems 10:8c0696b99692 268 log("Setting sort bits to 001");
IonSystems 19:78d4b78fa736 269 setSortSelect(true,false,false);
IonSystems 12:814a8fdbb6f7 270 greenAmount++;
IonSystems 19:78d4b78fa736 271 break;
IonSystems 10:8c0696b99692 272 case BLUE:
IonSystems 19:78d4b78fa736 273 if(blueAmount >= tubeSize) {
IonSystems 17:6a0bb0ad5bb4 274 recycle();
IonSystems 17:6a0bb0ad5bb4 275 return;
IonSystems 19:78d4b78fa736 276 }
IonSystems 10:8c0696b99692 277 log("Sort BLUE");
IonSystems 10:8c0696b99692 278 log("Setting sort bits to 010");
IonSystems 19:78d4b78fa736 279 setSortSelect(false,true,false);
IonSystems 12:814a8fdbb6f7 280 blueAmount++;
IonSystems 19:78d4b78fa736 281 break;
IonSystems 11:06f6e82b40a8 282 case BIN:
IonSystems 11:06f6e82b40a8 283 log("Sort BIN");
IonSystems 11:06f6e82b40a8 284 log("Setting sort bits to 011");
IonSystems 19:78d4b78fa736 285 setSortSelect(true,true,false);
IonSystems 19:78d4b78fa736 286 break;
IonSystems 10:8c0696b99692 287 }
IonSystems 19:78d4b78fa736 288
IonSystems 19:78d4b78fa736 289
IonSystems 10:8c0696b99692 290 log("Setting start sort to true");
IonSystems 10:8c0696b99692 291 startSort = true; //set the startDispense line high.
IonSystems 19:78d4b78fa736 292
IonSystems 19:78d4b78fa736 293
IonSystems 19:78d4b78fa736 294 waitForSortComplete();
IonSystems 10:8c0696b99692 295 log("Setting start sort to false");
IonSystems 19:78d4b78fa736 296 startSort = false;
IonSystems 19:78d4b78fa736 297
IonSystems 19:78d4b78fa736 298 log("Resetting dispense bits to 11");
IonSystems 19:78d4b78fa736 299 setSortSelect(true,true,true);
IonSystems 10:8c0696b99692 300 printLCD("SORT COMPLETE");
IonSystems 12:814a8fdbb6f7 301 writeFile(redAmount,greenAmount,blueAmount);
IonSystems 19:78d4b78fa736 302 }
IonSystems 12:814a8fdbb6f7 303 }
IonSystems 12:814a8fdbb6f7 304
IonSystems 19:78d4b78fa736 305 void lift()
IonSystems 19:78d4b78fa736 306 {
IonSystems 19:78d4b78fa736 307 select = 0; //Setting to operation mode just in case it has not been set.
IonSystems 19:78d4b78fa736 308 setSortSelect(true,true,true);
IonSystems 19:78d4b78fa736 309 setSortSelect(true,false,true);
IonSystems 19:78d4b78fa736 310 log("Setting start sort to true, FPGA should begin sorting now");
IonSystems 19:78d4b78fa736 311 startSort = true; //set the startDispense line high.
IonSystems 19:78d4b78fa736 312
IonSystems 19:78d4b78fa736 313
IonSystems 19:78d4b78fa736 314 //wait(1); //temporary delay
IonSystems 19:78d4b78fa736 315 waitForSortComplete();
IonSystems 19:78d4b78fa736 316 log("Setting start sort to false");
IonSystems 19:78d4b78fa736 317 startSort = false;
IonSystems 19:78d4b78fa736 318
IonSystems 19:78d4b78fa736 319 log("Resetting sort bits to 111");
IonSystems 19:78d4b78fa736 320 setSortSelect(true,true,true);
IonSystems 19:78d4b78fa736 321 printLCD("LIFT COMPLETE");
IonSystems 19:78d4b78fa736 322 }
IonSystems 19:78d4b78fa736 323
IonSystems 19:78d4b78fa736 324 void dispenseAll()
IonSystems 19:78d4b78fa736 325 {
IonSystems 12:814a8fdbb6f7 326 int red = redAmount;
IonSystems 12:814a8fdbb6f7 327 int green = greenAmount;
IonSystems 12:814a8fdbb6f7 328 int blue = blueAmount;
IonSystems 19:78d4b78fa736 329 for(int r = 0; r < red; r++) {
IonSystems 12:814a8fdbb6f7 330 dispense(RED);
IonSystems 12:814a8fdbb6f7 331 wait(0.5);
IonSystems 19:78d4b78fa736 332 }
IonSystems 19:78d4b78fa736 333 redAmount = 0;
IonSystems 19:78d4b78fa736 334
IonSystems 19:78d4b78fa736 335 for(int g = 0; g < green; g++) {
IonSystems 12:814a8fdbb6f7 336 dispense(GREEN);
IonSystems 12:814a8fdbb6f7 337 wait(0.5);
IonSystems 19:78d4b78fa736 338 }
IonSystems 19:78d4b78fa736 339 greenAmount = 0;
IonSystems 19:78d4b78fa736 340
IonSystems 19:78d4b78fa736 341 for(int b = 0; b < blue; b++) {
IonSystems 12:814a8fdbb6f7 342 dispense(BLUE);
IonSystems 12:814a8fdbb6f7 343 wait(0.5);
IonSystems 19:78d4b78fa736 344 }
IonSystems 19:78d4b78fa736 345 blueAmount = 0;
IonSystems 12:814a8fdbb6f7 346 writeFile(redAmount,greenAmount,blueAmount);
IonSystems 19:78d4b78fa736 347 }