main.cpp

00001 //3D Scanner
00002 //2017 Angelo Mottolese mottoleseangelo@gmail.com
00003 #include "mbed.h"
00004 #include "OV7670.h"
00005 #include "ST7735_TFT.h"
00006 #include "ADXL345_I2C.h"
00007 #include "HMC5843.h"
00008 #include "SDFileSystem.h"
00009 #include "Servo.h"
00010 
00011 #define SIZEX 320
00012 #define SIZEY 240
00013 #define SIZE SIZEX*SIZEY
00014 #define LABVIEW_MODE 1
00015 
00016 
00017 #define TRANSFER_SIZE   4
00018 #define N_VAR 14
00019 
00020 //LocalFileSystem local("local");               // Create the local filesystem under the name "local"
00021 
00022 
00023 OV7670 cam(
00024     p28,p27,       // SDA,SCL(I2C / SCCB)
00025     p23,NC,p25,   // VSYNC,HREF,WEN(FIFO)
00026     Port0,0x07878000,//PortIn(p18,p17,p16,p15,p11,p12,p14,p13) is D7-D0
00027     p26,p29,p30) ; // RRST,OE,RCLK
00028 ST7735_TFT TFT(p5, p6, p7, p8, p9, p10,"TFT"); // mosi, miso, sclk, cs, rs, reset
00029 // SDA, nc, SCK,CS,A0,RESET = mosi, miso, sclk, cs, rs, reset
00030 
00031 HMC5843 compass(p28, p27);
00032 ADXL345_I2C accelerometer(p28, p27);
00033 //SDFileSystem sd(p5, p6, p7, p21, "sd"); // the pinout on the mbed Cool Components workshop board
00034 //nRF24L01P my_nrf24l01p(p5, p6, p7, p24, p21, p20);    // mosi, miso, sck, csn, ce, irq
00035 Servo yaw(p21);
00036 Serial pc(USBTX,USBRX);
00037 Timer tempo;
00038 AnalogIn pot(p20);
00039 
00040 float Complementary(float newAngle, int looptime);
00041 float Complementary2(float newAngle, int looptime);
00042 double kalman_update(int index,double measurement);
00043 float kalmanCalculate(float newAngle, int looptime);
00044 void accMagInit(void);
00045 void ahrs(void);
00046 void kalman_init(void);
00047 void showImage(void);
00048 void image_minmax(void);
00049 void processImage(void);
00050 void sdInit(void);
00051 void nrfpInit(void);
00052 void countdown(void);
00053 
00054 extern "C" void mbed_reset();
00055 
00056 double q[N_VAR]; //process noise covariance
00057 double rk[N_VAR]; //measurement noise covariance
00058 double value[N_VAR]; //value
00059 double p[N_VAR]; //estimation error covariance
00060 double k[N_VAR]; //kalman gain
00061 
00062 int i=0, loopN=0, l=0;
00063 float mv=0.0,servoHead=0.0;
00064 
00065 int thr=5;
00066 
00067 float xrot=0.0,yrot=0.0,zrot=0.0, vx=0.0,vy=0.0,vz=0.0;
00068 int ax=0,ay=0,az=0,ax0,ay0,az0;
00069 int mx=0,my=0,mz=0,mx0,my0,mz0;
00070 double xh=0.0, yh=0.0, head=0.0;
00071 double ca=0.0, cb=0.0, cc=0.0;
00072 double sa=0.0, sb=0.0,sc=0.0;
00073     double fx,fy,fz;
00074 uint16_t thri=3,thrr=3,thrg=4;
00075 
00076 uint16_t max_red,max_blue, max_green,min_blue,min_green,min_red;
00077 uint16_t colour=0,r,g,b;
00078 int x=0,y=0, lineacc,cnt=0,x0[SIZEY];
00079 uint16_t bank0,bank1;
00080 int ok=0;
00081     uint16_t mr=0,mb=0,mg=0,id=0;
00082 
00083 //  FILE *out=fopen("/local/world.txt","w");
00084 
00085 int main()
00086 {
00087     //  if ( out == NULL )   error("Could not open file.\r\n");
00088     //fprintf(out,"v 0.0 0.0 0.0\n");
00089     pc.baud(115200);
00090         yaw=0.0;
00091     //////////////////////Camera and TFT init
00092     TFT.set_orientation(3);
00093     //TFT.background(Black);
00094     TFT.cls();
00095     cam.Reset() ;
00096     cam.InitQQVGA(SIZEX) ;
00097 
00098     pc.baud(115200);
00099     kalman_init();
00100     accMagInit();
00101 //nrfpInit();
00102 //sdInit();
00103 
00104     /////////////////////////////////// Loop control////////////////////////////////////////////////////
00105     int j=0;
00106 
00107     for(j=1; j>0; j--) {
00108        if(LABVIEW_MODE==0) pc.printf("Posizionamento %d s\n", j);
00109         tempo.start();
00110         while(tempo.read()<0.5) showImage();
00111         tempo.stop();
00112         tempo.reset();
00113     }
00114     //countdown();
00115 
00116     image_minmax();
00117 
00118     // while(1) {
00119     //tempo.start();
00120     ahrs();
00121     ///////////Image Acquisition and Processing
00122     //      if((hd[0]+0.22)<hd[1] || (hd[0]-0.22)>hd[1]){   //Heading noise gate
00123     ok=0;
00124     
00125     //if(loopN%20==0) 
00126     TFT.cls();
00127     //Misura intensità max e min
00128     if(loopN%100==0 || loopN==0) image_minmax(); //Se il numero di pixel validi è piccolo
00129 
00130    // printf("    SCANNING.\n");
00131    while(1) {
00132    
00133         ahrs();
00134         image_minmax();
00135         processImage();
00136         loopN++;
00137             yaw=kalman_update(12,mv);
00138         servoHead=mv*1.5768863175;
00139            //   wait_ms(20);
00140     }
00141    /* for(mv=1.0; mv>0.0; mv-=0.01) {
00142         
00143         ahrs();
00144         image_minmax();
00145         processImage();
00146         loopN++;
00147             yaw=kalman_update(12,mv);
00148         servoHead=mv*1.5768863175;
00149         wait_ms(20);
00150     }
00151     yaw=kalman_update(12,0.0);*/
00152     //fclose(out);
00153   //  printf("            END\n");
00154  //   tempo.stop();
00155   //  printf("Time taken= %f seconds, OK=%d\n", tempo.read(),ok);
00156  //   tempo.reset();
00157     // }//Fine while(1)
00158     //     mbed_reset();
00159 }
00160 
00161 
00162 double kalman_update(int index,double measurement)
00163 {
00164     //prediction update
00165     //omit x = x
00166     p[index]+= q[index];
00167     //measurement update
00168     k[index] = p[index] / (p[index] + rk[index]);
00169     value[index] += k[index] * (measurement - value[index]);
00170     p[index] *= (1 - k[index]);
00171     //fprintf(out,"%f %f \r\n",p,k);
00172     return value[index];
00173 }
00174 
00175 void kalman_init(void)
00176 {
00177     for(l=0; l<N_VAR; l++) {
00178         q[l]=0.001; //process noise covariance
00179         rk[l]=0.1; //measurement noise covariance
00180         p[l]=0.0; //estimation error covariance
00181         k[l]=0.0; //kalman gain
00182         value[l]=0.0;
00183     }
00184 }
00185 
00186 ///////////////////////////////////////////SD Card init
00187 void sdInit(void)
00188 {
00189     /*
00190        mkdir("/sd/", 0777);
00191        FILE *fp = fopen("/sd/worldSD.obj", "w");
00192        if(fp == NULL) error("Could not open file for write\n");
00193        fprintf(fp, "World mapping");
00194        fclose(fp);
00195     */
00196 }
00197 
00198 ////////////////////////////AHRS//////////////////
00199 void accMagInit(void)
00200 {
00201 
00202     int breadings[3] = {0, 0, 0};
00203     int bhmcreadings[3];
00204     char buffer[3];
00205    // if(LABVIEW_MODE==0) pc.printf("Starting ADXL345 test...\n");
00206   //  if(LABVIEW_MODE==0) pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
00207     accelerometer.setPowerControl(0x00);
00208     accelerometer.setDataFormatControl(0x0B);
00209     accelerometer.setDataRate(ADXL345_3200HZ);
00210     accelerometer.setPowerControl(0x08);
00211     compass.getAddress(buffer);
00212   //  if(LABVIEW_MODE==0) pc.printf("Magnetic Compass  Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
00213     compass.setDefault();
00214     wait(.1);
00215     for(i=0; i<50; i++) {
00216         accelerometer.getOutput(breadings);
00217         //13-bit, sign extended values.
00218         compass.readData(bhmcreadings);
00219         wait(0.1);
00220         ax+=(int16_t)(breadings[0]);
00221         ay+=(int16_t)(breadings[1]);
00222         az+=(int16_t)(breadings[2]);
00223         mx+=(int16_t)(bhmcreadings[0]);
00224         my+=(int16_t)(bhmcreadings[1]);
00225         mz+=(int16_t)(bhmcreadings[2]);
00226     }
00227     ax0=ax/50;
00228     ay0=ay/50;
00229     az0=az/50;
00230     mx0=mx/50;
00231     my0=my/50;
00232     mz0=mz/50;
00233     ax=0;
00234     ay=0;
00235     az=0;
00236     mx=0;
00237     my=0;
00238     mz=0;
00239 }
00240 
00241 void ahrs(void)
00242 {
00243     cnt=0;
00244     int accelero=0, deadr=0;
00245 
00246     if(accelero==1) {
00247 
00248 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;
00249 float aax=0.0,aay=0.0,aaz=0.0;
00250         int readings[3] = {0, 0, 0};
00251         int hmcreadings[3];
00252         accelerometer.getOutput(readings);
00253         compass.readData(hmcreadings);
00254         //13-bit, sign extended values.
00255         cnt++;
00256         ax+=(int16_t)(readings[0]);
00257         ay+=(int16_t)(readings[1]);
00258         az+=(int16_t)(readings[2]);
00259         ax-=ax0;
00260         ay-=ay0;
00261         az-=az0;
00262         mx+=(int16_t)(hmcreadings[0]);
00263         my+=(int16_t)(hmcreadings[1]);
00264         mz+=(int16_t)(hmcreadings[2]);
00265         mx-=mx0;
00266         my-=my0;
00267         mz-=mz0;
00268         wait(0.1);
00269 
00270         xd=(double)(ax);
00271         yd=(double)(ay);
00272         zd=(double)(az);
00273         aax=kalman_update(9,xd); //Kalman to accelerometer
00274         aay=kalman_update(10,yd);
00275         aaz=kalman_update(11,-zd);
00276 
00277         if(deadr==1) {
00278             x1+=kalman_update(3,aax);
00279             y1+= kalman_update(4,aay);
00280             z1+= kalman_update(5,aaz);
00281 
00282             if(aax<=1.6420 && aax>=-1.6420 ) x1=0.0;
00283             if(aay<=1.2315 && aay>=-1.1325 ) y1=0.0;
00284             if(aaz<=8.374 && aaz>=-8.374 ) z1=0.0;
00285             x2+=kalman_update(6,x1);
00286             y2+=kalman_update(7,y1);
00287             z2+=kalman_update(8,z1);
00288         }
00289         double xAngle=-0.00609*aay;
00290         double yAngle=0.00609*aax;
00291 
00292         double mmx=(double)(mx);
00293         double mmy=(double)(my);
00294         double mmz=(double)(mz);
00295 
00296         fx=kalman_update(0,xAngle);
00297         fy=kalman_update(1,yAngle);
00298 
00299         ca = cos(fx);//theta
00300         cb = cos(fy);//phi
00301         sa = sin(fx);
00302         sb = sin(fy);
00303 
00304         double yh=mmz*sb-mmy*cb;
00305         double xh=+mmx*ca+mmy*sa*sb+mmz*sa*cb;
00306         double head=atan2(yh,xh);
00307         fz=kalman_update(2,head);
00308     }
00309 
00310 
00311     //if(accelero==0) {
00312     fz=servoHead;
00313 fz=pot.read();
00314         fz-=0.1;
00315         fz*=6.2831168863175;
00316         fz=3.14-fz;
00317         
00318     cc = cos(fz);
00319     sc = sin(fz);
00320     //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));
00321     cnt=0;
00322     ax=0;
00323     ay=0;
00324     az=0;
00325 
00326 
00327 }
00328 
00329 /////////////////////CAMERA FUNCTIONS/////////////////////
00330 void image_minmax(void)
00331 {
00332     for(id=0;id<SIZEY;id++) x0[id]=0;
00333     max_red=0;max_blue=0; max_green=0;min_blue=200;min_green=200;min_red=200;
00334     
00335     cam.CaptureNext() ;
00336     while (cam.CaptureDone()==0) ;
00337     cam.ReadStart();
00338     
00339     for (y = 0; y < SIZEY; y++) {
00340         for ( x= 0; x < SIZEX; x++) {
00341             bank0 =  cam.ReadOneByte();
00342             bank1 =  cam.ReadOneByte();
00343             r=((bank1 & 0x1f)) ;
00344            g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
00345             b=((bank0 & 0xf8)>>3 );
00346 
00347            /* if(b>max_blue) {
00348                 max_blue=b;
00349                 if(max_green<g) max_green=g;
00350                 if(max_red<r) max_red=r;
00351                 if(min_blue>b) min_blue=b;
00352                 if(min_green>g) min_green=g;
00353                 if(min_red>r) min_red=r;
00354                 mr+=r;
00355                 mb+=b;
00356                 mg+=g;
00357                 id++;
00358             }*/    
00359             if(r>max_red) {
00360                 max_red=r;
00361                 if(max_green<g) max_green=g;
00362                 if(max_blue<b) max_blue=b;
00363                 if(min_blue>b) min_blue=b;
00364                 if(min_green>g) min_green=g;
00365                 if(min_red>r) min_red=r;
00366                 mr+=r;
00367                 mb+=b;
00368                 mg+=g;
00369                 id++;
00370             }
00371         }
00372     }
00373     cam.ReadStop() ;
00374 }
00375 
00376 
00377 
00378 void processImage(void)
00379 {
00380     cnt=0;
00381     double xrot,yrot,zrot;
00382     cam.CaptureNext() ;
00383     while (cam.CaptureDone()==0) ;
00384     cam.ReadStart();
00385     for (y = 0; y < SIZEY; y++) {
00386         for ( x= 0; x < SIZEX; x++) {
00387             bank0 =  cam.ReadOneByte();
00388             bank1 =  cam.ReadOneByte();
00389             r=((bank1 & 0x1f)) ;
00390            g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
00391             b=((bank0 & 0xf8)>>3 );
00392            //if(b>=(mb-thri) && r<=(mr+thrr) )  {
00393         if(r>=(max_red-thrr) && b>=(max_blue-thri))  {
00394                 lineacc+=x;
00395                 cnt++;
00396             }
00397         }
00398         if(cnt>1) {
00399             x0[y]=lineacc/cnt;
00400         
00401             ok++;
00402         }
00403         lineacc=0;
00404         cnt=0;
00405     }
00406     cam.ReadStop() ;
00407     
00408     for(y=0;y<SIZEY;y++){
00409             vy=(double)(SIZEX)-(double)x0[y];//vy*sc
00410         vx=(double)(SIZEX)-(double)(SIZEX)*cc;//vy*cc
00411         vx=vy*cc;//vy*cc
00412         yrot=vy*sc;
00413         zrot=y*0.6;
00414             TFT.pixel(x0[y],y,x0[y]*10);
00415             
00416 //           if(LABVIEW_MODE==0)  pc.printf("v %f. %f. %f\n",xrot,yrot,zrot);
00417            if(LABVIEW_MODE==0)  pc.printf("%f %f %f %f ",vx,vy,vz,fz);
00418             if(LABVIEW_MODE==1) pc.printf("%.3f,%.3f,%.3f\n",vx,yrot,zrot);
00419         }
00420 }
00421 
00422 
00423 void showImage(void)
00424 {
00425     cam.CaptureNext() ;
00426     while (cam.CaptureDone()==0) ;
00427     cam.ReadStart();
00428     for (y = 0; y < SIZEY; y++) {
00429         for ( x= 0; x < SIZEX; x++) {
00430             bank0 =  cam.ReadOneByte();
00431             bank1 =  cam.ReadOneByte();
00432             r=((bank1 & 0x1f)) ;
00433            g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
00434             b=((bank0 & 0xf8)>>3 );
00435             colour = (((r<<11)&0xF800)|((g<<5)&0x7E0)| (b&0x1f));
00436             TFT.pixel(x,y,colour);
00437         }
00438     }
00439     cam.ReadStop() ;
00440 }
00441 
00442 void countdown(void)
00443 {
00444     int i;
00445     for(i=3; i>0; i--) {
00446         showImage();
00447         wait(1);
00448         if(LABVIEW_MODE==0) pc.printf("Acquisisco fra %d s\n",i);
00449     }
00450 }