angelo mottolese
3D point cloud scanner with OV7670 camera and line laser.
Dependencies: ADXL345_I2C_ HMC5843_ OV7670_ok SDFileSystem ST7735_TFT Servo mbed
Fork of
OV7670_edm_full
by 
angelo mottolese
3D point cloud scanner with OV7670 FIFO camera and line laser. I take the brightest pixels in an image, I measure their horizontal displacement to deduce a depth value, and I generate XYZ coordinates. The camera is rotating, and I know the angle, so I can map 3D space in cylindrical coordinates. The home of the project is http://angelomottolese.eu.ai/sitoScanner.html
main.cpp@0:aabbf2286bf2, 2012-03-27 (annotated)
- Committer:
- ms523
- Date:
- Tue Mar 27 19:10:18 2012 +0000
- Revision:
- 0:aabbf2286bf2
- Child:
- 1:c98598814170
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ms523 | 0:aabbf2286bf2 | 1 | // |
| ms523 | 0:aabbf2286bf2 | 2 | // OV7670 + FIFO AL422B camera board test |
| ms523 | 0:aabbf2286bf2 | 3 | // |
| ms523 | 0:aabbf2286bf2 | 4 | #include "mbed.h" |
| ms523 | 0:aabbf2286bf2 | 5 | #include "ov7670.h" |
| ms523 | 0:aabbf2286bf2 | 6 | #include "stdio.h" |
| ms523 | 0:aabbf2286bf2 | 7 | #include "SPI_TFT.h" |
| ms523 | 0:aabbf2286bf2 | 8 | #include "string" |
| ms523 | 0:aabbf2286bf2 | 9 | #include "Arial12x12.h" |
| ms523 | 0:aabbf2286bf2 | 10 | #include "Arial24x23.h" |
| ms523 | 0:aabbf2286bf2 | 11 | #include "Arial28x28.h" |
| ms523 | 0:aabbf2286bf2 | 12 | #include "font_big.h" |
| ms523 | 0:aabbf2286bf2 | 13 | |
| ms523 | 0:aabbf2286bf2 | 14 | OV7670 camera( |
| ms523 | 0:aabbf2286bf2 | 15 | p28,p27, // SDA,SCL(I2C / SCCB) |
| ms523 | 0:aabbf2286bf2 | 16 | p21,p22,p20, // VSYNC,HREF,WEN(FIFO) |
| ms523 | 0:aabbf2286bf2 | 17 | p19,p18,p17,p16,p15,p14,p13,p12, // D7-D0 |
| ms523 | 0:aabbf2286bf2 | 18 | p23,p24,p25) ; // RRST,OE,RCLK |
| ms523 | 0:aabbf2286bf2 | 19 | |
| ms523 | 0:aabbf2286bf2 | 20 | // the TFT is connected to SPI pin 5-7 |
| ms523 | 0:aabbf2286bf2 | 21 | SPI_TFT TFT(p5, p6, p7, p8, p9,"TFT"); // mosi, miso, sclk, cs, reset |
| ms523 | 0:aabbf2286bf2 | 22 | |
| ms523 | 0:aabbf2286bf2 | 23 | Serial pc(USBTX,USBRX); |
| ms523 | 0:aabbf2286bf2 | 24 | |
| ms523 | 0:aabbf2286bf2 | 25 | #define SIZEX (160) |
| ms523 | 0:aabbf2286bf2 | 26 | #define SIZEY (120) |
| ms523 | 0:aabbf2286bf2 | 27 | |
| ms523 | 0:aabbf2286bf2 | 28 | int main() { |
| ms523 | 0:aabbf2286bf2 | 29 | |
| ms523 | 0:aabbf2286bf2 | 30 | // Set up the TFT |
| ms523 | 0:aabbf2286bf2 | 31 | TFT.claim(stdout); // Send stdout to the TFT display |
| ms523 | 0:aabbf2286bf2 | 32 | TFT.background(Black); // Set background to black |
| ms523 | 0:aabbf2286bf2 | 33 | TFT.foreground(White); // Set chars to white |
| ms523 | 0:aabbf2286bf2 | 34 | TFT.cls(); // Clear the screen |
| ms523 | 0:aabbf2286bf2 | 35 | TFT.set_font((unsigned char*) Arial12x12); // Select the font |
| ms523 | 0:aabbf2286bf2 | 36 | TFT.set_orientation(3); // Select orientation |
| ms523 | 0:aabbf2286bf2 | 37 | |
| ms523 | 0:aabbf2286bf2 | 38 | printf("Testing..."); |
| ms523 | 0:aabbf2286bf2 | 39 | |
| ms523 | 0:aabbf2286bf2 | 40 | int i ; |
| ms523 | 0:aabbf2286bf2 | 41 | pc.baud(115200) ; |
| ms523 | 0:aabbf2286bf2 | 42 | pc.printf("Camera resetting..\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 43 | |
| ms523 | 0:aabbf2286bf2 | 44 | camera.Reset() ; |
| ms523 | 0:aabbf2286bf2 | 45 | |
| ms523 | 0:aabbf2286bf2 | 46 | pc.printf("Before Init...\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 47 | pc.printf("AD : +0 +1 +2 +3 +4 +5 +6 +7 +8 +9 +A +B +C +D +E +F") ; |
| ms523 | 0:aabbf2286bf2 | 48 | for (i=0; i<OV7670_REGMAX; i++) { |
| ms523 | 0:aabbf2286bf2 | 49 | int data ; |
| ms523 | 0:aabbf2286bf2 | 50 | data = camera.ReadReg(i) ; // READ REG |
| ms523 | 0:aabbf2286bf2 | 51 | if ((i & 0x0F) == 0) { |
| ms523 | 0:aabbf2286bf2 | 52 | pc.printf("\r\n%02X : ",i) ; |
| ms523 | 0:aabbf2286bf2 | 53 | } |
| ms523 | 0:aabbf2286bf2 | 54 | pc.printf("%02X ",data) ; |
| ms523 | 0:aabbf2286bf2 | 55 | } |
| ms523 | 0:aabbf2286bf2 | 56 | pc.printf("\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 57 | |
| ms523 | 0:aabbf2286bf2 | 58 | camera.InitQQVGA() ; |
| ms523 | 0:aabbf2286bf2 | 59 | |
| ms523 | 0:aabbf2286bf2 | 60 | pc.printf("After Init...\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 61 | pc.printf("AD : +0 +1 +2 +3 +4 +5 +6 +7 +8 +9 +A +B +C +D +E +F") ; |
| ms523 | 0:aabbf2286bf2 | 62 | for (i=0; i<OV7670_REGMAX; i++) { |
| ms523 | 0:aabbf2286bf2 | 63 | int data ; |
| ms523 | 0:aabbf2286bf2 | 64 | data = camera.ReadReg(i) ; // READ REG |
| ms523 | 0:aabbf2286bf2 | 65 | if ((i & 0x0F) == 0) { |
| ms523 | 0:aabbf2286bf2 | 66 | pc.printf("\r\n%02X : ",i) ; |
| ms523 | 0:aabbf2286bf2 | 67 | } |
| ms523 | 0:aabbf2286bf2 | 68 | pc.printf("%02X ",data) ; |
| ms523 | 0:aabbf2286bf2 | 69 | } |
| ms523 | 0:aabbf2286bf2 | 70 | pc.printf("\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 71 | |
| ms523 | 0:aabbf2286bf2 | 72 | // CAPTURE and SEND LOOP |
| ms523 | 0:aabbf2286bf2 | 73 | while (1) { |
| ms523 | 0:aabbf2286bf2 | 74 | pc.printf("Hit Any Key to send RGBx160x120 Capture Data.\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 75 | while (!pc.readable()) ; |
| ms523 | 0:aabbf2286bf2 | 76 | pc.getc() ; |
| ms523 | 0:aabbf2286bf2 | 77 | camera.CaptureNext() ; |
| ms523 | 0:aabbf2286bf2 | 78 | while (camera.CaptureDone() == false) ; |
| ms523 | 0:aabbf2286bf2 | 79 | pc.printf("*\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 80 | camera.ReadStart() ; |
| ms523 | 0:aabbf2286bf2 | 81 | i = 0 ; |
| ms523 | 0:aabbf2286bf2 | 82 | int colour; |
| ms523 | 0:aabbf2286bf2 | 83 | |
| ms523 | 0:aabbf2286bf2 | 84 | for (int y = 0; y < SIZEY; y++) { |
| ms523 | 0:aabbf2286bf2 | 85 | int r,g,b,d1,d2 ; |
| ms523 | 0:aabbf2286bf2 | 86 | for (int x = 0; x < SIZEX; x++) { |
| ms523 | 0:aabbf2286bf2 | 87 | d1 = camera.ReadOneByte() ; // upper nibble is XXX , lower nibble is B |
| ms523 | 0:aabbf2286bf2 | 88 | d2 = camera.ReadOneByte() ; // upper nibble is G , lower nibble is R |
| ms523 | 0:aabbf2286bf2 | 89 | b = (d1 & 0x0F) ; |
| ms523 | 0:aabbf2286bf2 | 90 | g = (d2 & 0xF0) >> 4 ; |
| ms523 | 0:aabbf2286bf2 | 91 | r = (d2 & 0x0F) ; |
| ms523 | 0:aabbf2286bf2 | 92 | pc.printf ("%1X%1X%1X",r,g,b) ; |
| ms523 | 0:aabbf2286bf2 | 93 | |
| ms523 | 0:aabbf2286bf2 | 94 | // Calculate colour |
| ms523 | 0:aabbf2286bf2 | 95 | colour = r << 11; |
| ms523 | 0:aabbf2286bf2 | 96 | colour += g << 5; |
| ms523 | 0:aabbf2286bf2 | 97 | colour += b; |
| ms523 | 0:aabbf2286bf2 | 98 | |
| ms523 | 0:aabbf2286bf2 | 99 | // Display pixel on TFT screen |
| ms523 | 0:aabbf2286bf2 | 100 | TFT.pixel(x, y, colour); |
| ms523 | 0:aabbf2286bf2 | 101 | } |
| ms523 | 0:aabbf2286bf2 | 102 | pc.printf("\r\n") ; |
| ms523 | 0:aabbf2286bf2 | 103 | } |
| ms523 | 0:aabbf2286bf2 | 104 | camera.ReadStop() ; |
| ms523 | 0:aabbf2286bf2 | 105 | } |
| ms523 | 0:aabbf2286bf2 | 106 | } |