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@32:5badeff825dc, 2017-03-02 (annotated)

Committer:
hazheng
Date:
Thu Mar 02 23:56:42 2017 +0000
Revision:
32:5badeff825dc
Parent:
29:f87d8790f57d
Child:
33:e3fcc4d6bb9b
Successfully get the first picture from the camera and displayed on the computer screen.

Who changed what in which revision?

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