Diff: main.cpp

Revision:

3:9244869157bd

Parent:

2:4ebec58ffaca

--- 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