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

Hardwares/Camera.cpp@27:c68f711e5b67, 2017-02-21 (annotated)

Committer:
hazheng
Date:
Tue Feb 21 22:27:10 2017 +0000
Revision:
27:c68f711e5b67
Parent:
26:5814404856e2
Child:
29:f87d8790f57d
Finished the code to get picture from the camera, and push the data to computer side. (however, entire picture shows black.)

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 27:c68f711e5b67 101 m_regBuf->WriteDefaultRegisters();
hazheng 27:c68f711e5b67 102
hazheng 26:5814404856e2 103 #if (CAMERA_PIXEL_WIDTH == 80)
hazheng 26:5814404856e2 104 m_regBuf->SCCBWrite(OV7725_HOutSize, 0x14);
hazheng 26:5814404856e2 105 #elif (CAMERA_PIXEL_WIDTH == 160)
hazheng 26:5814404856e2 106 m_regBuf->SCCBWrite(OV7725_HOutSize, 0x28);
hazheng 26:5814404856e2 107 #elif (CAMERA_PIXEL_WIDTH == 240)
hazheng 26:5814404856e2 108 m_regBuf->SCCBWrite(OV7725_HOutSize, 0x3c);
hazheng 26:5814404856e2 109 #elif (CAMERA_PIXEL_WIDTH == 320)
hazheng 26:5814404856e2 110 m_regBuf->SCCBWrite(OV7725_HOutSize, 0x50);
hazheng 26:5814404856e2 111 #endif
hazheng 26:5814404856e2 112
hazheng 26:5814404856e2 113 #if (CAMERA_PIXEL_HEIGHT == 60 )
hazheng 26:5814404856e2 114 m_regBuf->SCCBWrite(OV7725_VOutSize, 0x1E);
hazheng 26:5814404856e2 115 #elif (CAMERA_PIXEL_HEIGHT == 120 )
hazheng 26:5814404856e2 116 m_regBuf->SCCBWrite(OV7725_VOutSize, 0x3c);
hazheng 26:5814404856e2 117 #elif (CAMERA_PIXEL_HEIGHT == 180 )
hazheng 26:5814404856e2 118 m_regBuf->SCCBWrite(OV7725_VOutSize, 0x5a);
hazheng 26:5814404856e2 119 #elif (CAMERA_PIXEL_HEIGHT == 240 )
hazheng 26:5814404856e2 120 m_regBuf->SCCBWrite(OV7725_VOutSize, 0x78);
hazheng 26:5814404856e2 121 #endif
hazheng 3:c8867972ffc7 122
hazheng 26:5814404856e2 123 m_regBuf->SCCBWrite(OV7725_COM4, 0x01);
hazheng 27:c68f711e5b67 124 m_regBuf->SCCBWrite(OV7725_CLKRC, 0x89);
hazheng 26:5814404856e2 125
hazheng 26:5814404856e2 126 char buf[20];
hazheng 26:5814404856e2 127 unsigned char tempV = m_regBuf->SCCBRead(OV7725_HOutSize);
hazheng 26:5814404856e2 128 sprintf(buf, "CamReg %#x=%#x", OV7725_HOutSize, tempV);
hazheng 26:5814404856e2 129 m_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 26:5814404856e2 130
hazheng 26:5814404856e2 131 tempV = m_regBuf->SCCBRead(OV7725_VOutSize);
hazheng 26:5814404856e2 132 sprintf(buf, "CamReg %#x=%#x", OV7725_VOutSize, tempV);
hazheng 26:5814404856e2 133 m_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 26:5814404856e2 134
hazheng 26:5814404856e2 135 tempV = m_regBuf->SCCBRead(OV7725_COM4);
hazheng 26:5814404856e2 136 sprintf(buf, "CamReg %#x=%#x", OV7725_COM4, tempV);
hazheng 26:5814404856e2 137 m_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 26:5814404856e2 138
hazheng 26:5814404856e2 139 tempV = m_regBuf->SCCBRead(OV7725_CLKRC);
hazheng 26:5814404856e2 140 sprintf(buf, "CamReg %#x=%#x", OV7725_CLKRC, tempV);
hazheng 26:5814404856e2 141 m_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 26:5814404856e2 142
hazheng 26:5814404856e2 143 delete m_regBuf;
hazheng 26:5814404856e2 144 m_regBuf = NULL;
hazheng 26:5814404856e2 145
hazheng 27:c68f711e5b67 146 memset(pictureData, 0, PICTURE_ARRAY_SIZE);
hazheng 27:c68f711e5b67 147
hazheng 26:5814404856e2 148 StartReceivingPic();
hazheng 3:c8867972ffc7 149 }
hazheng 3:c8867972ffc7 150
hazheng 3:c8867972ffc7 151 Camera::~Camera()
hazheng 3:c8867972ffc7 152 {
hazheng 3:c8867972ffc7 153
hazheng 3:c8867972ffc7 154 }
hazheng 3:c8867972ffc7 155
hazheng 26:5814404856e2 156 void Camera::Update(float deltaTime)
hazheng 26:5814404856e2 157 {
hazheng 26:5814404856e2 158 m_debugOutputTimer += deltaTime;
hazheng 26:5814404856e2 159
hazheng 27:c68f711e5b67 160 //if(hasPic)
hazheng 27:c68f711e5b67 161 //{
hazheng 27:c68f711e5b67 162 // StopReceivingPic();
hazheng 27:c68f711e5b67 163 //}
hazheng 27:c68f711e5b67 164
hazheng 27:c68f711e5b67 165 if(m_debugOutputTimer >= 10.0)
hazheng 26:5814404856e2 166 {
hazheng 26:5814404856e2 167 char buf[20];
hazheng 26:5814404856e2 168 sprintf(buf, "ColSize: %u", outCurrentCol);
hazheng 26:5814404856e2 169 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 26:5814404856e2 170 sprintf(buf, "RowSize: %u", outCurrentRow);
hazheng 26:5814404856e2 171 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 27:c68f711e5b67 172 //sprintf(buf, "ENum: %u", outENum);
hazheng 27:c68f711e5b67 173 //m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 26:5814404856e2 174
hazheng 26:5814404856e2 175 m_debugOutputTimer = 0.0f;
hazheng 27:c68f711e5b67 176
hazheng 27:c68f711e5b67 177 //if(hasPic)
hazheng 27:c68f711e5b67 178 //{
hazheng 27:c68f711e5b67 179 StopReceivingPic();
hazheng 27:c68f711e5b67 180 testLED2 = testLED2.read() == 1 ? 0 : 1;
hazheng 27:c68f711e5b67 181
hazheng 27:c68f711e5b67 182 for(uint8_t i = 0; i < CAMERA_PIXEL_HEIGHT; ++i)
hazheng 27:c68f711e5b67 183 {
hazheng 27:c68f711e5b67 184 std::string outBuf;
hazheng 27:c68f711e5b67 185 outBuf.push_back(i);
hazheng 27:c68f711e5b67 186 outBuf.insert(outBuf.end(), &pictureData[i * (CAMERA_PIXEL_WIDTH / 8)], &pictureData[(i + 1) * (CAMERA_PIXEL_WIDTH / 8)]);
hazheng 27:c68f711e5b67 187 m_core.GetUSBServer().PushReliableMsg('P', outBuf);
hazheng 27:c68f711e5b67 188 wait(0.02);
hazheng 27:c68f711e5b67 189 }
hazheng 27:c68f711e5b67 190
hazheng 27:c68f711e5b67 191 StartReceivingPic();
hazheng 27:c68f711e5b67 192 //}
hazheng 26:5814404856e2 193 }
hazheng 26:5814404856e2 194
hazheng 26:5814404856e2 195 }
hazheng 26:5814404856e2 196
hazheng 26:5814404856e2 197 void Camera::StartReceivingPic()
hazheng 26:5814404856e2 198 {
hazheng 27:c68f711e5b67 199 hasPic = false;
hazheng 27:c68f711e5b67 200
hazheng 26:5814404856e2 201 m_pClock.rise(&OnPixelClock);
hazheng 26:5814404856e2 202 m_href.rise(&OnHorizontalRise);
hazheng 26:5814404856e2 203 m_href.fall(&OnHorizontalFall);
hazheng 27:c68f711e5b67 204 m_vsnyc.fall(&OnFrameFall);
hazheng 27:c68f711e5b67 205 //m_vsnyc.rise(&OnFrameRise);
hazheng 26:5814404856e2 206 }
hazheng 26:5814404856e2 207
hazheng 26:5814404856e2 208 void Camera::StopReceivingPic()
hazheng 26:5814404856e2 209 {
hazheng 27:c68f711e5b67 210 //m_vsnyc.rise(NULL);
hazheng 26:5814404856e2 211 m_vsnyc.fall(NULL);
hazheng 26:5814404856e2 212 m_href.fall(NULL);
hazheng 26:5814404856e2 213 m_href.rise(NULL);
hazheng 26:5814404856e2 214 m_pClock.rise(NULL);
hazheng 26:5814404856e2 215 }
hazheng 26:5814404856e2 216
hazheng 3:c8867972ffc7 217 bool Camera::HasPicture() const
hazheng 3:c8867972ffc7 218 {
hazheng 27:c68f711e5b67 219 return hasPic;
hazheng 3:c8867972ffc7 220 }
hazheng 3:c8867972ffc7 221
hazheng 3:c8867972ffc7 222 const unsigned char * Camera::GetPicture() const
hazheng 3:c8867972ffc7 223 {
hazheng 13:7dcb1642ef99 224 return NULL; //m_pics[m_currentIndex == 0 ? 1 : 0];
hazheng 13:7dcb1642ef99 225 }
hazheng 13:7dcb1642ef99 226