![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |