Chandan Siyag / Mbed 2 deprecated AlooMBED

3rd year group project. Electronic and Electrical Engineering. Heriot-Watt University. This is the code for the mbed for the Automatic Little Object Organiser (ALOO).

Dependencies:   MCP23017 TCS3472_I2C WattBob_TextLCD mbed

globals.cpp@27:2cb1bdb7ae3d, 2015-12-01 (annotated)

Committer:
dreamselec
Date:
Tue Dec 01 16:16:26 2015 +0000
Revision:
27:2cb1bdb7ae3d
Parent:
26:bbcc25418ffa
Child:
28:7e4d29977d72
Adding set new haz block mode, not completed.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dreamselec 2:7a55cb10259f 1 #include "globals.h"
dreamselec 9:dc8f155b71c8 2 #include "Block.h"
dreamselec 9:dc8f155b71c8 3 #include "fpga.h"
dreamselec 10:16ba52f8e025 4 #include "TCS3472_I2C.h"
dreamselec 10:16ba52f8e025 5 #include "MCP23017.h"
dreamselec 10:16ba52f8e025 6 #include "WattBob_TextLCD.h"
dreamselec 2:7a55cb10259f 7
dreamselec 7:b6e31bfdb2af 8 int kDefaultBaudRate = 19200;
dreamselec 20:4e0f0944f28f 9 //TODO: Not let it be constant.
dreamselec 20:4e0f0944f28f 10 int ColourSensorError = 0.5;
dreamselec 2:7a55cb10259f 11 //SerialBase gParity = SerialBase::None;
dreamselec 2:7a55cb10259f 12 int gStopBits = 1;
dreamselec 25:792540d69c49 13 float gIntegrationTime = 3;
dreamselec 25:792540d69c49 14 int gToggleServoNumber = 0;
dreamselec 26:bbcc25418ffa 15 float currentMinError[3] = {0,0,0};
dreamselec 26:bbcc25418ffa 16 float currentMaxError[3] = {0,0,0};
dreamselec 20:4e0f0944f28f 17
dreamselec 26:bbcc25418ffa 18 Block defaultHazBlock = Block();
dreamselec 20:4e0f0944f28f 19 Block _HazBlock = Block();
dreamselec 8:e1da2ae62885 20
dreamselec 8:e1da2ae62885 21 bool connectedToPC = false;
dreamselec 10:16ba52f8e025 22 bool runServoTest = false;
dreamselec 23:db91aaa43a9e 23 bool runBreakBeamTest = false;
dreamselec 25:792540d69c49 24 bool runColourSensorTest = false;
dreamselec 25:792540d69c49 25 bool getColourSensorValue = false;
dreamselec 25:792540d69c49 26 bool getBlockColourValue = false;
dreamselec 26:bbcc25418ffa 27 int errorMultiplier = 2;
dreamselec 27:2cb1bdb7ae3d 28 int hazReadingCount = 1;
dreamselec 26:bbcc25418ffa 29
dreamselec 17:af373246bf80 30 PCModes currentMode = None;
dreamselec 18:44a1c1a30166 31 Controls currentState = Pause;
dreamselec 8:e1da2ae62885 32
dreamselec 20:4e0f0944f28f 33 void DefaultHazBlock(){
dreamselec 26:bbcc25418ffa 34
dreamselec 26:bbcc25418ffa 35 for (int i = 0; i < 3; i++){
dreamselec 26:bbcc25418ffa 36 currentMaxError[i] = kMaxError[i];
dreamselec 26:bbcc25418ffa 37 currentMinError[i] = kMinError[i];
dreamselec 26:bbcc25418ffa 38 }
dreamselec 20:4e0f0944f28f 39 Colour _minRedBlock = Colour();
dreamselec 20:4e0f0944f28f 40 Colour _maxRedBlock = Colour();
dreamselec 20:4e0f0944f28f 41 Colour _averageRedBlock = Colour();
dreamselec 20:4e0f0944f28f 42
dreamselec 20:4e0f0944f28f 43 for (int i = 0; i < 4; i++){
dreamselec 20:4e0f0944f28f 44 _minRedBlock.components[i] = kMinRedBlock[i];
dreamselec 20:4e0f0944f28f 45 _maxRedBlock.components[i] = kMaxRedBlock[i];
dreamselec 20:4e0f0944f28f 46 _averageRedBlock.components[i] = kAverageRedBlock[i];
dreamselec 20:4e0f0944f28f 47 }
dreamselec 20:4e0f0944f28f 48
dreamselec 26:bbcc25418ffa 49 // _HazBlock.minColour = Colour(_minRedBlock);
dreamselec 26:bbcc25418ffa 50 // _HazBlock.maxColour = Colour(_maxRedBlock);
dreamselec 26:bbcc25418ffa 51 // _HazBlock.averageColour = Colour(_averageRedBlock);
dreamselec 26:bbcc25418ffa 52 // _HazBlock.size = Block::Small;
dreamselec 26:bbcc25418ffa 53
dreamselec 26:bbcc25418ffa 54 defaultHazBlock.minColour = Colour(_minRedBlock);
dreamselec 26:bbcc25418ffa 55 defaultHazBlock.maxColour = Colour(_maxRedBlock);
dreamselec 26:bbcc25418ffa 56 defaultHazBlock.averageColour = Colour(_averageRedBlock);
dreamselec 26:bbcc25418ffa 57 defaultHazBlock.size = Block::Small;
dreamselec 20:4e0f0944f28f 58
dreamselec 26:bbcc25418ffa 59 _HazBlock = Block(defaultHazBlock);
dreamselec 27:2cb1bdb7ae3d 60 // pc.printf( "VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%f,%f,%f,%f\n \t Max Colour:%f,%f,%f,%f:VALUE", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
dreamselec 26:bbcc25418ffa 61 }
dreamselec 26:bbcc25418ffa 62
dreamselec 26:bbcc25418ffa 63 void printColourDescription(Colour colour){
dreamselec 26:bbcc25418ffa 64 pc.printf("Red: %.3f, Green: %.3f, Blue: %.3f, Clear: %.3f\n", colour.components[0], colour.components[1], colour.components[2], colour.components[3]);
dreamselec 26:bbcc25418ffa 65 }
dreamselec 26:bbcc25418ffa 66
dreamselec 26:bbcc25418ffa 67 void printBlockDescription(Block block){
dreamselec 26:bbcc25418ffa 68 pc.printf("VALUE:Size: %i\n", block.size);
dreamselec 26:bbcc25418ffa 69 printColourDescription(block.minColour);
dreamselec 26:bbcc25418ffa 70 printColourDescription(block.averageColour);
dreamselec 26:bbcc25418ffa 71 printColourDescription(block.maxColour);
dreamselec 26:bbcc25418ffa 72 //TODO: print errors
dreamselec 26:bbcc25418ffa 73 pc.printf(":VALUE");
dreamselec 20:4e0f0944f28f 74 }
dreamselec 20:4e0f0944f28f 75
dreamselec 10:16ba52f8e025 76 int readSwitches()
dreamselec 10:16ba52f8e025 77 {
dreamselec 20:4e0f0944f28f 78 if(i2cport->read_bit(8)) {
dreamselec 20:4e0f0944f28f 79 while (i2cport->read_bit(8)) { }
dreamselec 20:4e0f0944f28f 80 return 1;
dreamselec 20:4e0f0944f28f 81 } else if (i2cport->read_bit(9)) {
dreamselec 20:4e0f0944f28f 82 while (i2cport->read_bit(9)) { }
dreamselec 20:4e0f0944f28f 83 return 2;
dreamselec 20:4e0f0944f28f 84 } else if (i2cport->read_bit(10)) {
dreamselec 20:4e0f0944f28f 85 while (i2cport->read_bit(10)) { }
dreamselec 20:4e0f0944f28f 86 return 3;
dreamselec 20:4e0f0944f28f 87 } else if (i2cport->read_bit(11)) {
dreamselec 20:4e0f0944f28f 88 while (i2cport->read_bit(11)) { }
dreamselec 20:4e0f0944f28f 89 return 4;
dreamselec 20:4e0f0944f28f 90 } else {
dreamselec 20:4e0f0944f28f 91 return 0;
dreamselec 20:4e0f0944f28f 92 }
dreamselec 10:16ba52f8e025 93
dreamselec 8:e1da2ae62885 94 }
dreamselec 8:e1da2ae62885 95
dreamselec 10:16ba52f8e025 96 void connectToPC(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 97 {
dreamselec 20:4e0f0944f28f 98 connectedToPC = true;
dreamselec 24:02c61793f90b 99 pc.printf("INFO:PC connected to MBED.\n");
dreamselec 10:16ba52f8e025 100 }
dreamselec 10:16ba52f8e025 101
dreamselec 10:16ba52f8e025 102 void disconnectToPC(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 103 {
dreamselec 24:02c61793f90b 104 pc.printf("INFO:PC disconnected to MBED.\n");
dreamselec 20:4e0f0944f28f 105 connectedToPC = false;
dreamselec 20:4e0f0944f28f 106 currentMode = None;
dreamselec 8:e1da2ae62885 107 }
dreamselec 8:e1da2ae62885 108
dreamselec 10:16ba52f8e025 109 void hazBlock(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 110 {
dreamselec 20:4e0f0944f28f 111 if (typeRaw == Set) {
dreamselec 27:2cb1bdb7ae3d 112 fpga->moveSortingServo(Haz);
dreamselec 20:4e0f0944f28f 113 pc.printf("INFO:Setting new haz block.\n");
dreamselec 20:4e0f0944f28f 114 int lowerBeam = 0;
dreamselec 20:4e0f0944f28f 115 int higherBeam = 0;
dreamselec 20:4e0f0944f28f 116 char readyChar = '\0';
dreamselec 20:4e0f0944f28f 117
dreamselec 20:4e0f0944f28f 118 lcd->cls();
dreamselec 20:4e0f0944f28f 119 lcd->locate(0,0);
dreamselec 20:4e0f0944f28f 120 lcd->printf("New haz block");
dreamselec 27:2cb1bdb7ae3d 121
dreamselec 20:4e0f0944f28f 122 do {
dreamselec 20:4e0f0944f28f 123 higherBeam = fpga->getBeamValue(Top);
dreamselec 20:4e0f0944f28f 124 if (readSwitches() == 4) {
dreamselec 20:4e0f0944f28f 125 if (displayAbortDialog()) {
dreamselec 27:2cb1bdb7ae3d 126 //TODO: tell pc
dreamselec 27:2cb1bdb7ae3d 127 pc.printf(":<mbed>haz-block=pause");
dreamselec 20:4e0f0944f28f 128 pc.printf("INFO: Operation aborted form MBED.\n");
dreamselec 27:2cb1bdb7ae3d 129 fpga->moveSortingServo(NonHaz);
dreamselec 20:4e0f0944f28f 130 displayPCStatus();
dreamselec 20:4e0f0944f28f 131 return;
dreamselec 20:4e0f0944f28f 132 } else {
dreamselec 20:4e0f0944f28f 133 lcd->cls();
dreamselec 20:4e0f0944f28f 134 lcd->locate(0,0);
dreamselec 20:4e0f0944f28f 135 lcd->printf("New haz block");
dreamselec 20:4e0f0944f28f 136 }
dreamselec 18:44a1c1a30166 137
dreamselec 20:4e0f0944f28f 138 }
dreamselec 20:4e0f0944f28f 139 } while (higherBeam != 1);
dreamselec 27:2cb1bdb7ae3d 140
dreamselec 27:2cb1bdb7ae3d 141 int colourValues[4];
dreamselec 27:2cb1bdb7ae3d 142
dreamselec 27:2cb1bdb7ae3d 143 int colourValues[4];
dreamselec 27:2cb1bdb7ae3d 144 int averageColourValues[4] = {0, 0, 0, 0};
dreamselec 27:2cb1bdb7ae3d 145 int numberOfReadings = 3;
dreamselec 27:2cb1bdb7ae3d 146 for (int i = 0; i < 4; i++) {
dreamselec 27:2cb1bdb7ae3d 147 rgbSensor.getAllColors(colourValues);
dreamselec 27:2cb1bdb7ae3d 148 for (int j = 0; j < 4; j++) {
dreamselec 27:2cb1bdb7ae3d 149 averageColourValues[j] += colourValues[j];
dreamselec 27:2cb1bdb7ae3d 150 }
dreamselec 27:2cb1bdb7ae3d 151 }
dreamselec 27:2cb1bdb7ae3d 152 for (int i = 0; i < 4; i++) {
dreamselec 27:2cb1bdb7ae3d 153 averageColourValues[i] = averageColourValues[i] / numberOfReadings;
dreamselec 27:2cb1bdb7ae3d 154 }
dreamselec 10:16ba52f8e025 155
dreamselec 20:4e0f0944f28f 156 int colourValue[4];
dreamselec 20:4e0f0944f28f 157 rgbSensor.getAllColors(colourValue);
dreamselec 20:4e0f0944f28f 158
dreamselec 20:4e0f0944f28f 159 do {
dreamselec 20:4e0f0944f28f 160 lowerBeam = fpga->checkForBlock();
dreamselec 20:4e0f0944f28f 161 } while (lowerBeam != 1);
dreamselec 20:4e0f0944f28f 162 higherBeam = fpga->checkForSize();
dreamselec 18:44a1c1a30166 163
dreamselec 20:4e0f0944f28f 164 _HazBlock = Block();
dreamselec 20:4e0f0944f28f 165 if (higherBeam == 1)
dreamselec 20:4e0f0944f28f 166 _HazBlock.size = Block::Big;
dreamselec 20:4e0f0944f28f 167 else if (higherBeam == 0)
dreamselec 20:4e0f0944f28f 168 _HazBlock.size = Block::Small;
dreamselec 20:4e0f0944f28f 169 for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 170 _HazBlock.minColour.components[i] = colourValue[i]/colourValue[3] - ColourSensorError;
dreamselec 20:4e0f0944f28f 171 _HazBlock.maxColour.components[i] = colourValue[i]/colourValue[3] + ColourSensorError;
dreamselec 20:4e0f0944f28f 172 }
dreamselec 20:4e0f0944f28f 173 fpga->moveSortingServo(Haz);
dreamselec 20:4e0f0944f28f 174 fpga->moveStoppingServo(Go);
dreamselec 20:4e0f0944f28f 175 while (fpga->checkForBlock()) {}
dreamselec 20:4e0f0944f28f 176 fpga->moveStoppingServo(Stop);
dreamselec 20:4e0f0944f28f 177 fpga->moveSortingServo(NonHaz);
dreamselec 18:44a1c1a30166 178
dreamselec 26:bbcc25418ffa 179 //TODO: Fix format.
dreamselec 22:993821a4c396 180 pc.printf( "VALUE:HazBlock:\n \t Size:%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, colourValue[0], colourValue[1], colourValue[2], colourValue[3], _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
dreamselec 20:4e0f0944f28f 181 } else if (typeRaw == Query) {
dreamselec 20:4e0f0944f28f 182 pc.printf( "VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i:VALUE", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
dreamselec 20:4e0f0944f28f 183 }
dreamselec 8:e1da2ae62885 184 }
dreamselec 8:e1da2ae62885 185
dreamselec 10:16ba52f8e025 186 void getCurrentBlock(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 187 {
dreamselec 20:4e0f0944f28f 188 pc.printf("DEBUG: Getting current block readings\n");
dreamselec 8:e1da2ae62885 189 }
dreamselec 8:e1da2ae62885 190
dreamselec 15:777390eb5afd 191 void setIntegrationTimeTo(float integrationTime)
dreamselec 10:16ba52f8e025 192 {
dreamselec 25:792540d69c49 193 gIntegrationTime = integrationTime;
dreamselec 25:792540d69c49 194 rgbSensor.setIntegrationTime(gIntegrationTime);
dreamselec 25:792540d69c49 195 pc.printf("DEBUG: Setting integration-time to %i.\n", gIntegrationTime);
dreamselec 8:e1da2ae62885 196 }
dreamselec 8:e1da2ae62885 197
dreamselec 10:16ba52f8e025 198 void previewOnPC(bool on)
dreamselec 10:16ba52f8e025 199 {
dreamselec 20:4e0f0944f28f 200 pc.printf("setting preview on pc to %i.\n", on);
dreamselec 8:e1da2ae62885 201 }
dreamselec 8:e1da2ae62885 202
dreamselec 25:792540d69c49 203 void testColourSensor(Controls state){
dreamselec 25:792540d69c49 204 if (state == Start){
dreamselec 25:792540d69c49 205 pc.printf("INFO: Running colour test.\n");
dreamselec 25:792540d69c49 206 runColourSensorTest = true;
dreamselec 25:792540d69c49 207 } else if (state == Pause){
dreamselec 25:792540d69c49 208 pc.printf("INFO: Finished colour test.\n");
dreamselec 25:792540d69c49 209 runColourSensorTest = false;
dreamselec 25:792540d69c49 210 }
dreamselec 25:792540d69c49 211 }
dreamselec 25:792540d69c49 212
dreamselec 20:4e0f0944f28f 213 void readColourSensor()
dreamselec 20:4e0f0944f28f 214 {
dreamselec 20:4e0f0944f28f 215 int colourValue[4];
dreamselec 20:4e0f0944f28f 216 rgbSensor.getAllColors(colourValue);
dreamselec 20:4e0f0944f28f 217 pc.printf( "VALEU:Colour Reading:%i,%i,%i,%i\n:VALUE", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
dreamselec 18:44a1c1a30166 218
dreamselec 8:e1da2ae62885 219 }
dreamselec 8:e1da2ae62885 220
dreamselec 17:af373246bf80 221 void testServos(Controls state)
dreamselec 10:16ba52f8e025 222 {
dreamselec 20:4e0f0944f28f 223 if (state == Start) {
dreamselec 20:4e0f0944f28f 224 pc.printf("INFO: Running servo test.\n");
dreamselec 20:4e0f0944f28f 225 runServoTest = true;
dreamselec 20:4e0f0944f28f 226 } else if (state == Pause) {
dreamselec 20:4e0f0944f28f 227 pc.printf("INFO: Finished running servo test.\n");
dreamselec 20:4e0f0944f28f 228 runServoTest = false;
dreamselec 20:4e0f0944f28f 229 }
dreamselec 8:e1da2ae62885 230 }
dreamselec 8:e1da2ae62885 231
dreamselec 10:16ba52f8e025 232 void resetServos()
dreamselec 10:16ba52f8e025 233 {
dreamselec 20:4e0f0944f28f 234 pc.printf("resetting servos.\n");
dreamselec 8:e1da2ae62885 235 }
dreamselec 8:e1da2ae62885 236
dreamselec 10:16ba52f8e025 237 void getPortInfo()
dreamselec 10:16ba52f8e025 238 {
dreamselec 20:4e0f0944f28f 239 pc.printf("getting port info.\n");
dreamselec 8:e1da2ae62885 240 }
dreamselec 8:e1da2ae62885 241
dreamselec 17:af373246bf80 242 void setPortBaudRate(int baudRate)
dreamselec 10:16ba52f8e025 243 {
dreamselec 20:4e0f0944f28f 244 pc.baud(baudRate);
dreamselec 20:4e0f0944f28f 245 wait(0.1);
dreamselec 20:4e0f0944f28f 246 pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate);
dreamselec 10:16ba52f8e025 247 }
dreamselec 10:16ba52f8e025 248
dreamselec 20:4e0f0944f28f 249 void setPortParity(int parity)
dreamselec 20:4e0f0944f28f 250 {
dreamselec 20:4e0f0944f28f 251 SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity);
dreamselec 20:4e0f0944f28f 252 pc.format(8, _parity, 1);
dreamselec 20:4e0f0944f28f 253 wait(0.1);
dreamselec 20:4e0f0944f28f 254 pc.printf("DEBUG: Setting port parity to: %i.\n", parity);
dreamselec 14:cf2f255b5560 255 }
dreamselec 23:db91aaa43a9e 256
dreamselec 23:db91aaa43a9e 257 void testBreakBeams(Controls state){
dreamselec 23:db91aaa43a9e 258 if (state == Start){
dreamselec 23:db91aaa43a9e 259 pc.printf("INFO: Running break beam test.\n");
dreamselec 23:db91aaa43a9e 260 runBreakBeamTest = true;
dreamselec 23:db91aaa43a9e 261 }else if (state == Pause){
dreamselec 23:db91aaa43a9e 262 pc.printf("INFO: Exiting break beam test.\n");
dreamselec 23:db91aaa43a9e 263 runBreakBeamTest = false;
dreamselec 23:db91aaa43a9e 264 }
dreamselec 23:db91aaa43a9e 265 }