![](/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@29:9c0339e3c593, 2015-12-01 (annotated)
- Committer:
- dreamselec
- Date:
- Tue Dec 01 22:56:08 2015 +0000
- Revision:
- 29:9c0339e3c593
- Parent:
- 28:7e4d29977d72
- Child:
- 30:c0bc92d009fe
Set new haz block mode should work.
Added hardcoded other colour values.
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 | 28:7e4d29977d72 | 13 | float gIntegrationTime = 2.5; |
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 |
29:9c0339e3c593 | 18 | Block defaultHazBlock = Block(Block::Small, Block::Red); |
dreamselec |
29:9c0339e3c593 | 19 | Block _HazBlock = Block(defaultHazBlock); |
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 |
29:9c0339e3c593 | 27 | bool setNewHazBlock = false; |
dreamselec | 28:7e4d29977d72 | 28 | bool pcModeChanged = false; |
dreamselec | 28:7e4d29977d72 | 29 | int errorMultiplier = 1; |
dreamselec | 27:2cb1bdb7ae3d | 30 | int hazReadingCount = 1; |
dreamselec | 26:bbcc25418ffa | 31 | |
dreamselec | 17:af373246bf80 | 32 | PCModes currentMode = None; |
dreamselec |
18:44a1c1a30166 | 33 | Controls currentState = Pause; |
dreamselec |
8:e1da2ae62885 | 34 | |
dreamselec | 20:4e0f0944f28f | 35 | void DefaultHazBlock(){ |
dreamselec | 26:bbcc25418ffa | 36 | |
dreamselec | 26:bbcc25418ffa | 37 | for (int i = 0; i < 3; i++){ |
dreamselec |
29:9c0339e3c593 | 38 | currentMaxError[i] = kMaxRedError[i]; |
dreamselec |
29:9c0339e3c593 | 39 | currentMinError[i] = kMinRedError[i]; |
dreamselec | 26:bbcc25418ffa | 40 | } |
dreamselec | 20:4e0f0944f28f | 41 | Colour _minRedBlock = Colour(); |
dreamselec | 20:4e0f0944f28f | 42 | Colour _maxRedBlock = Colour(); |
dreamselec | 20:4e0f0944f28f | 43 | Colour _averageRedBlock = Colour(); |
dreamselec | 20:4e0f0944f28f | 44 | |
dreamselec | 20:4e0f0944f28f | 45 | for (int i = 0; i < 4; i++){ |
dreamselec | 20:4e0f0944f28f | 46 | _minRedBlock.components[i] = kMinRedBlock[i]; |
dreamselec | 20:4e0f0944f28f | 47 | _maxRedBlock.components[i] = kMaxRedBlock[i]; |
dreamselec | 20:4e0f0944f28f | 48 | _averageRedBlock.components[i] = kAverageRedBlock[i]; |
dreamselec | 20:4e0f0944f28f | 49 | } |
dreamselec | 20:4e0f0944f28f | 50 | |
dreamselec | 26:bbcc25418ffa | 51 | // _HazBlock.minColour = Colour(_minRedBlock); |
dreamselec | 26:bbcc25418ffa | 52 | // _HazBlock.maxColour = Colour(_maxRedBlock); |
dreamselec | 26:bbcc25418ffa | 53 | // _HazBlock.averageColour = Colour(_averageRedBlock); |
dreamselec | 26:bbcc25418ffa | 54 | // _HazBlock.size = Block::Small; |
dreamselec | 26:bbcc25418ffa | 55 | |
dreamselec | 26:bbcc25418ffa | 56 | defaultHazBlock.minColour = Colour(_minRedBlock); |
dreamselec | 26:bbcc25418ffa | 57 | defaultHazBlock.maxColour = Colour(_maxRedBlock); |
dreamselec | 26:bbcc25418ffa | 58 | defaultHazBlock.averageColour = Colour(_averageRedBlock); |
dreamselec | 26:bbcc25418ffa | 59 | defaultHazBlock.size = Block::Small; |
dreamselec | 20:4e0f0944f28f | 60 | |
dreamselec | 26:bbcc25418ffa | 61 | _HazBlock = Block(defaultHazBlock); |
dreamselec | 27:2cb1bdb7ae3d | 62 | // 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 | 63 | } |
dreamselec | 26:bbcc25418ffa | 64 | |
dreamselec | 26:bbcc25418ffa | 65 | void printColourDescription(Colour colour){ |
dreamselec | 26:bbcc25418ffa | 66 | 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 | 67 | } |
dreamselec | 26:bbcc25418ffa | 68 | |
dreamselec | 26:bbcc25418ffa | 69 | void printBlockDescription(Block block){ |
dreamselec | 26:bbcc25418ffa | 70 | pc.printf("VALUE:Size: %i\n", block.size); |
dreamselec | 26:bbcc25418ffa | 71 | printColourDescription(block.minColour); |
dreamselec | 26:bbcc25418ffa | 72 | printColourDescription(block.averageColour); |
dreamselec | 26:bbcc25418ffa | 73 | printColourDescription(block.maxColour); |
dreamselec | 26:bbcc25418ffa | 74 | //TODO: print errors |
dreamselec | 26:bbcc25418ffa | 75 | pc.printf(":VALUE"); |
dreamselec | 20:4e0f0944f28f | 76 | } |
dreamselec | 20:4e0f0944f28f | 77 | |
dreamselec | 10:16ba52f8e025 | 78 | int readSwitches() |
dreamselec | 10:16ba52f8e025 | 79 | { |
dreamselec | 20:4e0f0944f28f | 80 | if(i2cport->read_bit(8)) { |
dreamselec | 20:4e0f0944f28f | 81 | while (i2cport->read_bit(8)) { } |
dreamselec | 20:4e0f0944f28f | 82 | return 1; |
dreamselec | 20:4e0f0944f28f | 83 | } else if (i2cport->read_bit(9)) { |
dreamselec | 20:4e0f0944f28f | 84 | while (i2cport->read_bit(9)) { } |
dreamselec | 20:4e0f0944f28f | 85 | return 2; |
dreamselec | 20:4e0f0944f28f | 86 | } else if (i2cport->read_bit(10)) { |
dreamselec | 20:4e0f0944f28f | 87 | while (i2cport->read_bit(10)) { } |
dreamselec | 20:4e0f0944f28f | 88 | return 3; |
dreamselec | 20:4e0f0944f28f | 89 | } else if (i2cport->read_bit(11)) { |
dreamselec | 20:4e0f0944f28f | 90 | while (i2cport->read_bit(11)) { } |
dreamselec | 20:4e0f0944f28f | 91 | return 4; |
dreamselec | 20:4e0f0944f28f | 92 | } else { |
dreamselec | 20:4e0f0944f28f | 93 | return 0; |
dreamselec | 20:4e0f0944f28f | 94 | } |
dreamselec | 10:16ba52f8e025 | 95 | |
dreamselec |
8:e1da2ae62885 | 96 | } |
dreamselec |
8:e1da2ae62885 | 97 | |
dreamselec | 10:16ba52f8e025 | 98 | void connectToPC(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 99 | { |
dreamselec | 20:4e0f0944f28f | 100 | connectedToPC = true; |
dreamselec | 24:02c61793f90b | 101 | pc.printf("INFO:PC connected to MBED.\n"); |
dreamselec | 10:16ba52f8e025 | 102 | } |
dreamselec | 10:16ba52f8e025 | 103 | |
dreamselec | 10:16ba52f8e025 | 104 | void disconnectToPC(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 105 | { |
dreamselec | 24:02c61793f90b | 106 | pc.printf("INFO:PC disconnected to MBED.\n"); |
dreamselec | 20:4e0f0944f28f | 107 | connectedToPC = false; |
dreamselec | 20:4e0f0944f28f | 108 | currentMode = None; |
dreamselec |
8:e1da2ae62885 | 109 | } |
dreamselec |
8:e1da2ae62885 | 110 | |
dreamselec | 10:16ba52f8e025 | 111 | void hazBlock(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 112 | { |
dreamselec | 20:4e0f0944f28f | 113 | if (typeRaw == Set) { |
dreamselec |
29:9c0339e3c593 | 114 | trySetHazBlockAgain: |
dreamselec |
29:9c0339e3c593 | 115 | pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour); |
dreamselec | 27:2cb1bdb7ae3d | 116 | fpga->moveSortingServo(Haz); |
dreamselec | 20:4e0f0944f28f | 117 | pc.printf("INFO:Setting new haz block.\n"); |
dreamselec | 20:4e0f0944f28f | 118 | int lowerBeam = 0; |
dreamselec | 20:4e0f0944f28f | 119 | int higherBeam = 0; |
dreamselec |
29:9c0339e3c593 | 120 | int colourValues[6][4]; |
dreamselec |
29:9c0339e3c593 | 121 | for (int i = 0; i < 6; i++){ |
dreamselec |
29:9c0339e3c593 | 122 | memset(colourValues[i], 0, sizeof(colourValues[6])); |
dreamselec |
29:9c0339e3c593 | 123 | } |
dreamselec |
29:9c0339e3c593 | 124 | int readingCount = 0; |
dreamselec | 20:4e0f0944f28f | 125 | |
dreamselec | 20:4e0f0944f28f | 126 | lcd->cls(); |
dreamselec | 20:4e0f0944f28f | 127 | lcd->locate(0,0); |
dreamselec | 20:4e0f0944f28f | 128 | lcd->printf("New haz block"); |
dreamselec | 27:2cb1bdb7ae3d | 129 | |
dreamselec | 20:4e0f0944f28f | 130 | do { |
dreamselec | 20:4e0f0944f28f | 131 | higherBeam = fpga->getBeamValue(Top); |
dreamselec | 20:4e0f0944f28f | 132 | if (readSwitches() == 4) { |
dreamselec | 20:4e0f0944f28f | 133 | if (displayAbortDialog()) { |
dreamselec | 27:2cb1bdb7ae3d | 134 | //TODO: tell pc |
dreamselec |
29:9c0339e3c593 | 135 | pc.printf(":<mbed>haz-block=pause;"); |
dreamselec | 20:4e0f0944f28f | 136 | pc.printf("INFO: Operation aborted form MBED.\n"); |
dreamselec | 27:2cb1bdb7ae3d | 137 | fpga->moveSortingServo(NonHaz); |
dreamselec | 20:4e0f0944f28f | 138 | displayPCStatus(); |
dreamselec | 20:4e0f0944f28f | 139 | return; |
dreamselec | 20:4e0f0944f28f | 140 | } else { |
dreamselec | 20:4e0f0944f28f | 141 | lcd->cls(); |
dreamselec | 20:4e0f0944f28f | 142 | lcd->locate(0,0); |
dreamselec | 20:4e0f0944f28f | 143 | lcd->printf("New haz block"); |
dreamselec | 20:4e0f0944f28f | 144 | } |
dreamselec |
29:9c0339e3c593 | 145 | } |
dreamselec |
29:9c0339e3c593 | 146 | } while (higherBeam != 1 && setNewHazBlock == true); |
dreamselec |
18:44a1c1a30166 | 147 | |
dreamselec |
29:9c0339e3c593 | 148 | if (setNewHazBlock == false) { |
dreamselec |
29:9c0339e3c593 | 149 | displayPCStatus(); |
dreamselec |
29:9c0339e3c593 | 150 | return; |
dreamselec |
29:9c0339e3c593 | 151 | } |
dreamselec | 27:2cb1bdb7ae3d | 152 | |
dreamselec | 20:4e0f0944f28f | 153 | do { |
dreamselec |
29:9c0339e3c593 | 154 | rgbSensor.getAllColors(colourValues[readingCount]); |
dreamselec |
29:9c0339e3c593 | 155 | readingCount++; |
dreamselec |
29:9c0339e3c593 | 156 | } while (readingCount < 6 && fpga->getBeamValue(Top) == 1 && fpga->getBeamValue(Bottom) == 0); |
dreamselec |
29:9c0339e3c593 | 157 | |
dreamselec |
29:9c0339e3c593 | 158 | lowerBeam = fpga->getBeamValue(Bottom); |
dreamselec |
29:9c0339e3c593 | 159 | higherBeam = fpga->getBeamValue(Top); |
dreamselec |
29:9c0339e3c593 | 160 | if (lowerBeam != 1){ |
dreamselec |
29:9c0339e3c593 | 161 | lowerBeam = fpga->getBeamValue(Top); |
dreamselec |
29:9c0339e3c593 | 162 | while(lowerBeam != 1) { lowerBeam = fpga->getBeamValue(Bottom); } |
dreamselec |
29:9c0339e3c593 | 163 | higherBeam = fpga->getBeamValue(Bottom); |
dreamselec |
29:9c0339e3c593 | 164 | } |
dreamselec |
29:9c0339e3c593 | 165 | Block::Size blockSize = higherBeam; |
dreamselec |
29:9c0339e3c593 | 166 | |
dreamselec |
29:9c0339e3c593 | 167 | int totalComponents[3]; |
dreamselec |
29:9c0339e3c593 | 168 | memset(totalComponents, 0, sizeof(totalComponents)); |
dreamselec |
29:9c0339e3c593 | 169 | for (int k = 0; k < readingCount; k++){ |
dreamselec |
29:9c0339e3c593 | 170 | for (int i = 0; i < 4; i++){ |
dreamselec |
29:9c0339e3c593 | 171 | totalComponents[i] += colourValues[k][i]; |
dreamselec |
29:9c0339e3c593 | 172 | } |
dreamselec |
29:9c0339e3c593 | 173 | } |
dreamselec |
29:9c0339e3c593 | 174 | float averageComponents[3]; |
dreamselec |
29:9c0339e3c593 | 175 | memset(averageComponents, 0, sizeof(averageComponents)); |
dreamselec |
29:9c0339e3c593 | 176 | |
dreamselec |
29:9c0339e3c593 | 177 | for (int i = 0; i < 4; i++){ |
dreamselec |
29:9c0339e3c593 | 178 | averageComponents = (float)totalComponents[i] / (float)readingCount; |
dreamselec |
29:9c0339e3c593 | 179 | } |
dreamselec |
29:9c0339e3c593 | 180 | |
dreamselec |
29:9c0339e3c593 | 181 | for (int i = 0; i < 3; i++){ |
dreamselec |
29:9c0339e3c593 | 182 | adjustedValues[i] = averageComponents[i] / averageComponents[i]; |
dreamselec |
29:9c0339e3c593 | 183 | } |
dreamselec |
29:9c0339e3c593 | 184 | |
dreamselec |
29:9c0339e3c593 | 185 | Block::BlockColour detectedColour = Block::Wrong; |
dreamselec |
29:9c0339e3c593 | 186 | bool matchesColour[3] = {false, false, false}; |
dreamselec |
29:9c0339e3c593 | 187 | |
dreamselec |
29:9c0339e3c593 | 188 | for (int k = 0; k < 7; k++){ |
dreamselec |
29:9c0339e3c593 | 189 | for (int i = 0; i < 3; i++) { |
dreamselec |
29:9c0339e3c593 | 190 | percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i]; |
dreamselec |
18:44a1c1a30166 | 191 | |
dreamselec |
29:9c0339e3c593 | 192 | if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[HazBlock->colour][i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[HazBlock->colour][i] * errorMultiplier)) { |
dreamselec |
29:9c0339e3c593 | 193 | matchesColour[i] = true; |
dreamselec |
29:9c0339e3c593 | 194 | } |
dreamselec |
29:9c0339e3c593 | 195 | if (matchesColour[0] && matchesColour[1] && matchesColour[2]){ |
dreamselec |
29:9c0339e3c593 | 196 | detectedColour = k; |
dreamselec |
29:9c0339e3c593 | 197 | break; |
dreamselec |
29:9c0339e3c593 | 198 | } |
dreamselec |
29:9c0339e3c593 | 199 | } |
dreamselec | 20:4e0f0944f28f | 200 | } |
dreamselec |
29:9c0339e3c593 | 201 | |
dreamselec |
29:9c0339e3c593 | 202 | if (detectedColour != Block::Wrong){ |
dreamselec |
29:9c0339e3c593 | 203 | pc.printf("ERROR: Could not detect colour.\n"); |
dreamselec |
29:9c0339e3c593 | 204 | lcd->cls(); |
dreamselec |
29:9c0339e3c593 | 205 | lcd->printf("1: Try again"); |
dreamselec |
29:9c0339e3c593 | 206 | lcd->locate(1,0); |
dreamselec |
29:9c0339e3c593 | 207 | lcd->printf("2: Revert to last"); |
dreamselec |
29:9c0339e3c593 | 208 | int button = 0; |
dreamselec |
29:9c0339e3c593 | 209 | do { |
dreamselec |
29:9c0339e3c593 | 210 | button = readSwitches(); |
dreamselec |
29:9c0339e3c593 | 211 | if (button == 1){ |
dreamselec |
29:9c0339e3c593 | 212 | goto trySetHazBlockAgain; |
dreamselec |
29:9c0339e3c593 | 213 | } |
dreamselec |
29:9c0339e3c593 | 214 | } while (button != 2); |
dreamselec |
29:9c0339e3c593 | 215 | } |
dreamselec |
29:9c0339e3c593 | 216 | |
dreamselec |
29:9c0339e3c593 | 217 | // Point and literal might not sync... |
dreamselec |
29:9c0339e3c593 | 218 | _HazBlock.size = blockSize; |
dreamselec |
29:9c0339e3c593 | 219 | _HazBlock.colour = detectedColour; |
dreamselec |
29:9c0339e3c593 | 220 | |
dreamselec |
29:9c0339e3c593 | 221 | pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour); |
dreamselec |
29:9c0339e3c593 | 222 | |
dreamselec |
29:9c0339e3c593 | 223 | |
dreamselec |
29:9c0339e3c593 | 224 | pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\n:VALUE", HazBlock->size, kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][2], kMaxError[HazBlock->colour][0], kMaxError[HazBlock->colour][1], kMaxError[HazBlock->colour][2]); |
dreamselec |
29:9c0339e3c593 | 225 | pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[HazBlock->colour][0], kAverageValues[HazBlock->colour][1], kAverageValues[HazBlock->colour][2], kAverageValues[HazBlock->colour][3]); |
dreamselec | 20:4e0f0944f28f | 226 | fpga->moveSortingServo(NonHaz); |
dreamselec | 20:4e0f0944f28f | 227 | } else if (typeRaw == Query) { |
dreamselec |
29:9c0339e3c593 | 228 | pc.printf("VALUE:Hazardous Block:\n \tSize:%i \n \tMin Error:%i, %i, %i\n \t Max Error:%i, %i, %i\n:VALUE", HazBlock->size, kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][1], kMinError[HazBlock->colour][2], kMaxError[HazBlock->colour][0], kMaxError[HazBlock->colour][1], kMaxError[HazBlock->colour][2]); |
dreamselec |
29:9c0339e3c593 | 229 | pc.printf("VALUE:\tAverage Colour:%.3f, %.3f, %.3f, %.3f\n:VALUE", kAverageValues[HazBlock->colour][0], kAverageValues[HazBlock->colour][1], kAverageValues[HazBlock->colour][2], kAverageValues[HazBlock->colour][3]); |
dreamselec | 20:4e0f0944f28f | 230 | } |
dreamselec |
8:e1da2ae62885 | 231 | } |
dreamselec |
8:e1da2ae62885 | 232 | |
dreamselec | 10:16ba52f8e025 | 233 | void getCurrentBlock(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 234 | { |
dreamselec | 20:4e0f0944f28f | 235 | pc.printf("DEBUG: Getting current block readings\n"); |
dreamselec |
8:e1da2ae62885 | 236 | } |
dreamselec |
8:e1da2ae62885 | 237 | |
dreamselec | 15:777390eb5afd | 238 | void setIntegrationTimeTo(float integrationTime) |
dreamselec | 10:16ba52f8e025 | 239 | { |
dreamselec | 25:792540d69c49 | 240 | gIntegrationTime = integrationTime; |
dreamselec | 25:792540d69c49 | 241 | rgbSensor.setIntegrationTime(gIntegrationTime); |
dreamselec | 25:792540d69c49 | 242 | pc.printf("DEBUG: Setting integration-time to %i.\n", gIntegrationTime); |
dreamselec |
8:e1da2ae62885 | 243 | } |
dreamselec |
8:e1da2ae62885 | 244 | |
dreamselec | 10:16ba52f8e025 | 245 | void previewOnPC(bool on) |
dreamselec | 10:16ba52f8e025 | 246 | { |
dreamselec | 20:4e0f0944f28f | 247 | pc.printf("setting preview on pc to %i.\n", on); |
dreamselec |
8:e1da2ae62885 | 248 | } |
dreamselec |
8:e1da2ae62885 | 249 | |
dreamselec | 25:792540d69c49 | 250 | void testColourSensor(Controls state){ |
dreamselec | 25:792540d69c49 | 251 | if (state == Start){ |
dreamselec | 25:792540d69c49 | 252 | pc.printf("INFO: Running colour test.\n"); |
dreamselec | 25:792540d69c49 | 253 | runColourSensorTest = true; |
dreamselec | 25:792540d69c49 | 254 | } else if (state == Pause){ |
dreamselec | 25:792540d69c49 | 255 | pc.printf("INFO: Finished colour test.\n"); |
dreamselec | 25:792540d69c49 | 256 | runColourSensorTest = false; |
dreamselec | 25:792540d69c49 | 257 | } |
dreamselec | 25:792540d69c49 | 258 | } |
dreamselec | 25:792540d69c49 | 259 | |
dreamselec | 20:4e0f0944f28f | 260 | void readColourSensor() |
dreamselec | 20:4e0f0944f28f | 261 | { |
dreamselec | 20:4e0f0944f28f | 262 | int colourValue[4]; |
dreamselec | 20:4e0f0944f28f | 263 | rgbSensor.getAllColors(colourValue); |
dreamselec | 20:4e0f0944f28f | 264 | pc.printf( "VALEU:Colour Reading:%i,%i,%i,%i\n:VALUE", colourValue[0], colourValue[1], colourValue[2], colourValue[3]); |
dreamselec |
18:44a1c1a30166 | 265 | |
dreamselec |
8:e1da2ae62885 | 266 | } |
dreamselec |
8:e1da2ae62885 | 267 | |
dreamselec | 17:af373246bf80 | 268 | void testServos(Controls state) |
dreamselec | 10:16ba52f8e025 | 269 | { |
dreamselec | 20:4e0f0944f28f | 270 | if (state == Start) { |
dreamselec | 20:4e0f0944f28f | 271 | pc.printf("INFO: Running servo test.\n"); |
dreamselec | 20:4e0f0944f28f | 272 | runServoTest = true; |
dreamselec | 20:4e0f0944f28f | 273 | } else if (state == Pause) { |
dreamselec | 20:4e0f0944f28f | 274 | pc.printf("INFO: Finished running servo test.\n"); |
dreamselec | 20:4e0f0944f28f | 275 | runServoTest = false; |
dreamselec | 20:4e0f0944f28f | 276 | } |
dreamselec |
8:e1da2ae62885 | 277 | } |
dreamselec |
8:e1da2ae62885 | 278 | |
dreamselec | 10:16ba52f8e025 | 279 | void resetServos() |
dreamselec | 10:16ba52f8e025 | 280 | { |
dreamselec | 20:4e0f0944f28f | 281 | pc.printf("resetting servos.\n"); |
dreamselec |
8:e1da2ae62885 | 282 | } |
dreamselec |
8:e1da2ae62885 | 283 | |
dreamselec | 10:16ba52f8e025 | 284 | void getPortInfo() |
dreamselec | 10:16ba52f8e025 | 285 | { |
dreamselec | 20:4e0f0944f28f | 286 | pc.printf("getting port info.\n"); |
dreamselec |
8:e1da2ae62885 | 287 | } |
dreamselec |
8:e1da2ae62885 | 288 | |
dreamselec | 17:af373246bf80 | 289 | void setPortBaudRate(int baudRate) |
dreamselec | 10:16ba52f8e025 | 290 | { |
dreamselec | 20:4e0f0944f28f | 291 | pc.baud(baudRate); |
dreamselec | 20:4e0f0944f28f | 292 | wait(0.1); |
dreamselec | 20:4e0f0944f28f | 293 | pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate); |
dreamselec | 10:16ba52f8e025 | 294 | } |
dreamselec | 10:16ba52f8e025 | 295 | |
dreamselec | 20:4e0f0944f28f | 296 | void setPortParity(int parity) |
dreamselec | 20:4e0f0944f28f | 297 | { |
dreamselec | 20:4e0f0944f28f | 298 | SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity); |
dreamselec | 20:4e0f0944f28f | 299 | pc.format(8, _parity, 1); |
dreamselec | 20:4e0f0944f28f | 300 | wait(0.1); |
dreamselec | 20:4e0f0944f28f | 301 | pc.printf("DEBUG: Setting port parity to: %i.\n", parity); |
dreamselec |
14:cf2f255b5560 | 302 | } |
dreamselec |
23:db91aaa43a9e | 303 | |
dreamselec |
23:db91aaa43a9e | 304 | void testBreakBeams(Controls state){ |
dreamselec |
23:db91aaa43a9e | 305 | if (state == Start){ |
dreamselec |
23:db91aaa43a9e | 306 | pc.printf("INFO: Running break beam test.\n"); |
dreamselec |
23:db91aaa43a9e | 307 | runBreakBeamTest = true; |
dreamselec |
23:db91aaa43a9e | 308 | }else if (state == Pause){ |
dreamselec |
23:db91aaa43a9e | 309 | pc.printf("INFO: Exiting break beam test.\n"); |
dreamselec |
23:db91aaa43a9e | 310 | runBreakBeamTest = false; |
dreamselec |
23:db91aaa43a9e | 311 | } |
dreamselec |
23:db91aaa43a9e | 312 | } |