![](/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@18:44a1c1a30166, 2015-11-24 (annotated)
- Committer:
- dreamselec
- Date:
- Tue Nov 24 00:24:39 2015 +0000
- Revision:
- 18:44a1c1a30166
- Parent:
- 17:af373246bf80
- Child:
- 19:61b21ac4896e
Implemented new algorithm, now supports different modes on PC.
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 | 10:16ba52f8e025 | 9 | int ColourSensorError = 25; |
dreamselec | 2:7a55cb10259f | 10 | //SerialBase gParity = SerialBase::None; |
dreamselec | 2:7a55cb10259f | 11 | int gStopBits = 1; |
dreamselec | 3:843b830ee8bd | 12 | |
dreamselec | 7:b6e31bfdb2af | 13 | Block _HazBlock = Block(kDefaultHazBlock); |
dreamselec |
8:e1da2ae62885 | 14 | |
dreamselec |
8:e1da2ae62885 | 15 | bool connectedToPC = false; |
dreamselec | 10:16ba52f8e025 | 16 | bool runServoTest = false; |
dreamselec | 17:af373246bf80 | 17 | PCModes currentMode = None; |
dreamselec |
18:44a1c1a30166 | 18 | Controls currentState = Pause; |
dreamselec |
8:e1da2ae62885 | 19 | |
dreamselec | 10:16ba52f8e025 | 20 | int readSwitches() |
dreamselec | 10:16ba52f8e025 | 21 | { |
dreamselec |
18:44a1c1a30166 | 22 | if(i2cport->read_bit(8)) { |
dreamselec |
18:44a1c1a30166 | 23 | while (i2cport->read_bit(8)) { } |
dreamselec |
18:44a1c1a30166 | 24 | return 1; |
dreamselec |
18:44a1c1a30166 | 25 | } else if (i2cport->read_bit(9)) { |
dreamselec |
18:44a1c1a30166 | 26 | while (i2cport->read_bit(9)) { } |
dreamselec |
18:44a1c1a30166 | 27 | return 2; |
dreamselec |
18:44a1c1a30166 | 28 | } else if (i2cport->read_bit(10)) { |
dreamselec |
18:44a1c1a30166 | 29 | while (i2cport->read_bit(10)) { } |
dreamselec |
18:44a1c1a30166 | 30 | return 3; |
dreamselec |
18:44a1c1a30166 | 31 | } else if (i2cport->read_bit(11)) { |
dreamselec |
18:44a1c1a30166 | 32 | while (i2cport->read_bit(11)) { } |
dreamselec |
18:44a1c1a30166 | 33 | return 4; |
dreamselec |
18:44a1c1a30166 | 34 | } else { |
dreamselec |
18:44a1c1a30166 | 35 | return 0; |
dreamselec |
18:44a1c1a30166 | 36 | } |
dreamselec | 10:16ba52f8e025 | 37 | |
dreamselec |
8:e1da2ae62885 | 38 | } |
dreamselec |
8:e1da2ae62885 | 39 | |
dreamselec | 10:16ba52f8e025 | 40 | void connectToPC(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 41 | { |
dreamselec |
18:44a1c1a30166 | 42 | connectedToPC = true; |
dreamselec |
18:44a1c1a30166 | 43 | pc.printf("INFO:MBED connected to PC.\n"); |
dreamselec | 10:16ba52f8e025 | 44 | } |
dreamselec | 10:16ba52f8e025 | 45 | |
dreamselec | 10:16ba52f8e025 | 46 | void disconnectToPC(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 47 | { |
dreamselec |
18:44a1c1a30166 | 48 | pc.printf("INFO:MBED disconnected from PC.\n"); |
dreamselec |
18:44a1c1a30166 | 49 | connectedToPC = false; |
dreamselec |
18:44a1c1a30166 | 50 | currentMode = None; |
dreamselec |
8:e1da2ae62885 | 51 | } |
dreamselec |
8:e1da2ae62885 | 52 | |
dreamselec | 10:16ba52f8e025 | 53 | void hazBlock(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 54 | { |
dreamselec |
18:44a1c1a30166 | 55 | if (typeRaw == Set) { |
dreamselec |
18:44a1c1a30166 | 56 | pc.printf("Setting new haz block.\n"); |
dreamselec |
18:44a1c1a30166 | 57 | // pc.printf("Send 'y' when block is inserted.\n"); |
dreamselec |
18:44a1c1a30166 | 58 | int lowerBeam = 0; |
dreamselec |
18:44a1c1a30166 | 59 | int higherBeam = 0; |
dreamselec |
18:44a1c1a30166 | 60 | char readyChar = '\0'; |
dreamselec |
18:44a1c1a30166 | 61 | |
dreamselec |
18:44a1c1a30166 | 62 | do { higherBeam = fpga->checkForSize(); |
dreamselec |
18:44a1c1a30166 | 63 | } while (higherBeam != 1); |
dreamselec |
18:44a1c1a30166 | 64 | |
dreamselec |
18:44a1c1a30166 | 65 | int colourValue[4]; |
dreamselec |
18:44a1c1a30166 | 66 | rgbSensor.getAllColors(colourValue); |
dreamselec | 10:16ba52f8e025 | 67 | |
dreamselec |
18:44a1c1a30166 | 68 | do { |
dreamselec |
18:44a1c1a30166 | 69 | // readyChar = pc.getc(); |
dreamselec |
18:44a1c1a30166 | 70 | lowerBeam = fpga->checkForBlock(); |
dreamselec |
18:44a1c1a30166 | 71 | // TODO: Check beam reading |
dreamselec |
18:44a1c1a30166 | 72 | // if (readyChar != 'y'){ |
dreamselec |
18:44a1c1a30166 | 73 | // pc.printf("No block detected by beam\nTry again."); |
dreamselec |
18:44a1c1a30166 | 74 | // } |
dreamselec |
18:44a1c1a30166 | 75 | } while (lowerBeam != 1); |
dreamselec |
18:44a1c1a30166 | 76 | higherBeam = fpga->checkForSize(); |
dreamselec |
18:44a1c1a30166 | 77 | |
dreamselec |
18:44a1c1a30166 | 78 | _HazBlock = Block(); |
dreamselec |
18:44a1c1a30166 | 79 | if (higherBeam == 1) |
dreamselec |
18:44a1c1a30166 | 80 | _HazBlock.size = Block::Big; |
dreamselec |
18:44a1c1a30166 | 81 | else if (higherBeam == 0) |
dreamselec |
18:44a1c1a30166 | 82 | _HazBlock.size = Block::Small; |
dreamselec |
18:44a1c1a30166 | 83 | for (int i = 0; i < 4; i++) { |
dreamselec |
18:44a1c1a30166 | 84 | _HazBlock.minColour.components[i] = colourValue[i] - ColourSensorError; |
dreamselec |
18:44a1c1a30166 | 85 | _HazBlock.maxColour.components[i] = colourValue[i] + ColourSensorError; |
dreamselec |
18:44a1c1a30166 | 86 | } |
dreamselec |
18:44a1c1a30166 | 87 | fpga->moveSortingServo(Haz); |
dreamselec |
18:44a1c1a30166 | 88 | fpga->moveStoppingServo(Go); |
dreamselec |
18:44a1c1a30166 | 89 | while (fpga->checkForBlock()) {} |
dreamselec |
18:44a1c1a30166 | 90 | fpga->moveStoppingServo(Stop); |
dreamselec |
18:44a1c1a30166 | 91 | fpga->moveSortingServo(NonHaz); |
dreamselec |
18:44a1c1a30166 | 92 | |
dreamselec |
18:44a1c1a30166 | 93 | pc.printf( "HazBlock:\n \t Size:%i\n \t Real Colour:%i,%i,%i,%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _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 |
18:44a1c1a30166 | 94 | } else if (typeRaw == Query) { |
dreamselec |
18:44a1c1a30166 | 95 | pc.printf( "HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _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 |
18:44a1c1a30166 | 96 | } |
dreamselec |
8:e1da2ae62885 | 97 | } |
dreamselec |
8:e1da2ae62885 | 98 | |
dreamselec | 10:16ba52f8e025 | 99 | void getCurrentBlock(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 100 | { |
dreamselec |
18:44a1c1a30166 | 101 | pc.printf("DEBUG: Getting current block readings\n"); |
dreamselec |
8:e1da2ae62885 | 102 | } |
dreamselec |
8:e1da2ae62885 | 103 | |
dreamselec | 15:777390eb5afd | 104 | void setIntegrationTimeTo(float integrationTime) |
dreamselec | 10:16ba52f8e025 | 105 | { |
dreamselec |
18:44a1c1a30166 | 106 | rgbSensor.setIntegrationTime(integrationTime); |
dreamselec |
18:44a1c1a30166 | 107 | pc.printf("DEBUG: Setting integration-time to %i.\n", integrationTime); |
dreamselec |
8:e1da2ae62885 | 108 | } |
dreamselec |
8:e1da2ae62885 | 109 | |
dreamselec | 10:16ba52f8e025 | 110 | void previewOnPC(bool on) |
dreamselec | 10:16ba52f8e025 | 111 | { |
dreamselec |
18:44a1c1a30166 | 112 | pc.printf("setting preview on pc to %i.\n", on); |
dreamselec |
8:e1da2ae62885 | 113 | } |
dreamselec |
8:e1da2ae62885 | 114 | |
dreamselec | 10:16ba52f8e025 | 115 | void readColourSensor(){ |
dreamselec | 10:16ba52f8e025 | 116 | int colourValue[4]; |
dreamselec |
18:44a1c1a30166 | 117 | rgbSensor.getAllColors(colourValue); |
dreamselec |
18:44a1c1a30166 | 118 | pc.printf( "Colour Reading:%i,%i,%i,%i\n", colourValue[0], colourValue[1], colourValue[2], colourValue[3]); |
dreamselec |
18:44a1c1a30166 | 119 | |
dreamselec |
8:e1da2ae62885 | 120 | } |
dreamselec |
8:e1da2ae62885 | 121 | |
dreamselec | 10:16ba52f8e025 | 122 | |
dreamselec | 17:af373246bf80 | 123 | void testServos(Controls state) |
dreamselec | 10:16ba52f8e025 | 124 | { |
dreamselec |
18:44a1c1a30166 | 125 | if (state == Start){ |
dreamselec |
18:44a1c1a30166 | 126 | pc.printf("INFO: Running servo test.\n"); |
dreamselec |
18:44a1c1a30166 | 127 | runServoTest = true; |
dreamselec | 17:af373246bf80 | 128 | }else if (state == Pause){ |
dreamselec | 17:af373246bf80 | 129 | pc.printf("INFO: Finished running servo test.\n"); |
dreamselec | 17:af373246bf80 | 130 | runServoTest = false; |
dreamselec | 17:af373246bf80 | 131 | } |
dreamselec |
8:e1da2ae62885 | 132 | } |
dreamselec |
8:e1da2ae62885 | 133 | |
dreamselec | 10:16ba52f8e025 | 134 | void resetServos() |
dreamselec | 10:16ba52f8e025 | 135 | { |
dreamselec |
18:44a1c1a30166 | 136 | pc.printf("resetting servos.\n"); |
dreamselec |
8:e1da2ae62885 | 137 | } |
dreamselec |
8:e1da2ae62885 | 138 | |
dreamselec | 10:16ba52f8e025 | 139 | void getPortInfo() |
dreamselec | 10:16ba52f8e025 | 140 | { |
dreamselec |
18:44a1c1a30166 | 141 | pc.printf("getting port info.\n"); |
dreamselec |
8:e1da2ae62885 | 142 | } |
dreamselec |
8:e1da2ae62885 | 143 | |
dreamselec | 17:af373246bf80 | 144 | void setPortBaudRate(int baudRate) |
dreamselec | 10:16ba52f8e025 | 145 | { |
dreamselec |
14:cf2f255b5560 | 146 | pc.baud(baudRate); |
dreamselec | 17:af373246bf80 | 147 | wait(0.1); |
dreamselec |
18:44a1c1a30166 | 148 | pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate); |
dreamselec | 10:16ba52f8e025 | 149 | } |
dreamselec | 10:16ba52f8e025 | 150 | |
dreamselec |
14:cf2f255b5560 | 151 | void setPortParity(int parity){ |
dreamselec |
14:cf2f255b5560 | 152 | SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity); |
dreamselec |
14:cf2f255b5560 | 153 | pc.format(8, _parity, 1); |
dreamselec | 17:af373246bf80 | 154 | wait(0.1); |
dreamselec | 17:af373246bf80 | 155 | pc.printf("DEBUG: Setting port parity to: %i.\n", parity); |
dreamselec |
14:cf2f255b5560 | 156 | } |