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@35:ac4fcca21560, 2017-03-13 (annotated)

Committer:
hazheng
Date:
Mon Mar 13 02:43:53 2017 +0000
Revision:
35:ac4fcca21560
Parent:
34:f79db3bc2f86
Child:
36:7e747e19f660
Added More test code for camera.

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