![](/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@11:0fe833f8a1ab, 2015-11-21 (annotated)
- Committer:
- dreamselec
- Date:
- Sat Nov 21 18:52:07 2015 +0000
- Revision:
- 11:0fe833f8a1ab
- Parent:
- 10:16ba52f8e025
- Child:
- 14:cf2f255b5560
Testing with hardware, working fine. Implemented servo test mode. Need to change sorting algorithm because now we're reading colour right after top beam break, and size after that.
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 |
8:e1da2ae62885 | 17 | |
dreamselec | 10:16ba52f8e025 | 18 | int readSwitches() |
dreamselec | 10:16ba52f8e025 | 19 | { |
dreamselec | 10:16ba52f8e025 | 20 | |
dreamselec | 10:16ba52f8e025 | 21 | if(i2cport->read_bit(8)) { |
dreamselec | 10:16ba52f8e025 | 22 | return 1; |
dreamselec | 10:16ba52f8e025 | 23 | } else if (i2cport->read_bit(9)) { |
dreamselec | 10:16ba52f8e025 | 24 | return 2; |
dreamselec | 10:16ba52f8e025 | 25 | } else if (i2cport->read_bit(10)) { |
dreamselec | 10:16ba52f8e025 | 26 | return 3; |
dreamselec | 10:16ba52f8e025 | 27 | } else if (i2cport->read_bit(11)) { |
dreamselec | 10:16ba52f8e025 | 28 | return 4; |
dreamselec | 10:16ba52f8e025 | 29 | } else { |
dreamselec | 10:16ba52f8e025 | 30 | return 0; |
dreamselec | 10:16ba52f8e025 | 31 | } |
dreamselec | 10:16ba52f8e025 | 32 | |
dreamselec |
8:e1da2ae62885 | 33 | } |
dreamselec |
8:e1da2ae62885 | 34 | |
dreamselec | 10:16ba52f8e025 | 35 | void connectToPC(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 36 | { |
dreamselec | 10:16ba52f8e025 | 37 | pc.printf("pc told to connect\n"); |
dreamselec | 10:16ba52f8e025 | 38 | connectedToPC = true; |
dreamselec | 10:16ba52f8e025 | 39 | } |
dreamselec | 10:16ba52f8e025 | 40 | |
dreamselec | 10:16ba52f8e025 | 41 | void disconnectToPC(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 42 | { |
dreamselec | 10:16ba52f8e025 | 43 | pc.printf("pc told to disconnect\n"); |
dreamselec | 10:16ba52f8e025 | 44 | connectedToPC = false; |
dreamselec |
8:e1da2ae62885 | 45 | } |
dreamselec |
8:e1da2ae62885 | 46 | |
dreamselec | 10:16ba52f8e025 | 47 | void hazBlock(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 48 | { |
dreamselec | 10:16ba52f8e025 | 49 | if (typeRaw == Set) { |
dreamselec | 11:0fe833f8a1ab | 50 | pc.printf("Setting new haz block.\n"); |
dreamselec | 10:16ba52f8e025 | 51 | // pc.printf("Send 'y' when block is inserted.\n"); |
dreamselec | 10:16ba52f8e025 | 52 | int lowerBeam = 0; |
dreamselec | 10:16ba52f8e025 | 53 | int higherBeam = 0; |
dreamselec | 10:16ba52f8e025 | 54 | char readyChar = '\0'; |
dreamselec | 10:16ba52f8e025 | 55 | |
dreamselec | 10:16ba52f8e025 | 56 | do { higherBeam = fpga->checkForSize(); |
dreamselec | 10:16ba52f8e025 | 57 | } while (higherBeam != 1); |
dreamselec | 10:16ba52f8e025 | 58 | |
dreamselec | 10:16ba52f8e025 | 59 | int colourValue[4]; |
dreamselec | 10:16ba52f8e025 | 60 | rgbSensor.getAllColors(colourValue); |
dreamselec | 10:16ba52f8e025 | 61 | |
dreamselec | 10:16ba52f8e025 | 62 | do { |
dreamselec | 10:16ba52f8e025 | 63 | // readyChar = pc.getc(); |
dreamselec | 10:16ba52f8e025 | 64 | lowerBeam = fpga->checkForBlock(); |
dreamselec | 10:16ba52f8e025 | 65 | // TODO: Check beam reading |
dreamselec | 10:16ba52f8e025 | 66 | // if (readyChar != 'y'){ |
dreamselec | 10:16ba52f8e025 | 67 | // pc.printf("No block detected by beam\nTry again."); |
dreamselec | 10:16ba52f8e025 | 68 | // } |
dreamselec | 10:16ba52f8e025 | 69 | } while (lowerBeam != 1); |
dreamselec | 10:16ba52f8e025 | 70 | higherBeam = fpga->checkForSize(); |
dreamselec | 10:16ba52f8e025 | 71 | |
dreamselec | 10:16ba52f8e025 | 72 | _HazBlock = Block(); |
dreamselec | 10:16ba52f8e025 | 73 | if (higherBeam == 1) |
dreamselec | 10:16ba52f8e025 | 74 | _HazBlock.size = Block::Big; |
dreamselec | 10:16ba52f8e025 | 75 | else if (higherBeam == 0) |
dreamselec | 10:16ba52f8e025 | 76 | _HazBlock.size = Block::Small; |
dreamselec | 10:16ba52f8e025 | 77 | for (int i = 0; i < 4; i++) { |
dreamselec | 10:16ba52f8e025 | 78 | _HazBlock.minColour.components[i] = colourValue[i] - ColourSensorError; |
dreamselec | 10:16ba52f8e025 | 79 | _HazBlock.maxColour.components[i] = colourValue[i] + ColourSensorError; |
dreamselec | 10:16ba52f8e025 | 80 | } |
dreamselec | 10:16ba52f8e025 | 81 | fpga->moveSortingServo(Haz); |
dreamselec | 10:16ba52f8e025 | 82 | fpga->moveStoppingServo(Go); |
dreamselec | 10:16ba52f8e025 | 83 | while (fpga->checkForBlock()) {} |
dreamselec | 11:0fe833f8a1ab | 84 | fpga->moveStoppingServo(Stop); |
dreamselec | 10:16ba52f8e025 | 85 | fpga->moveSortingServo(NonHaz); |
dreamselec | 9:dc8f155b71c8 | 86 | |
dreamselec | 10:16ba52f8e025 | 87 | 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 | 10:16ba52f8e025 | 88 | } else if (typeRaw == Query) { |
dreamselec | 10:16ba52f8e025 | 89 | 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 | 10:16ba52f8e025 | 90 | } |
dreamselec |
8:e1da2ae62885 | 91 | } |
dreamselec |
8:e1da2ae62885 | 92 | |
dreamselec | 10:16ba52f8e025 | 93 | void getCurrentBlock(CommandTypeRaw typeRaw) |
dreamselec | 10:16ba52f8e025 | 94 | { |
dreamselec | 10:16ba52f8e025 | 95 | pc.printf("getting current block readings\n"); |
dreamselec |
8:e1da2ae62885 | 96 | } |
dreamselec |
8:e1da2ae62885 | 97 | |
dreamselec | 10:16ba52f8e025 | 98 | void setIntegrationTime(int integrationTime) |
dreamselec | 10:16ba52f8e025 | 99 | { |
dreamselec | 10:16ba52f8e025 | 100 | rgbSensor.setIntegrationTime(integrationTime); |
dreamselec | 10:16ba52f8e025 | 101 | pc.printf("setting i-time to %i.\n", integrationTime); |
dreamselec |
8:e1da2ae62885 | 102 | } |
dreamselec |
8:e1da2ae62885 | 103 | |
dreamselec | 10:16ba52f8e025 | 104 | void previewOnPC(bool on) |
dreamselec | 10:16ba52f8e025 | 105 | { |
dreamselec | 10:16ba52f8e025 | 106 | pc.printf("setting preview on pc to %i.\n", on); |
dreamselec |
8:e1da2ae62885 | 107 | } |
dreamselec |
8:e1da2ae62885 | 108 | |
dreamselec | 10:16ba52f8e025 | 109 | void readColourSensor(){ |
dreamselec | 10:16ba52f8e025 | 110 | int colourValue[4]; |
dreamselec | 10:16ba52f8e025 | 111 | rgbSensor.getAllColors(colourValue); |
dreamselec | 10:16ba52f8e025 | 112 | pc.printf( "Colour Reading:%i,%i,%i,%i\n", colourValue[0], colourValue[1], colourValue[2], colourValue[3]); |
dreamselec | 10:16ba52f8e025 | 113 | |
dreamselec |
8:e1da2ae62885 | 114 | } |
dreamselec |
8:e1da2ae62885 | 115 | |
dreamselec | 10:16ba52f8e025 | 116 | |
dreamselec | 10:16ba52f8e025 | 117 | void testServos() |
dreamselec | 10:16ba52f8e025 | 118 | { |
dreamselec | 10:16ba52f8e025 | 119 | runServoTest = true; |
dreamselec |
8:e1da2ae62885 | 120 | } |
dreamselec |
8:e1da2ae62885 | 121 | |
dreamselec | 10:16ba52f8e025 | 122 | void resetServos() |
dreamselec | 10:16ba52f8e025 | 123 | { |
dreamselec | 10:16ba52f8e025 | 124 | pc.printf("resetting servos.\n"); |
dreamselec |
8:e1da2ae62885 | 125 | } |
dreamselec |
8:e1da2ae62885 | 126 | |
dreamselec | 10:16ba52f8e025 | 127 | void getPortInfo() |
dreamselec | 10:16ba52f8e025 | 128 | { |
dreamselec | 10:16ba52f8e025 | 129 | pc.printf("getting port info.\n"); |
dreamselec |
8:e1da2ae62885 | 130 | } |
dreamselec |
8:e1da2ae62885 | 131 | |
dreamselec | 10:16ba52f8e025 | 132 | void setPortBaudRate(int baudRate) |
dreamselec | 10:16ba52f8e025 | 133 | { |
dreamselec | 10:16ba52f8e025 | 134 | pc.printf("setting port baudrate to: %i\n", baudRate); |
dreamselec | 10:16ba52f8e025 | 135 | } |
dreamselec | 10:16ba52f8e025 | 136 |