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
Revision 3:9244869157bd, committed 2017-10-03
- Comitter:
- rulla
- Date:
- Tue Oct 03 16:47:15 2017 +0000
- Parent:
- 2:4ebec58ffaca
- Commit message:
- 3D point cloud scanner with OV7670 camera.
Changed in this revision
diff -r 4ebec58ffaca -r 9244869157bd ADXL345_I2C.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ADXL345_I2C.lib Tue Oct 03 16:47:15 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/rulla/code/ADXL345_I2C_/#42b8b44580ec
diff -r 4ebec58ffaca -r 9244869157bd HMC5843.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC5843.lib Tue Oct 03 16:47:15 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/rulla/code/HMC5843_/#7f74d68580c3
diff -r 4ebec58ffaca -r 9244869157bd OV7670.lib --- a/OV7670.lib Wed Nov 16 10:38:37 2016 +0000 +++ b/OV7670.lib Tue Oct 03 16:47:15 2017 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/users/rulla/code/OV7670/#101fbacf126f +https://os.mbed.com/users/rulla/code/OV7670_ok/#f5f5d7c979cf
diff -r 4ebec58ffaca -r 9244869157bd SDFileSystem.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Tue Oct 03 16:47:15 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/SDFileSystem/#8db0d3b02cec
diff -r 4ebec58ffaca -r 9244869157bd Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Tue Oct 03 16:47:15 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 4ebec58ffaca -r 9244869157bd main.cpp
--- a/main.cpp Wed Nov 16 10:38:37 2016 +0000
+++ b/main.cpp Tue Oct 03 16:47:15 2017 +0000
@@ -1,111 +1,450 @@
-//
-// OV7670 + FIFO AL422B camera board test
-//
+//3D Scanner
+//2017 Angelo Mottolese mottoleseangelo@gmail.com
#include "mbed.h"
#include "OV7670.h"
- #include "ST7735_TFT.h"
+#include "ST7735_TFT.h"
+#include "ADXL345_I2C.h"
+#include "HMC5843.h"
+#include "SDFileSystem.h"
+#include "Servo.h"
+
+#define SIZEX 320
+#define SIZEY 240
+#define SIZE SIZEX*SIZEY
+#define LABVIEW_MODE 1
+
+
+#define TRANSFER_SIZE 4
+#define N_VAR 14
+
+//LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+
OV7670 cam(
p28,p27, // SDA,SCL(I2C / SCCB)
p23,NC,p25, // VSYNC,HREF,WEN(FIFO)
- Port0,0x07878000,
+ Port0,0x07878000,//PortIn(p18,p17,p16,p15,p11,p12,p14,p13) is D7-D0
p26,p29,p30) ; // RRST,OE,RCLK
+ST7735_TFT TFT(p5, p6, p7, p8, p9, p10,"TFT"); // mosi, miso, sclk, cs, rs, reset
+// SDA, nc, SCK,CS,A0,RESET = mosi, miso, sclk, cs, rs, reset
+
+HMC5843 compass(p28, p27);
+ADXL345_I2C accelerometer(p28, p27);
+//SDFileSystem sd(p5, p6, p7, p21, "sd"); // the pinout on the mbed Cool Components workshop board
+//nRF24L01P my_nrf24l01p(p5, p6, p7, p24, p21, p20); // mosi, miso, sck, csn, ce, irq
+Servo yaw(p21);
+Serial pc(USBTX,USBRX);
+Timer tempo;
+AnalogIn pot(p20);
+
+float Complementary(float newAngle, int looptime);
+float Complementary2(float newAngle, int looptime);
+double kalman_update(int index,double measurement);
+float kalmanCalculate(float newAngle, int looptime);
+void accMagInit(void);
+void ahrs(void);
+void kalman_init(void);
+void showImage(void);
+void image_minmax(void);
+void processImage(void);
+void sdInit(void);
+void nrfpInit(void);
+void countdown(void);
+
+extern "C" void mbed_reset();
+
+double q[N_VAR]; //process noise covariance
+double rk[N_VAR]; //measurement noise covariance
+double value[N_VAR]; //value
+double p[N_VAR]; //estimation error covariance
+double k[N_VAR]; //kalman gain
+
+int i=0, loopN=0, l=0;
+float mv=0.0,servoHead=0.0;
- ST7735_TFT TFT(p5, p6, p7, p8, p9, p10,"TFT"); // mosi, miso, sclk, cs, rs, reset
-
-Serial pc(USBTX,USBRX);
+int thr=5;
+
+float xrot=0.0,yrot=0.0,zrot=0.0, vx=0.0,vy=0.0,vz=0.0;
+int ax=0,ay=0,az=0,ax0,ay0,az0;
+int mx=0,my=0,mz=0,mx0,my0,mz0;
+double xh=0.0, yh=0.0, head=0.0;
+double ca=0.0, cb=0.0, cc=0.0;
+double sa=0.0, sb=0.0,sc=0.0;
+ double fx,fy,fz;
+uint16_t thri=3,thrr=3,thrg=4;
+
+uint16_t max_red,max_blue, max_green,min_blue,min_green,min_red;
+uint16_t colour=0,r,g,b;
+int x=0,y=0, lineacc,cnt=0,x0[SIZEY];
+uint16_t bank0,bank1;
+int ok=0;
+ uint16_t mr=0,mb=0,mg=0,id=0;
+
+// FILE *out=fopen("/local/world.txt","w");
+
+int main()
+{
+ // if ( out == NULL ) error("Could not open file.\r\n");
+ //fprintf(out,"v 0.0 0.0 0.0\n");
+ pc.baud(115200);
+ yaw=0.0;
+ //////////////////////Camera and TFT init
+ TFT.set_orientation(3);
+ //TFT.background(Black);
+ TFT.cls();
+ cam.Reset() ;
+ cam.InitQQVGA(SIZEX) ;
+
+ pc.baud(115200);
+ kalman_init();
+ accMagInit();
+//nrfpInit();
+//sdInit();
+
+ /////////////////////////////////// Loop control////////////////////////////////////////////////////
+ int j=0;
-#define SIZEX (160)
-#define SIZEY (120)
-#define SIZE SIZEX*SIZEY
+ for(j=1; j>0; j--) {
+ if(LABVIEW_MODE==0) pc.printf("Posizionamento %d s\n", j);
+ tempo.start();
+ while(tempo.read()<0.5) showImage();
+ tempo.stop();
+ tempo.reset();
+ }
+ //countdown();
+
+ image_minmax();
+
+ // while(1) {
+ //tempo.start();
+ ahrs();
+ ///////////Image Acquisition and Processing
+ // if((hd[0]+0.22)<hd[1] || (hd[0]-0.22)>hd[1]){ //Heading noise gate
+ ok=0;
+
+ //if(loopN%20==0)
+ TFT.cls();
+ //Misura intensità max e min
+ if(loopN%100==0 || loopN==0) image_minmax(); //Se il numero di pixel validi è piccolo
-//unsigned char bank0[SIZE];
-//unsigned char *bank1 = (unsigned char *)(0x2007C000);
-unsigned char bank0,bank1,bank2,bank3;
+ // printf(" SCANNING.\n");
+ while(1) {
+
+ ahrs();
+ image_minmax();
+ processImage();
+ loopN++;
+ yaw=kalman_update(12,mv);
+ servoHead=mv*1.5768863175;
+ // wait_ms(20);
+ }
+ /* for(mv=1.0; mv>0.0; mv-=0.01) {
+
+ ahrs();
+ image_minmax();
+ processImage();
+ loopN++;
+ yaw=kalman_update(12,mv);
+ servoHead=mv*1.5768863175;
+ wait_ms(20);
+ }
+ yaw=kalman_update(12,0.0);*/
+ //fclose(out);
+ // printf(" END\n");
+ // tempo.stop();
+ // printf("Time taken= %f seconds, OK=%d\n", tempo.read(),ok);
+ // tempo.reset();
+ // }//Fine while(1)
+ // mbed_reset();
+}
-int main() {
+
+double kalman_update(int index,double measurement)
+{
+ //prediction update
+ //omit x = x
+ p[index]+= q[index];
+ //measurement update
+ k[index] = p[index] / (p[index] + rk[index]);
+ value[index] += k[index] * (measurement - value[index]);
+ p[index] *= (1 - k[index]);
+ //fprintf(out,"%f %f \r\n",p,k);
+ return value[index];
+}
-pc.baud(115200);
+void kalman_init(void)
+{
+ for(l=0; l<N_VAR; l++) {
+ q[l]=0.001; //process noise covariance
+ rk[l]=0.1; //measurement noise covariance
+ p[l]=0.0; //estimation error covariance
+ k[l]=0.0; //kalman gain
+ value[l]=0.0;
+ }
+}
- // Set up the TFT
+///////////////////////////////////////////SD Card init
+void sdInit(void)
+{
/*
- TFT.claim(stdout); // Send stdout to the TFT display
- TFT.background(Black); // Set background to black
- TFT.foreground(White); // Set chars to white
- TFT.cls(); // Clear the screen
- TFT.set_orientation(3); // Select orientation
-*/
- TFT.set_orientation(3);
-
- cam.Reset() ;
- pc.printf("\r\nDefault\r\n");
- int i;
- char data1[OV7670_REGMAX],data2[OV7670_REGMAX];
- for (i=0;i<OV7670_REGMAX;i++)
- {
- data1[i]=cam.ReadReg(i); // READ REG
- //if ((i & 0x0F) == 0) pc.printf("\r\n%02X : ",i);
- //pc.printf("Add %02X = %02X ,",i,data);
- }
- cam.InitQQVGA() ;
- pc.printf("\r\nDefault\r\n");
- for (i=0;i<OV7670_REGMAX;i++)
- {
- data2[i]=cam.ReadReg(i); // READ REG
- // if ((i & 0x0F) == 0) pc.printf("\r\n%02X : ",i);
- if(data1[i]!=data2[i]) pc.printf("Add %02X = %02X/%02X ,",i,data1[i],data2[i]);
- }
- int j=0;
+ mkdir("/sd/", 0777);
+ FILE *fp = fopen("/sd/worldSD.obj", "w");
+ if(fp == NULL) error("Could not open file for write\n");
+ fprintf(fp, "World mapping");
+ fclose(fp);
+ */
+}
+
+////////////////////////////AHRS//////////////////
+void accMagInit(void)
+{
+
+ int breadings[3] = {0, 0, 0};
+ int bhmcreadings[3];
+ char buffer[3];
+ // if(LABVIEW_MODE==0) pc.printf("Starting ADXL345 test...\n");
+ // if(LABVIEW_MODE==0) pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
+ accelerometer.setPowerControl(0x00);
+ accelerometer.setDataFormatControl(0x0B);
+ accelerometer.setDataRate(ADXL345_3200HZ);
+ accelerometer.setPowerControl(0x08);
+ compass.getAddress(buffer);
+ // if(LABVIEW_MODE==0) pc.printf("Magnetic Compass Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
+ compass.setDefault();
+ wait(.1);
+ for(i=0; i<50; i++) {
+ accelerometer.getOutput(breadings);
+ //13-bit, sign extended values.
+ compass.readData(bhmcreadings);
+ wait(0.1);
+ ax+=(int16_t)(breadings[0]);
+ ay+=(int16_t)(breadings[1]);
+ az+=(int16_t)(breadings[2]);
+ mx+=(int16_t)(bhmcreadings[0]);
+ my+=(int16_t)(bhmcreadings[1]);
+ mz+=(int16_t)(bhmcreadings[2]);
+ }
+ ax0=ax/50;
+ ay0=ay/50;
+ az0=az/50;
+ mx0=mx/50;
+ my0=my/50;
+ mz0=mz/50;
+ ax=0;
+ ay=0;
+ az=0;
+ mx=0;
+ my=0;
+ mz=0;
+}
+
+void ahrs(void)
+{
+ cnt=0;
+ int accelero=0, deadr=0;
+
+ if(accelero==1) {
+
+float x1=0.0,y1=0.0,z1=0.0,x2=0.0,y2=0.0,z2=0.0,xd=0.0,yd=0.0,zd=0.0;
+float aax=0.0,aay=0.0,aaz=0.0;
+ int readings[3] = {0, 0, 0};
+ int hmcreadings[3];
+ accelerometer.getOutput(readings);
+ compass.readData(hmcreadings);
+ //13-bit, sign extended values.
+ cnt++;
+ ax+=(int16_t)(readings[0]);
+ ay+=(int16_t)(readings[1]);
+ az+=(int16_t)(readings[2]);
+ ax-=ax0;
+ ay-=ay0;
+ az-=az0;
+ mx+=(int16_t)(hmcreadings[0]);
+ my+=(int16_t)(hmcreadings[1]);
+ mz+=(int16_t)(hmcreadings[2]);
+ mx-=mx0;
+ my-=my0;
+ mz-=mz0;
+ wait(0.1);
+
+ xd=(double)(ax);
+ yd=(double)(ay);
+ zd=(double)(az);
+ aax=kalman_update(9,xd); //Kalman to accelerometer
+ aay=kalman_update(10,yd);
+ aaz=kalman_update(11,-zd);
+
+ if(deadr==1) {
+ x1+=kalman_update(3,aax);
+ y1+= kalman_update(4,aay);
+ z1+= kalman_update(5,aaz);
+
+ if(aax<=1.6420 && aax>=-1.6420 ) x1=0.0;
+ if(aay<=1.2315 && aay>=-1.1325 ) y1=0.0;
+ if(aaz<=8.374 && aaz>=-8.374 ) z1=0.0;
+ x2+=kalman_update(6,x1);
+ y2+=kalman_update(7,y1);
+ z2+=kalman_update(8,z1);
+ }
+ double xAngle=-0.00609*aay;
+ double yAngle=0.00609*aax;
+
+ double mmx=(double)(mx);
+ double mmy=(double)(my);
+ double mmz=(double)(mz);
+
+ fx=kalman_update(0,xAngle);
+ fy=kalman_update(1,yAngle);
+
+ ca = cos(fx);//theta
+ cb = cos(fy);//phi
+ sa = sin(fx);
+ sb = sin(fy);
+
+ double yh=mmz*sb-mmy*cb;
+ double xh=+mmx*ca+mmy*sa*sb+mmz*sa*cb;
+ double head=atan2(yh,xh);
+ fz=kalman_update(2,head);
+ }
+
+
+ //if(accelero==0) {
+ fz=servoHead;
+fz=pot.read();
+ fz-=0.1;
+ fz*=6.2831168863175;
+ fz=3.14-fz;
- //while(j<15){
- while(1){
- // Kick things off by capturing an image
+ cc = cos(fz);
+ sc = sin(fz);
+ //if(LABVIEW_MODE==0) pc.printf("%f %f %f %f %f %f %f %f %f\n",fx,fy,fz,x2,y2,z2,kalman_update(9,x2),kalman_update(10,y2),kalman_update(11,z2));
+ cnt=0;
+ ax=0;
+ ay=0;
+ az=0;
+
+
+}
+
+/////////////////////CAMERA FUNCTIONS/////////////////////
+void image_minmax(void)
+{
+ for(id=0;id<SIZEY;id++) x0[id]=0;
+ max_red=0;max_blue=0; max_green=0;min_blue=200;min_green=200;min_red=200;
+
cam.CaptureNext() ;
while (cam.CaptureDone()==0) ;
+ cam.ReadStart();
+
+ for (y = 0; y < SIZEY; y++) {
+ for ( x= 0; x < SIZEX; x++) {
+ bank0 = cam.ReadOneByte();
+ bank1 = cam.ReadOneByte();
+ r=((bank1 & 0x1f)) ;
+ g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
+ b=((bank0 & 0xf8)>>3 );
- pc.printf("Done %d\n",cam.CaptureDone());
- int x=0,y=0, r=0,g=0,b=0,colour;
- char rr,gg,bb;
- // Start reading in the image data from the camera hardware buffer
- cam.ReadStart();
-
- for (int i = 0; i < SIZE; i++) {
-//if(i==0) cam.ReadOneByte();
+ /* if(b>max_blue) {
+ max_blue=b;
+ if(max_green<g) max_green=g;
+ if(max_red<r) max_red=r;
+ if(min_blue>b) min_blue=b;
+ if(min_green>g) min_green=g;
+ if(min_red>r) min_red=r;
+ mr+=r;
+ mb+=b;
+ mg+=g;
+ id++;
+ }*/
+ if(r>max_red) {
+ max_red=r;
+ if(max_green<g) max_green=g;
+ if(max_blue<b) max_blue=b;
+ if(min_blue>b) min_blue=b;
+ if(min_green>g) min_green=g;
+ if(min_red>r) min_red=r;
+ mr+=r;
+ mb+=b;
+ mg+=g;
+ id++;
+ }
+ }
+ }
+ cam.ReadStop() ;
+}
+
+
- bank0 = cam.ReadOneByte();
- bank1 = cam.ReadOneByte();
- //bank1=0;
-//RGB
-
- rr=(bank1 & 0x1f) ;
- gg= ((bank0<<3) & 0x38) | ((bank1>>5)&0x07);
- b=(bank0 & 0xf8)>>3 ;
- //444
- /*
- b = (bank0 & 0x0F) ;
- g = (bank1 & 0xF0) >> 4 ;
- r = (bank1 & 0x0F) ;
- */
- colour = ((r<<11)+(g<<5)+b);
- if(i%200==0) pc.printf("RGB %d %d %d, B0 %d, B1 %d\n",rr,gg,b,bank0,bank1);
- // TFT.pixel(x,y,colour);
- TFT.pixel(x,y,rr);
-
- x++;
- if(i%SIZEX==0) x=0,y++;
- }
+void processImage(void)
+{
+ cnt=0;
+ double xrot,yrot,zrot;
+ cam.CaptureNext() ;
+ while (cam.CaptureDone()==0) ;
+ cam.ReadStart();
+ for (y = 0; y < SIZEY; y++) {
+ for ( x= 0; x < SIZEX; x++) {
+ bank0 = cam.ReadOneByte();
+ bank1 = cam.ReadOneByte();
+ r=((bank1 & 0x1f)) ;
+ g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
+ b=((bank0 & 0xf8)>>3 );
+ //if(b>=(mb-thri) && r<=(mr+thrr) ) {
+ if(r>=(max_red-thrr) && b>=(max_blue-thri)) {
+ lineacc+=x;
+ cnt++;
+ }
+ }
+ if(cnt>1) {
+ x0[y]=lineacc/cnt;
- cam.ReadStop() ;
- //TFT.Bitmap(0,0,160,60,bank1);
- //TFT.Bitmap(0,60,160,60,bank0);
- x=0;y=0;
- /*
- for (int i = 0; i <= SIZE; i++) {
- if(i==SIZEX) x=0,y++;
- r=(bank0[i]&0xF8)>>3;
- // TFT.pixel(x,y,bank0[i]);
- }
- */
-//j++;
+ ok++;
+ }
+ lineacc=0;
+ cnt=0;
+ }
+ cam.ReadStop() ;
+
+ for(y=0;y<SIZEY;y++){
+ vy=(double)(SIZEX)-(double)x0[y];//vy*sc
+ vx=(double)(SIZEX)-(double)(SIZEX)*cc;//vy*cc
+ vx=vy*cc;//vy*cc
+ yrot=vy*sc;
+ zrot=y*0.6;
+ TFT.pixel(x0[y],y,x0[y]*10);
+
+// if(LABVIEW_MODE==0) pc.printf("v %f. %f. %f\n",xrot,yrot,zrot);
+ if(LABVIEW_MODE==0) pc.printf("%f %f %f %f ",vx,vy,vz,fz);
+ if(LABVIEW_MODE==1) pc.printf("%.3f,%.3f,%.3f\n",vx,yrot,zrot);
+ }
}
+
+
+void showImage(void)
+{
+ cam.CaptureNext() ;
+ while (cam.CaptureDone()==0) ;
+ cam.ReadStart();
+ for (y = 0; y < SIZEY; y++) {
+ for ( x= 0; x < SIZEX; x++) {
+ bank0 = cam.ReadOneByte();
+ bank1 = cam.ReadOneByte();
+ r=((bank1 & 0x1f)) ;
+ g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
+ b=((bank0 & 0xf8)>>3 );
+ colour = (((r<<11)&0xF800)|((g<<5)&0x7E0)| (b&0x1f));
+ TFT.pixel(x,y,colour);
+ }
+ }
+ cam.ReadStop() ;
+}
+
+void countdown(void)
+{
+ int i;
+ for(i=3; i>0; i--) {
+ showImage();
+ wait(1);
+ if(LABVIEW_MODE==0) pc.printf("Acquisisco fra %d s\n",i);
+ }
}
\ No newline at end of file
diff -r 4ebec58ffaca -r 9244869157bd mbed.bld --- a/mbed.bld Wed Nov 16 10:38:37 2016 +0000 +++ b/mbed.bld Tue Oct 03 16:47:15 2017 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/14f4805c468c +http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90 \ No newline at end of file