Jordan Brack / SmartWheelsPublic

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 haofan Zheng

RemovedSources/Camera.cpp.txt@100:ffbeefc9e218, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 21:04:10 2017 +0000
Revision:
100:ffbeefc9e218
Parent:
36:7e747e19f660 
Better version of Intersection detection.

Who changed what in which revision?

UserRevisionLine numberNew 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