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/ArduCAM.cpp@43:0d1886f4848a, 2017-03-27 (annotated)

Committer:
hazheng
Date:
Mon Mar 27 22:09:22 2017 +0000
Revision:
43:0d1886f4848a
Parent:
42:c4e1606087ff
Child:
44:15de535c4005
display live camera view on screen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 28:271fc8445e89 1 #include "ArduCAM.h"
hazheng 28:271fc8445e89 2
hazheng 33:e3fcc4d6bb9b 3
hazheng 29:f87d8790f57d 4 #include "GlobalVariable.h"
hazheng 29:f87d8790f57d 5 #include "SWUSBServer.h"
hazheng 29:f87d8790f57d 6
hazheng 32:5badeff825dc 7 #include "CamRegBuf.h"
hazheng 32:5badeff825dc 8
hazheng 41:7b21c5e3599e 9 #include "ArduUTFT.h"
hazheng 41:7b21c5e3599e 10
hazheng 32:5badeff825dc 11 #include <string>
hazheng 32:5badeff825dc 12
hazheng 28:271fc8445e89 13 extern SPI g_spi_port;
hazheng 28:271fc8445e89 14
hazheng 41:7b21c5e3599e 15 DigitalOut cam_cs(PIN_ACC_CS, 1);
hazheng 29:f87d8790f57d 16
hazheng 34:f79db3bc2f86 17
hazheng 34:f79db3bc2f86 18 inline void ardu_cam_spi_write_8(int address, int value)
hazheng 29:f87d8790f57d 19 {
hazheng 34:f79db3bc2f86 20 // take the SS pin low to select the chip:
hazheng 34:f79db3bc2f86 21 cam_cs = 0;
hazheng 34:f79db3bc2f86 22 // send in the address and value via SPI:
hazheng 34:f79db3bc2f86 23 g_spi_port.write(address | 0x80);
hazheng 34:f79db3bc2f86 24 g_spi_port.write(value);
hazheng 34:f79db3bc2f86 25 // take the SS pin high to de-select the chip:
hazheng 34:f79db3bc2f86 26 cam_cs = 1;
hazheng 34:f79db3bc2f86 27 }
hazheng 34:f79db3bc2f86 28
hazheng 34:f79db3bc2f86 29 inline uint8_t ardu_cam_spi_read_8(int address)
hazheng 34:f79db3bc2f86 30 {
hazheng 34:f79db3bc2f86 31 // take the SS pin low to select the chip:
hazheng 29:f87d8790f57d 32 cam_cs = 0;
hazheng 34:f79db3bc2f86 33 // send in the address and value via SPI:
hazheng 34:f79db3bc2f86 34 g_spi_port.write(address & 0x7F);
hazheng 34:f79db3bc2f86 35 uint8_t value = static_cast<uint8_t>(g_spi_port.write(0x00));
hazheng 34:f79db3bc2f86 36 // take the SS pin high to de-select the chip:
hazheng 29:f87d8790f57d 37 cam_cs = 1;
hazheng 34:f79db3bc2f86 38 return value;
hazheng 34:f79db3bc2f86 39 }
hazheng 34:f79db3bc2f86 40 /*
hazheng 34:f79db3bc2f86 41 inline void ardu_cam_spi_set_burst()
hazheng 34:f79db3bc2f86 42 {
hazheng 34:f79db3bc2f86 43 cam_cs = 0;
hazheng 34:f79db3bc2f86 44
hazheng 34:f79db3bc2f86 45 g_spi_port.write(BURST_FIFO_READ & 0x7F);
hazheng 29:f87d8790f57d 46
hazheng 34:f79db3bc2f86 47 cam_cs = 1;
hazheng 34:f79db3bc2f86 48 }
hazheng 34:f79db3bc2f86 49
hazheng 34:f79db3bc2f86 50 inline uint16_t ardu_cam_spi_burst_read_16()
hazheng 34:f79db3bc2f86 51 {
hazheng 34:f79db3bc2f86 52 cam_cs = 0;
hazheng 29:f87d8790f57d 53
hazheng 34:f79db3bc2f86 54 uint16_t value = static_cast<uint16_t>(g_spi_port.write(0x00));
hazheng 34:f79db3bc2f86 55 value = (value << 8) & 0xFF00;
hazheng 34:f79db3bc2f86 56 value = value | static_cast<uint16_t>(g_spi_port.write(0x00));
hazheng 34:f79db3bc2f86 57
hazheng 34:f79db3bc2f86 58 cam_cs = 1;
hazheng 34:f79db3bc2f86 59 return value;
hazheng 29:f87d8790f57d 60 }
hazheng 29:f87d8790f57d 61 */
hazheng 29:f87d8790f57d 62
hazheng 29:f87d8790f57d 63
hazheng 28:271fc8445e89 64 bool ardu_cam_init()
hazheng 28:271fc8445e89 65 {
hazheng 32:5badeff825dc 66 char buf[20];
hazheng 32:5badeff825dc 67
hazheng 36:7e747e19f660 68 CamRegBuf * camReg = new CamRegBuf(g_core, CAM_SCCB_WRITE, CAM_SCCB_READ);
hazheng 36:7e747e19f660 69
hazheng 36:7e747e19f660 70 camReg->WriteRegSet(ResetProg);
hazheng 32:5badeff825dc 71 wait_ms(5);
hazheng 37:7074a6118d03 72 camReg->WriteRegSet(QVGA);
hazheng 32:5badeff825dc 73 wait_ms(100);
hazheng 32:5badeff825dc 74
hazheng 36:7e747e19f660 75 #if defined(ARDUCAM_OV2640)
hazheng 32:5badeff825dc 76 camReg->SCCBWrite(0xff, 0x01);
hazheng 36:7e747e19f660 77 #endif
hazheng 36:7e747e19f660 78 uint8_t camVer = camReg->SCCBRead(CAM_PID_ADDR);
hazheng 32:5badeff825dc 79 sprintf(buf, "Cam VerH %#x", camVer);
hazheng 32:5badeff825dc 80 g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 32:5badeff825dc 81
hazheng 36:7e747e19f660 82 camVer = camReg->SCCBRead(CAM_VER_ADDR);
hazheng 32:5badeff825dc 83 sprintf(buf, "Cam VerL %#x", camVer);
hazheng 32:5badeff825dc 84 g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 36:7e747e19f660 85
hazheng 36:7e747e19f660 86
hazheng 32:5badeff825dc 87 delete camReg;
hazheng 32:5badeff825dc 88 camReg = NULL;
hazheng 32:5badeff825dc 89
hazheng 32:5badeff825dc 90
hazheng 32:5badeff825dc 91
hazheng 32:5badeff825dc 92
hazheng 34:f79db3bc2f86 93 uint8_t VerNum = ardu_cam_spi_read_8(0x40);
hazheng 34:f79db3bc2f86 94 VerNum = ardu_cam_spi_read_8(0x40);
hazheng 29:f87d8790f57d 95
hazheng 32:5badeff825dc 96
hazheng 29:f87d8790f57d 97 sprintf(buf, "Ardu Ver %#x", VerNum);
hazheng 29:f87d8790f57d 98 g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 29:f87d8790f57d 99
hazheng 34:f79db3bc2f86 100 ardu_cam_spi_write_8(ARDUCHIP_TEST1, ARDUCHIP_TEST_MSG);
hazheng 34:f79db3bc2f86 101 uint8_t testV = ardu_cam_spi_read_8(ARDUCHIP_TEST1);
hazheng 35:ac4fcca21560 102 if(VerNum != ARDUCHIP_VER_NUM || testV != ARDUCHIP_TEST_MSG)
hazheng 29:f87d8790f57d 103 {
hazheng 29:f87d8790f57d 104 g_core.GetUSBServer().PushReliableMsg('D', "CameraInit Fa");
hazheng 29:f87d8790f57d 105 return false;
hazheng 29:f87d8790f57d 106 }
hazheng 29:f87d8790f57d 107 g_core.GetUSBServer().PushReliableMsg('D', "CameraInit Su");
hazheng 29:f87d8790f57d 108
hazheng 35:ac4fcca21560 109 //ardu_cam_set_mode(MCU2LCD_MODE);
hazheng 43:0d1886f4848a 110 ardu_cam_spi_write_8(ARDUCHIP_CAP_CTRL, 0x00);
hazheng 29:f87d8790f57d 111 ardu_cam_start_capture();
hazheng 29:f87d8790f57d 112
hazheng 29:f87d8790f57d 113 wait(0.1);
hazheng 28:271fc8445e89 114
hazheng 28:271fc8445e89 115
hazheng 32:5badeff825dc 116 //unsigned char tempV = ardu_cam_read_reg(ARDUCHIP_MODE);
hazheng 32:5badeff825dc 117 //sprintf(buf, "Ardu Stat %#x", tempV);
hazheng 32:5badeff825dc 118 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 29:f87d8790f57d 119
hazheng 32:5badeff825dc 120 //tempV = ardu_cam_read_reg(ARDUCHIP_CAP_CTRL);
hazheng 32:5badeff825dc 121 //sprintf(buf, "Ardu FS1 %#x", tempV);
hazheng 32:5badeff825dc 122 //g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 29:f87d8790f57d 123
hazheng 29:f87d8790f57d 124 return true;
hazheng 29:f87d8790f57d 125 }
hazheng 29:f87d8790f57d 126
hazheng 29:f87d8790f57d 127 void ardu_cam_start_capture()
hazheng 29:f87d8790f57d 128 {
hazheng 34:f79db3bc2f86 129 ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_CLEAR_MASK);
hazheng 43:0d1886f4848a 130 //wait(0.1);
hazheng 34:f79db3bc2f86 131 ardu_cam_spi_write_8(ARDUCHIP_FIFO, FIFO_START_MASK);
hazheng 29:f87d8790f57d 132 }
hazheng 29:f87d8790f57d 133
hazheng 32:5badeff825dc 134 uint32_t ardu_cam_get_fifo_length()
hazheng 32:5badeff825dc 135 {
hazheng 32:5badeff825dc 136 uint32_t len1,len2,len3,length=0;
hazheng 34:f79db3bc2f86 137 len1 = ardu_cam_spi_read_8(FIFO_SIZE1);
hazheng 34:f79db3bc2f86 138 len2 = ardu_cam_spi_read_8(FIFO_SIZE2);
hazheng 34:f79db3bc2f86 139 len3 = ardu_cam_spi_read_8(FIFO_SIZE3) & 0x07;
hazheng 32:5badeff825dc 140 length = ((len3 << 16) | (len2 << 8) | len1) & 0x07ffff;
hazheng 32:5badeff825dc 141 return length;
hazheng 32:5badeff825dc 142 }
hazheng 32:5badeff825dc 143
hazheng 32:5badeff825dc 144 uint8_t ardu_cam_get_pixel()
hazheng 32:5badeff825dc 145 {
hazheng 34:f79db3bc2f86 146 uint16_t VH = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 34:f79db3bc2f86 147 uint16_t VL = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 34:f79db3bc2f86 148 //uint16_t VL = ardu_cam_spi_burst_read_16();
hazheng 32:5badeff825dc 149
hazheng 32:5badeff825dc 150 VL = (VL & 0x00FF) | ((VH << 8) & 0xFF00);
hazheng 33:e3fcc4d6bb9b 151 uint8_t ch = ((VL & 0xF800) >> 9);// << 2;
hazheng 33:e3fcc4d6bb9b 152 float pixel = (static_cast<float>(ch) * 0.21);
hazheng 32:5badeff825dc 153
hazheng 33:e3fcc4d6bb9b 154 ch = ((VL & 0x07E0) >> 3);// << 2;
hazheng 33:e3fcc4d6bb9b 155 pixel += (static_cast<float>(ch) * 0.72);
hazheng 32:5badeff825dc 156
hazheng 33:e3fcc4d6bb9b 157 ch = (VL & 0x001F) << 2;
hazheng 33:e3fcc4d6bb9b 158 pixel += (static_cast<float>(ch) * 0.07);
hazheng 32:5badeff825dc 159
hazheng 32:5badeff825dc 160 return static_cast<uint8_t>(pixel);
hazheng 32:5badeff825dc 161 }
hazheng 32:5badeff825dc 162
hazheng 32:5badeff825dc 163 void ardu_cam_print_debug()
hazheng 32:5badeff825dc 164 {
hazheng 32:5badeff825dc 165 uint32_t len = ardu_cam_get_fifo_length();
hazheng 32:5badeff825dc 166 char buf[20];
hazheng 32:5badeff825dc 167 sprintf(buf, "Cam L %#x", len);
hazheng 32:5badeff825dc 168 g_core.GetUSBServer().PushReliableMsg('D', buf);
hazheng 32:5badeff825dc 169
hazheng 33:e3fcc4d6bb9b 170 //if(len < (RESOLUTION_HEIGHT * RESOLUTION_WIDTH * 2))
hazheng 33:e3fcc4d6bb9b 171 // return;
hazheng 33:e3fcc4d6bb9b 172
hazheng 33:e3fcc4d6bb9b 173 //Begin output the picture
hazheng 32:5badeff825dc 174 std::string lineBuf;
hazheng 37:7074a6118d03 175 #if defined(MANUAL_REDUCE_RESULOTION_BY2)
hazheng 37:7074a6118d03 176 lineBuf.resize((RESOLUTION_WIDTH / 2) + 1);
hazheng 37:7074a6118d03 177 #else
hazheng 33:e3fcc4d6bb9b 178 lineBuf.resize(RESOLUTION_WIDTH + 1);
hazheng 37:7074a6118d03 179 #endif
hazheng 32:5badeff825dc 180
hazheng 34:f79db3bc2f86 181 //ardu_cam_spi_set_burst();
hazheng 41:7b21c5e3599e 182
hazheng 42:c4e1606087ff 183 //ardu_cam_get_pixel(); //Get the first dummy pixel
hazheng 41:7b21c5e3599e 184
hazheng 43:0d1886f4848a 185
hazheng 33:e3fcc4d6bb9b 186 for (uint8_t i = 0; i < RESOLUTION_HEIGHT; ++i)
hazheng 32:5badeff825dc 187 {
hazheng 37:7074a6118d03 188 #if defined(MANUAL_REDUCE_RESULOTION_BY2)
hazheng 37:7074a6118d03 189 lineBuf[0] = i / 2;
hazheng 37:7074a6118d03 190
hazheng 37:7074a6118d03 191 for (int j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 37:7074a6118d03 192 {
hazheng 37:7074a6118d03 193 if(i % 2 == 0 || j % 2 == 0)
hazheng 37:7074a6118d03 194 {
hazheng 37:7074a6118d03 195 ardu_cam_get_pixel();
hazheng 37:7074a6118d03 196 }
hazheng 37:7074a6118d03 197 else
hazheng 37:7074a6118d03 198 {
hazheng 37:7074a6118d03 199 lineBuf[(j / 2) + 1] = ardu_cam_get_pixel();
hazheng 37:7074a6118d03 200 }
hazheng 37:7074a6118d03 201 }
hazheng 37:7074a6118d03 202
hazheng 37:7074a6118d03 203 if(i % 2 == 0)
hazheng 37:7074a6118d03 204 {
hazheng 37:7074a6118d03 205
hazheng 37:7074a6118d03 206 }
hazheng 37:7074a6118d03 207 else
hazheng 37:7074a6118d03 208 {
hazheng 37:7074a6118d03 209 g_core.GetUSBServer().PushReliableMsg('P', lineBuf);
hazheng 37:7074a6118d03 210 wait(0.35);
hazheng 37:7074a6118d03 211 }
hazheng 37:7074a6118d03 212
hazheng 37:7074a6118d03 213 #else
hazheng 37:7074a6118d03 214
hazheng 32:5badeff825dc 215 lineBuf[0] = i;
hazheng 32:5badeff825dc 216
hazheng 33:e3fcc4d6bb9b 217 for (int j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 32:5badeff825dc 218 {
hazheng 33:e3fcc4d6bb9b 219 //uint8_t p = ardu_cam_get_pixel();
hazheng 33:e3fcc4d6bb9b 220 lineBuf[j + 1] = ardu_cam_get_pixel();
hazheng 32:5badeff825dc 221 }
hazheng 32:5badeff825dc 222
hazheng 32:5badeff825dc 223 g_core.GetUSBServer().PushReliableMsg('P', lineBuf);
hazheng 35:ac4fcca21560 224 wait(0.35);
hazheng 37:7074a6118d03 225
hazheng 37:7074a6118d03 226 #endif
hazheng 37:7074a6118d03 227
hazheng 32:5badeff825dc 228 }
hazheng 43:0d1886f4848a 229
hazheng 43:0d1886f4848a 230 }
hazheng 43:0d1886f4848a 231
hazheng 43:0d1886f4848a 232 void ardu_cam_display_img_utft()
hazheng 43:0d1886f4848a 233 {
hazheng 43:0d1886f4848a 234 ardu_cam_start_capture();
hazheng 43:0d1886f4848a 235 wait(0.2);
hazheng 43:0d1886f4848a 236 while(ardu_cam_get_fifo_length() < (RESOLUTION_WIDTH * RESOLUTION_HEIGHT * 2))
hazheng 43:0d1886f4848a 237 {
hazheng 43:0d1886f4848a 238 }
hazheng 43:0d1886f4848a 239 for (uint8_t i = 0; i < RESOLUTION_HEIGHT; ++i)
hazheng 43:0d1886f4848a 240 {
hazheng 43:0d1886f4848a 241 ardu_utft_set_xy(0, i + 20, 300, i + 20);
hazheng 43:0d1886f4848a 242 for (int j = 0; j < RESOLUTION_WIDTH; ++j)
hazheng 43:0d1886f4848a 243 {
hazheng 43:0d1886f4848a 244 uint8_t VH = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 43:0d1886f4848a 245 uint8_t VL = ardu_cam_spi_read_8(SINGLE_FIFO_READ);
hazheng 43:0d1886f4848a 246
hazheng 43:0d1886f4848a 247 ardu_utft_write_DATA(VH, VL);
hazheng 43:0d1886f4848a 248 }
hazheng 43:0d1886f4848a 249 }
hazheng 32:5badeff825dc 250 }