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@25:792540d69c49, 2015-11-30 (annotated)

Committer:
dreamselec
Date:
Mon Nov 30 15:06:38 2015 +0000
Revision:
25:792540d69c49
Parent:
24:02c61793f90b
Child:
26:bbcc25418ffa
Colour sensor test mode, new commands for servo to toggle. Needs to be tested with hardware.

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 20:4e0f0944f28f 15
dreamselec 20:4e0f0944f28f 16 Block _HazBlock = Block();
dreamselec 8:e1da2ae62885 17
dreamselec 8:e1da2ae62885 18 bool connectedToPC = false;
dreamselec 10:16ba52f8e025 19 bool runServoTest = false;
dreamselec 23:db91aaa43a9e 20 bool runBreakBeamTest = false;
dreamselec 25:792540d69c49 21 bool runColourSensorTest = false;
dreamselec 25:792540d69c49 22 bool getColourSensorValue = false;
dreamselec 25:792540d69c49 23 bool getBlockColourValue = false;
dreamselec 17:af373246bf80 24 PCModes currentMode = None;
dreamselec 18:44a1c1a30166 25 Controls currentState = Pause;
dreamselec 8:e1da2ae62885 26
dreamselec 20:4e0f0944f28f 27 void DefaultHazBlock(){
dreamselec 20:4e0f0944f28f 28 Colour _minRedBlock = Colour();
dreamselec 20:4e0f0944f28f 29 Colour _maxRedBlock = Colour();
dreamselec 20:4e0f0944f28f 30 Colour _averageRedBlock = Colour();
dreamselec 20:4e0f0944f28f 31
dreamselec 20:4e0f0944f28f 32 for (int i = 0; i < 4; i++){
dreamselec 20:4e0f0944f28f 33 _minRedBlock.components[i] = kMinRedBlock[i];
dreamselec 20:4e0f0944f28f 34 _maxRedBlock.components[i] = kMaxRedBlock[i];
dreamselec 20:4e0f0944f28f 35 _averageRedBlock.components[i] = kAverageRedBlock[i];
dreamselec 20:4e0f0944f28f 36 }
dreamselec 20:4e0f0944f28f 37
dreamselec 20:4e0f0944f28f 38 _HazBlock.minColour = Colour(_minRedBlock);
dreamselec 20:4e0f0944f28f 39 _HazBlock.maxColour = Colour(_maxRedBlock);
dreamselec 20:4e0f0944f28f 40 _HazBlock.averageColour = Colour(_averageRedBlock);
dreamselec 20:4e0f0944f28f 41 _HazBlock.size = Block::Small;
dreamselec 20:4e0f0944f28f 42
dreamselec 20:4e0f0944f28f 43 // defaultHazBlock.minColour = Colour(_minRedBlock);
dreamselec 20:4e0f0944f28f 44 // defaultHazBlock.maxColour = Colour(_maxRedBlock);
dreamselec 20:4e0f0944f28f 45 // defaultHazBlock.averageColour = Colour(_averageRedBlock);
dreamselec 20:4e0f0944f28f 46
dreamselec 20:4e0f0944f28f 47 // Block _HazBlock = Block(defaultHazBlock);
dreamselec 20:4e0f0944f28f 48 }
dreamselec 20:4e0f0944f28f 49
dreamselec 10:16ba52f8e025 50 int readSwitches()
dreamselec 10:16ba52f8e025 51 {
dreamselec 20:4e0f0944f28f 52 if(i2cport->read_bit(8)) {
dreamselec 20:4e0f0944f28f 53 while (i2cport->read_bit(8)) { }
dreamselec 20:4e0f0944f28f 54 return 1;
dreamselec 20:4e0f0944f28f 55 } else if (i2cport->read_bit(9)) {
dreamselec 20:4e0f0944f28f 56 while (i2cport->read_bit(9)) { }
dreamselec 20:4e0f0944f28f 57 return 2;
dreamselec 20:4e0f0944f28f 58 } else if (i2cport->read_bit(10)) {
dreamselec 20:4e0f0944f28f 59 while (i2cport->read_bit(10)) { }
dreamselec 20:4e0f0944f28f 60 return 3;
dreamselec 20:4e0f0944f28f 61 } else if (i2cport->read_bit(11)) {
dreamselec 20:4e0f0944f28f 62 while (i2cport->read_bit(11)) { }
dreamselec 20:4e0f0944f28f 63 return 4;
dreamselec 20:4e0f0944f28f 64 } else {
dreamselec 20:4e0f0944f28f 65 return 0;
dreamselec 20:4e0f0944f28f 66 }
dreamselec 10:16ba52f8e025 67
dreamselec 8:e1da2ae62885 68 }
dreamselec 8:e1da2ae62885 69
dreamselec 10:16ba52f8e025 70 void connectToPC(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 71 {
dreamselec 20:4e0f0944f28f 72 connectedToPC = true;
dreamselec 24:02c61793f90b 73 pc.printf("INFO:PC connected to MBED.\n");
dreamselec 10:16ba52f8e025 74 }
dreamselec 10:16ba52f8e025 75
dreamselec 10:16ba52f8e025 76 void disconnectToPC(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 77 {
dreamselec 24:02c61793f90b 78 pc.printf("INFO:PC disconnected to MBED.\n");
dreamselec 20:4e0f0944f28f 79 connectedToPC = false;
dreamselec 20:4e0f0944f28f 80 currentMode = None;
dreamselec 8:e1da2ae62885 81 }
dreamselec 8:e1da2ae62885 82
dreamselec 10:16ba52f8e025 83 void hazBlock(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 84 {
dreamselec 20:4e0f0944f28f 85 if (typeRaw == Set) {
dreamselec 20:4e0f0944f28f 86 pc.printf("INFO:Setting new haz block.\n");
dreamselec 20:4e0f0944f28f 87 int lowerBeam = 0;
dreamselec 20:4e0f0944f28f 88 int higherBeam = 0;
dreamselec 20:4e0f0944f28f 89 char readyChar = '\0';
dreamselec 20:4e0f0944f28f 90
dreamselec 20:4e0f0944f28f 91 lcd->cls();
dreamselec 20:4e0f0944f28f 92 lcd->locate(0,0);
dreamselec 20:4e0f0944f28f 93 lcd->printf("New haz block");
dreamselec 18:44a1c1a30166 94
dreamselec 20:4e0f0944f28f 95 do {
dreamselec 20:4e0f0944f28f 96 higherBeam = fpga->getBeamValue(Top);
dreamselec 20:4e0f0944f28f 97 if (readSwitches() == 4) {
dreamselec 20:4e0f0944f28f 98 if (displayAbortDialog()) {
dreamselec 20:4e0f0944f28f 99 pc.printf("INFO: Operation aborted form MBED.\n");
dreamselec 20:4e0f0944f28f 100 displayPCStatus();
dreamselec 20:4e0f0944f28f 101 return;
dreamselec 20:4e0f0944f28f 102 } else {
dreamselec 20:4e0f0944f28f 103 lcd->cls();
dreamselec 20:4e0f0944f28f 104 lcd->locate(0,0);
dreamselec 20:4e0f0944f28f 105 lcd->printf("New haz block");
dreamselec 20:4e0f0944f28f 106 }
dreamselec 18:44a1c1a30166 107
dreamselec 20:4e0f0944f28f 108 }
dreamselec 20:4e0f0944f28f 109 } while (higherBeam != 1);
dreamselec 10:16ba52f8e025 110
dreamselec 20:4e0f0944f28f 111 int colourValue[4];
dreamselec 20:4e0f0944f28f 112 rgbSensor.getAllColors(colourValue);
dreamselec 20:4e0f0944f28f 113
dreamselec 20:4e0f0944f28f 114 do {
dreamselec 20:4e0f0944f28f 115 lowerBeam = fpga->checkForBlock();
dreamselec 20:4e0f0944f28f 116 } while (lowerBeam != 1);
dreamselec 20:4e0f0944f28f 117 higherBeam = fpga->checkForSize();
dreamselec 18:44a1c1a30166 118
dreamselec 20:4e0f0944f28f 119 _HazBlock = Block();
dreamselec 20:4e0f0944f28f 120 if (higherBeam == 1)
dreamselec 20:4e0f0944f28f 121 _HazBlock.size = Block::Big;
dreamselec 20:4e0f0944f28f 122 else if (higherBeam == 0)
dreamselec 20:4e0f0944f28f 123 _HazBlock.size = Block::Small;
dreamselec 20:4e0f0944f28f 124 for (int i = 0; i < 3; i++) {
dreamselec 20:4e0f0944f28f 125 _HazBlock.minColour.components[i] = colourValue[i]/colourValue[3] - ColourSensorError;
dreamselec 20:4e0f0944f28f 126 _HazBlock.maxColour.components[i] = colourValue[i]/colourValue[3] + ColourSensorError;
dreamselec 20:4e0f0944f28f 127 }
dreamselec 20:4e0f0944f28f 128 fpga->moveSortingServo(Haz);
dreamselec 20:4e0f0944f28f 129 fpga->moveStoppingServo(Go);
dreamselec 20:4e0f0944f28f 130 while (fpga->checkForBlock()) {}
dreamselec 20:4e0f0944f28f 131 fpga->moveStoppingServo(Stop);
dreamselec 20:4e0f0944f28f 132 fpga->moveSortingServo(NonHaz);
dreamselec 18:44a1c1a30166 133
dreamselec 22:993821a4c396 134 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 135 } else if (typeRaw == Query) {
dreamselec 20:4e0f0944f28f 136 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 137 }
dreamselec 8:e1da2ae62885 138 }
dreamselec 8:e1da2ae62885 139
dreamselec 10:16ba52f8e025 140 void getCurrentBlock(CommandTypeRaw typeRaw)
dreamselec 10:16ba52f8e025 141 {
dreamselec 20:4e0f0944f28f 142 pc.printf("DEBUG: Getting current block readings\n");
dreamselec 8:e1da2ae62885 143 }
dreamselec 8:e1da2ae62885 144
dreamselec 15:777390eb5afd 145 void setIntegrationTimeTo(float integrationTime)
dreamselec 10:16ba52f8e025 146 {
dreamselec 25:792540d69c49 147 gIntegrationTime = integrationTime;
dreamselec 25:792540d69c49 148 rgbSensor.setIntegrationTime(gIntegrationTime);
dreamselec 25:792540d69c49 149 pc.printf("DEBUG: Setting integration-time to %i.\n", gIntegrationTime);
dreamselec 8:e1da2ae62885 150 }
dreamselec 8:e1da2ae62885 151
dreamselec 10:16ba52f8e025 152 void previewOnPC(bool on)
dreamselec 10:16ba52f8e025 153 {
dreamselec 20:4e0f0944f28f 154 pc.printf("setting preview on pc to %i.\n", on);
dreamselec 8:e1da2ae62885 155 }
dreamselec 8:e1da2ae62885 156
dreamselec 25:792540d69c49 157 void testColourSensor(Controls state){
dreamselec 25:792540d69c49 158 if (state == Start){
dreamselec 25:792540d69c49 159 pc.printf("INFO: Running colour test.\n");
dreamselec 25:792540d69c49 160 runColourSensorTest = true;
dreamselec 25:792540d69c49 161 } else if (state == Pause){
dreamselec 25:792540d69c49 162 pc.printf("INFO: Finished colour test.\n");
dreamselec 25:792540d69c49 163 runColourSensorTest = false;
dreamselec 25:792540d69c49 164 }
dreamselec 25:792540d69c49 165 }
dreamselec 25:792540d69c49 166
dreamselec 20:4e0f0944f28f 167 void readColourSensor()
dreamselec 20:4e0f0944f28f 168 {
dreamselec 20:4e0f0944f28f 169 int colourValue[4];
dreamselec 20:4e0f0944f28f 170 rgbSensor.getAllColors(colourValue);
dreamselec 20:4e0f0944f28f 171 pc.printf( "VALEU:Colour Reading:%i,%i,%i,%i\n:VALUE", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
dreamselec 18:44a1c1a30166 172
dreamselec 8:e1da2ae62885 173 }
dreamselec 8:e1da2ae62885 174
dreamselec 17:af373246bf80 175 void testServos(Controls state)
dreamselec 10:16ba52f8e025 176 {
dreamselec 20:4e0f0944f28f 177 if (state == Start) {
dreamselec 20:4e0f0944f28f 178 pc.printf("INFO: Running servo test.\n");
dreamselec 20:4e0f0944f28f 179 runServoTest = true;
dreamselec 20:4e0f0944f28f 180 } else if (state == Pause) {
dreamselec 20:4e0f0944f28f 181 pc.printf("INFO: Finished running servo test.\n");
dreamselec 20:4e0f0944f28f 182 runServoTest = false;
dreamselec 20:4e0f0944f28f 183 }
dreamselec 8:e1da2ae62885 184 }
dreamselec 8:e1da2ae62885 185
dreamselec 10:16ba52f8e025 186 void resetServos()
dreamselec 10:16ba52f8e025 187 {
dreamselec 20:4e0f0944f28f 188 pc.printf("resetting servos.\n");
dreamselec 8:e1da2ae62885 189 }
dreamselec 8:e1da2ae62885 190
dreamselec 10:16ba52f8e025 191 void getPortInfo()
dreamselec 10:16ba52f8e025 192 {
dreamselec 20:4e0f0944f28f 193 pc.printf("getting port info.\n");
dreamselec 8:e1da2ae62885 194 }
dreamselec 8:e1da2ae62885 195
dreamselec 17:af373246bf80 196 void setPortBaudRate(int baudRate)
dreamselec 10:16ba52f8e025 197 {
dreamselec 20:4e0f0944f28f 198 pc.baud(baudRate);
dreamselec 20:4e0f0944f28f 199 wait(0.1);
dreamselec 20:4e0f0944f28f 200 pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate);
dreamselec 10:16ba52f8e025 201 }
dreamselec 10:16ba52f8e025 202
dreamselec 20:4e0f0944f28f 203 void setPortParity(int parity)
dreamselec 20:4e0f0944f28f 204 {
dreamselec 20:4e0f0944f28f 205 SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity);
dreamselec 20:4e0f0944f28f 206 pc.format(8, _parity, 1);
dreamselec 20:4e0f0944f28f 207 wait(0.1);
dreamselec 20:4e0f0944f28f 208 pc.printf("DEBUG: Setting port parity to: %i.\n", parity);
dreamselec 14:cf2f255b5560 209 }
dreamselec 23:db91aaa43a9e 210
dreamselec 23:db91aaa43a9e 211 void testBreakBeams(Controls state){
dreamselec 23:db91aaa43a9e 212 if (state == Start){
dreamselec 23:db91aaa43a9e 213 pc.printf("INFO: Running break beam test.\n");
dreamselec 23:db91aaa43a9e 214 runBreakBeamTest = true;
dreamselec 23:db91aaa43a9e 215 }else if (state == Pause){
dreamselec 23:db91aaa43a9e 216 pc.printf("INFO: Exiting break beam test.\n");
dreamselec 23:db91aaa43a9e 217 runBreakBeamTest = false;
dreamselec 23:db91aaa43a9e 218 }
dreamselec 23:db91aaa43a9e 219 }