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@33:e3fcc4d6bb9b, 2017-03-03 (annotated)

Committer:
hazheng
Date:
Fri Mar 03 18:26:44 2017 +0000
Revision:
33:e3fcc4d6bb9b
Parent:
32:5badeff825dc
Child:
34:f79db3bc2f86
Added adjustable size.

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