SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.
Dependencies: TSI USBDevice mbed-dev
Fork of SmartWheels by
RemovedSources/Camera.cpp.txt@36:7e747e19f660, 2017-03-14 (annotated)
- Committer:
- hazheng
- Date:
- Tue Mar 14 22:23:15 2017 +0000
- Revision:
- 36:7e747e19f660
- Parent:
- Hardwares/Camera.cpp@29:f87d8790f57d
Removed trash codes. Made it easier to switch different camera.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
hazheng | 3:c8867972ffc7 | 1 | #include "Camera.h" |
hazheng | 3:c8867972ffc7 | 2 | |
hazheng | 13:7dcb1642ef99 | 3 | #include "Core.h" |
hazheng | 13:7dcb1642ef99 | 4 | #include "OV7725RegAddr.h" |
hazheng | 13:7dcb1642ef99 | 5 | #include "PinAssignment.h" |
hazheng | 13:7dcb1642ef99 | 6 | #include "SWUSBServer.h" |
hazheng | 13:7dcb1642ef99 | 7 | |
hazheng | 26:5814404856e2 | 8 | namespace |
hazheng | 3:c8867972ffc7 | 9 | { |
hazheng | 26:5814404856e2 | 10 | DigitalOut testLED(LED_GREEN, 1); |
hazheng | 27:c68f711e5b67 | 11 | DigitalOut testLED2(LED_RED, 1); |
hazheng | 27:c68f711e5b67 | 12 | |
hazheng | 27:c68f711e5b67 | 13 | BusIn dataBus(PIN_CC_D_0, PIN_CC_D_1, PIN_CC_D_2, PIN_CC_D_3, PIN_CC_D_4, PIN_CC_D_5, PIN_CC_D_6, PIN_CC_D_7); |
hazheng | 25:6f63053cee81 | 14 | |
hazheng | 26:5814404856e2 | 15 | bool isInLine = false; |
hazheng | 26:5814404856e2 | 16 | uint8_t currentCol = 0; |
hazheng | 26:5814404856e2 | 17 | uint8_t outCurrentCol = 0; |
hazheng | 26:5814404856e2 | 18 | |
hazheng | 26:5814404856e2 | 19 | uint16_t currentRow = 0; |
hazheng | 26:5814404856e2 | 20 | uint16_t outCurrentRow = 0; |
hazheng | 27:c68f711e5b67 | 21 | |
hazheng | 27:c68f711e5b67 | 22 | uint16_t outENum = 0; |
hazheng | 27:c68f711e5b67 | 23 | |
hazheng | 27:c68f711e5b67 | 24 | uint8_t pictureData[PICTURE_ARRAY_SIZE]; |
hazheng | 27:c68f711e5b67 | 25 | |
hazheng | 27:c68f711e5b67 | 26 | bool hasPic = false; |
hazheng | 3:c8867972ffc7 | 27 | } |
hazheng | 3:c8867972ffc7 | 28 | |
hazheng | 26:5814404856e2 | 29 | |
hazheng | 26:5814404856e2 | 30 | static void OnPixelClock() |
hazheng | 3:c8867972ffc7 | 31 | { |
hazheng | 27:c68f711e5b67 | 32 | if(hasPic) |
hazheng | 27:c68f711e5b67 | 33 | return; |
hazheng | 27:c68f711e5b67 | 34 | |
hazheng | 27:c68f711e5b67 | 35 | unsigned int pos = (currentRow * (CAMERA_PIXEL_WIDTH/8)) + currentCol; |
hazheng | 27:c68f711e5b67 | 36 | if(isInLine && (pos < PICTURE_ARRAY_SIZE)) |
hazheng | 26:5814404856e2 | 37 | { |
hazheng | 27:c68f711e5b67 | 38 | pictureData[pos] = dataBus.read(); |
hazheng | 26:5814404856e2 | 39 | ++currentCol; |
hazheng | 26:5814404856e2 | 40 | } |
hazheng | 27:c68f711e5b67 | 41 | /* |
hazheng | 27:c68f711e5b67 | 42 | else if(pos > PICTURE_ARRAY_SIZE) |
hazheng | 27:c68f711e5b67 | 43 | { |
hazheng | 27:c68f711e5b67 | 44 | outENum = pos; |
hazheng | 27:c68f711e5b67 | 45 | testLED2 = 0; |
hazheng | 27:c68f711e5b67 | 46 | } |
hazheng | 27:c68f711e5b67 | 47 | */ |
hazheng | 3:c8867972ffc7 | 48 | } |
hazheng | 3:c8867972ffc7 | 49 | |
hazheng | 26:5814404856e2 | 50 | static void OnHorizontalRise() |
hazheng | 3:c8867972ffc7 | 51 | { |
hazheng | 27:c68f711e5b67 | 52 | if(hasPic) |
hazheng | 27:c68f711e5b67 | 53 | return; |
hazheng | 27:c68f711e5b67 | 54 | |
hazheng | 26:5814404856e2 | 55 | isInLine = true; |
hazheng | 25:6f63053cee81 | 56 | |
hazheng | 27:c68f711e5b67 | 57 | //++currentRow; |
hazheng | 25:6f63053cee81 | 58 | } |
hazheng | 25:6f63053cee81 | 59 | |
hazheng | 26:5814404856e2 | 60 | static void OnHorizontalFall() |
hazheng | 25:6f63053cee81 | 61 | { |
hazheng | 27:c68f711e5b67 | 62 | if(hasPic) |
hazheng | 27:c68f711e5b67 | 63 | return; |
hazheng | 27:c68f711e5b67 | 64 | |
hazheng | 26:5814404856e2 | 65 | isInLine = false; |
hazheng | 3:c8867972ffc7 | 66 | |
hazheng | 27:c68f711e5b67 | 67 | if(currentCol > 0) |
hazheng | 27:c68f711e5b67 | 68 | { |
hazheng | 27:c68f711e5b67 | 69 | ++currentRow; |
hazheng | 27:c68f711e5b67 | 70 | } |
hazheng | 26:5814404856e2 | 71 | outCurrentCol = currentCol; |
hazheng | 26:5814404856e2 | 72 | currentCol = 0; |
hazheng | 26:5814404856e2 | 73 | } |
hazheng | 26:5814404856e2 | 74 | |
hazheng | 27:c68f711e5b67 | 75 | static void OnFrameRise() |
hazheng | 26:5814404856e2 | 76 | { |
hazheng | 27:c68f711e5b67 | 77 | if(currentRow >= CAMERA_PIXEL_HEIGHT) |
hazheng | 27:c68f711e5b67 | 78 | hasPic = true; |
hazheng | 27:c68f711e5b67 | 79 | } |
hazheng | 27:c68f711e5b67 | 80 | |
hazheng | 27:c68f711e5b67 | 81 | static void OnFrameFall() |
hazheng | 27:c68f711e5b67 | 82 | { |
hazheng | 27:c68f711e5b67 | 83 | if(hasPic) |
hazheng | 27:c68f711e5b67 | 84 | return; |
hazheng | 27:c68f711e5b67 | 85 | |
hazheng | 26:5814404856e2 | 86 | outCurrentRow = currentRow; |
hazheng | 26:5814404856e2 | 87 | currentRow = 0; |
hazheng | 3:c8867972ffc7 | 88 | testLED = testLED.read() == 1 ? 0 : 1; |
hazheng | 3:c8867972ffc7 | 89 | } |
hazheng | 3:c8867972ffc7 | 90 | |
hazheng | 13:7dcb1642ef99 | 91 | Camera::Camera(SW::Core & core) : |
hazheng | 13:7dcb1642ef99 | 92 | m_core(core), |
hazheng | 25:6f63053cee81 | 93 | m_pClock(InterruptIn(PIN_CC_PCLOCK)), |
hazheng | 25:6f63053cee81 | 94 | m_href(InterruptIn(PIN_CC_HREF)), |
hazheng | 25:6f63053cee81 | 95 | m_vsnyc(InterruptIn(PIN_CC_VSYNC)), |
hazheng | 26:5814404856e2 | 96 | m_regBuf(NULL), |
hazheng | 26:5814404856e2 | 97 | m_debugOutputTimer(0.0f) |
hazheng | 3:c8867972ffc7 | 98 | { |
hazheng | 26:5814404856e2 | 99 | m_regBuf = new OV7725RegBuf(m_core); |
hazheng | 26:5814404856e2 | 100 | |
hazheng | 29:f87d8790f57d | 101 | /* |
hazheng | 27:c68f711e5b67 | 102 | m_regBuf->WriteDefaultRegisters(); |
hazheng | 27:c68f711e5b67 | 103 | |
hazheng | 26:5814404856e2 | 104 | #if (CAMERA_PIXEL_WIDTH == 80) |
hazheng | 26:5814404856e2 | 105 | m_regBuf->SCCBWrite(OV7725_HOutSize, 0x14); |
hazheng | 26:5814404856e2 | 106 | #elif (CAMERA_PIXEL_WIDTH == 160) |
hazheng | 26:5814404856e2 | 107 | m_regBuf->SCCBWrite(OV7725_HOutSize, 0x28); |
hazheng | 26:5814404856e2 | 108 | #elif (CAMERA_PIXEL_WIDTH == 240) |
hazheng | 26:5814404856e2 | 109 | m_regBuf->SCCBWrite(OV7725_HOutSize, 0x3c); |
hazheng | 26:5814404856e2 | 110 | #elif (CAMERA_PIXEL_WIDTH == 320) |
hazheng | 26:5814404856e2 | 111 | m_regBuf->SCCBWrite(OV7725_HOutSize, 0x50); |
hazheng | 26:5814404856e2 | 112 | #endif |
hazheng | 26:5814404856e2 | 113 | |
hazheng | 26:5814404856e2 | 114 | #if (CAMERA_PIXEL_HEIGHT == 60 ) |
hazheng | 26:5814404856e2 | 115 | m_regBuf->SCCBWrite(OV7725_VOutSize, 0x1E); |
hazheng | 26:5814404856e2 | 116 | #elif (CAMERA_PIXEL_HEIGHT == 120 ) |
hazheng | 26:5814404856e2 | 117 | m_regBuf->SCCBWrite(OV7725_VOutSize, 0x3c); |
hazheng | 26:5814404856e2 | 118 | #elif (CAMERA_PIXEL_HEIGHT == 180 ) |
hazheng | 26:5814404856e2 | 119 | m_regBuf->SCCBWrite(OV7725_VOutSize, 0x5a); |
hazheng | 26:5814404856e2 | 120 | #elif (CAMERA_PIXEL_HEIGHT == 240 ) |
hazheng | 26:5814404856e2 | 121 | m_regBuf->SCCBWrite(OV7725_VOutSize, 0x78); |
hazheng | 26:5814404856e2 | 122 | #endif |
hazheng | 3:c8867972ffc7 | 123 | |
hazheng | 26:5814404856e2 | 124 | m_regBuf->SCCBWrite(OV7725_COM4, 0x01); |
hazheng | 27:c68f711e5b67 | 125 | m_regBuf->SCCBWrite(OV7725_CLKRC, 0x89); |
hazheng | 29:f87d8790f57d | 126 | */ |
hazheng | 26:5814404856e2 | 127 | char buf[20]; |
hazheng | 26:5814404856e2 | 128 | unsigned char tempV = m_regBuf->SCCBRead(OV7725_HOutSize); |
hazheng | 26:5814404856e2 | 129 | sprintf(buf, "CamReg %#x=%#x", OV7725_HOutSize, tempV); |
hazheng | 26:5814404856e2 | 130 | m_core.GetUSBServer().PushReliableMsg('D', buf); |
hazheng | 26:5814404856e2 | 131 | |
hazheng | 26:5814404856e2 | 132 | tempV = m_regBuf->SCCBRead(OV7725_VOutSize); |
hazheng | 26:5814404856e2 | 133 | sprintf(buf, "CamReg %#x=%#x", OV7725_VOutSize, tempV); |
hazheng | 26:5814404856e2 | 134 | m_core.GetUSBServer().PushReliableMsg('D', buf); |
hazheng | 26:5814404856e2 | 135 | |
hazheng | 26:5814404856e2 | 136 | tempV = m_regBuf->SCCBRead(OV7725_COM4); |
hazheng | 26:5814404856e2 | 137 | sprintf(buf, "CamReg %#x=%#x", OV7725_COM4, tempV); |
hazheng | 26:5814404856e2 | 138 | m_core.GetUSBServer().PushReliableMsg('D', buf); |
hazheng | 26:5814404856e2 | 139 | |
hazheng | 26:5814404856e2 | 140 | tempV = m_regBuf->SCCBRead(OV7725_CLKRC); |
hazheng | 26:5814404856e2 | 141 | sprintf(buf, "CamReg %#x=%#x", OV7725_CLKRC, tempV); |
hazheng | 26:5814404856e2 | 142 | m_core.GetUSBServer().PushReliableMsg('D', buf); |
hazheng | 26:5814404856e2 | 143 | |
hazheng | 26:5814404856e2 | 144 | delete m_regBuf; |
hazheng | 26:5814404856e2 | 145 | m_regBuf = NULL; |
hazheng | 26:5814404856e2 | 146 | |
hazheng | 27:c68f711e5b67 | 147 | memset(pictureData, 0, PICTURE_ARRAY_SIZE); |
hazheng | 27:c68f711e5b67 | 148 | |
hazheng | 26:5814404856e2 | 149 | StartReceivingPic(); |
hazheng | 3:c8867972ffc7 | 150 | } |
hazheng | 3:c8867972ffc7 | 151 | |
hazheng | 3:c8867972ffc7 | 152 | Camera::~Camera() |
hazheng | 3:c8867972ffc7 | 153 | { |
hazheng | 3:c8867972ffc7 | 154 | |
hazheng | 3:c8867972ffc7 | 155 | } |
hazheng | 3:c8867972ffc7 | 156 | |
hazheng | 26:5814404856e2 | 157 | void Camera::Update(float deltaTime) |
hazheng | 26:5814404856e2 | 158 | { |
hazheng | 26:5814404856e2 | 159 | m_debugOutputTimer += deltaTime; |
hazheng | 26:5814404856e2 | 160 | |
hazheng | 27:c68f711e5b67 | 161 | //if(hasPic) |
hazheng | 27:c68f711e5b67 | 162 | //{ |
hazheng | 27:c68f711e5b67 | 163 | // StopReceivingPic(); |
hazheng | 27:c68f711e5b67 | 164 | //} |
hazheng | 27:c68f711e5b67 | 165 | |
hazheng | 27:c68f711e5b67 | 166 | if(m_debugOutputTimer >= 10.0) |
hazheng | 26:5814404856e2 | 167 | { |
hazheng | 26:5814404856e2 | 168 | char buf[20]; |
hazheng | 26:5814404856e2 | 169 | sprintf(buf, "ColSize: %u", outCurrentCol); |
hazheng | 26:5814404856e2 | 170 | m_core.GetUSBServer().PushUnreliableMsg('D', buf); |
hazheng | 26:5814404856e2 | 171 | sprintf(buf, "RowSize: %u", outCurrentRow); |
hazheng | 26:5814404856e2 | 172 | m_core.GetUSBServer().PushUnreliableMsg('D', buf); |
hazheng | 27:c68f711e5b67 | 173 | //sprintf(buf, "ENum: %u", outENum); |
hazheng | 27:c68f711e5b67 | 174 | //m_core.GetUSBServer().PushUnreliableMsg('D', buf); |
hazheng | 26:5814404856e2 | 175 | |
hazheng | 26:5814404856e2 | 176 | m_debugOutputTimer = 0.0f; |
hazheng | 27:c68f711e5b67 | 177 | |
hazheng | 27:c68f711e5b67 | 178 | //if(hasPic) |
hazheng | 27:c68f711e5b67 | 179 | //{ |
hazheng | 27:c68f711e5b67 | 180 | StopReceivingPic(); |
hazheng | 27:c68f711e5b67 | 181 | testLED2 = testLED2.read() == 1 ? 0 : 1; |
hazheng | 27:c68f711e5b67 | 182 | |
hazheng | 27:c68f711e5b67 | 183 | for(uint8_t i = 0; i < CAMERA_PIXEL_HEIGHT; ++i) |
hazheng | 27:c68f711e5b67 | 184 | { |
hazheng | 27:c68f711e5b67 | 185 | std::string outBuf; |
hazheng | 27:c68f711e5b67 | 186 | outBuf.push_back(i); |
hazheng | 27:c68f711e5b67 | 187 | outBuf.insert(outBuf.end(), &pictureData[i * (CAMERA_PIXEL_WIDTH / 8)], &pictureData[(i + 1) * (CAMERA_PIXEL_WIDTH / 8)]); |
hazheng | 27:c68f711e5b67 | 188 | m_core.GetUSBServer().PushReliableMsg('P', outBuf); |
hazheng | 27:c68f711e5b67 | 189 | wait(0.02); |
hazheng | 27:c68f711e5b67 | 190 | } |
hazheng | 27:c68f711e5b67 | 191 | |
hazheng | 27:c68f711e5b67 | 192 | StartReceivingPic(); |
hazheng | 27:c68f711e5b67 | 193 | //} |
hazheng | 26:5814404856e2 | 194 | } |
hazheng | 26:5814404856e2 | 195 | |
hazheng | 26:5814404856e2 | 196 | } |
hazheng | 26:5814404856e2 | 197 | |
hazheng | 26:5814404856e2 | 198 | void Camera::StartReceivingPic() |
hazheng | 26:5814404856e2 | 199 | { |
hazheng | 27:c68f711e5b67 | 200 | hasPic = false; |
hazheng | 27:c68f711e5b67 | 201 | |
hazheng | 26:5814404856e2 | 202 | m_pClock.rise(&OnPixelClock); |
hazheng | 26:5814404856e2 | 203 | m_href.rise(&OnHorizontalRise); |
hazheng | 26:5814404856e2 | 204 | m_href.fall(&OnHorizontalFall); |
hazheng | 27:c68f711e5b67 | 205 | m_vsnyc.fall(&OnFrameFall); |
hazheng | 27:c68f711e5b67 | 206 | //m_vsnyc.rise(&OnFrameRise); |
hazheng | 26:5814404856e2 | 207 | } |
hazheng | 26:5814404856e2 | 208 | |
hazheng | 26:5814404856e2 | 209 | void Camera::StopReceivingPic() |
hazheng | 26:5814404856e2 | 210 | { |
hazheng | 27:c68f711e5b67 | 211 | //m_vsnyc.rise(NULL); |
hazheng | 26:5814404856e2 | 212 | m_vsnyc.fall(NULL); |
hazheng | 26:5814404856e2 | 213 | m_href.fall(NULL); |
hazheng | 26:5814404856e2 | 214 | m_href.rise(NULL); |
hazheng | 26:5814404856e2 | 215 | m_pClock.rise(NULL); |
hazheng | 26:5814404856e2 | 216 | } |
hazheng | 26:5814404856e2 | 217 | |
hazheng | 3:c8867972ffc7 | 218 | bool Camera::HasPicture() const |
hazheng | 3:c8867972ffc7 | 219 | { |
hazheng | 27:c68f711e5b67 | 220 | return hasPic; |
hazheng | 3:c8867972ffc7 | 221 | } |
hazheng | 3:c8867972ffc7 | 222 | |
hazheng | 3:c8867972ffc7 | 223 | const unsigned char * Camera::GetPicture() const |
hazheng | 3:c8867972ffc7 | 224 | { |
hazheng | 13:7dcb1642ef99 | 225 | return NULL; //m_pics[m_currentIndex == 0 ? 1 : 0]; |
hazheng | 13:7dcb1642ef99 | 226 | } |
hazheng | 13:7dcb1642ef99 | 227 |