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

Committer:
rulla
Date:
Tue Oct 03 16:47:15 2017 +0000
Revision:
3:9244869157bd
Parent:
2:4ebec58ffaca
3D point cloud scanner with OV7670 camera.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rulla 3:9244869157bd 1 //3D Scanner
rulla 3:9244869157bd 2 //2017 Angelo Mottolese mottoleseangelo@gmail.com
ms523 0:aabbf2286bf2 3 #include "mbed.h"
rulla 2:4ebec58ffaca 4 #include "OV7670.h"
rulla 3:9244869157bd 5 #include "ST7735_TFT.h"
rulla 3:9244869157bd 6 #include "ADXL345_I2C.h"
rulla 3:9244869157bd 7 #include "HMC5843.h"
rulla 3:9244869157bd 8 #include "SDFileSystem.h"
rulla 3:9244869157bd 9 #include "Servo.h"
rulla 3:9244869157bd 10
rulla 3:9244869157bd 11 #define SIZEX 320
rulla 3:9244869157bd 12 #define SIZEY 240
rulla 3:9244869157bd 13 #define SIZE SIZEX*SIZEY
rulla 3:9244869157bd 14 #define LABVIEW_MODE 1
rulla 3:9244869157bd 15
rulla 3:9244869157bd 16
rulla 3:9244869157bd 17 #define TRANSFER_SIZE 4
rulla 3:9244869157bd 18 #define N_VAR 14
rulla 3:9244869157bd 19
rulla 3:9244869157bd 20 //LocalFileSystem local("local"); // Create the local filesystem under the name "local"
rulla 3:9244869157bd 21
ms523 0:aabbf2286bf2 22
rulla 2:4ebec58ffaca 23 OV7670 cam(
ms523 0:aabbf2286bf2 24 p28,p27, // SDA,SCL(I2C / SCCB)
rulla 2:4ebec58ffaca 25 p23,NC,p25, // VSYNC,HREF,WEN(FIFO)
rulla 3:9244869157bd 26 Port0,0x07878000,//PortIn(p18,p17,p16,p15,p11,p12,p14,p13) is D7-D0
rulla 2:4ebec58ffaca 27 p26,p29,p30) ; // RRST,OE,RCLK
rulla 3:9244869157bd 28 ST7735_TFT TFT(p5, p6, p7, p8, p9, p10,"TFT"); // mosi, miso, sclk, cs, rs, reset
rulla 3:9244869157bd 29 // SDA, nc, SCK,CS,A0,RESET = mosi, miso, sclk, cs, rs, reset
rulla 3:9244869157bd 30
rulla 3:9244869157bd 31 HMC5843 compass(p28, p27);
rulla 3:9244869157bd 32 ADXL345_I2C accelerometer(p28, p27);
rulla 3:9244869157bd 33 //SDFileSystem sd(p5, p6, p7, p21, "sd"); // the pinout on the mbed Cool Components workshop board
rulla 3:9244869157bd 34 //nRF24L01P my_nrf24l01p(p5, p6, p7, p24, p21, p20); // mosi, miso, sck, csn, ce, irq
rulla 3:9244869157bd 35 Servo yaw(p21);
rulla 3:9244869157bd 36 Serial pc(USBTX,USBRX);
rulla 3:9244869157bd 37 Timer tempo;
rulla 3:9244869157bd 38 AnalogIn pot(p20);
rulla 3:9244869157bd 39
rulla 3:9244869157bd 40 float Complementary(float newAngle, int looptime);
rulla 3:9244869157bd 41 float Complementary2(float newAngle, int looptime);
rulla 3:9244869157bd 42 double kalman_update(int index,double measurement);
rulla 3:9244869157bd 43 float kalmanCalculate(float newAngle, int looptime);
rulla 3:9244869157bd 44 void accMagInit(void);
rulla 3:9244869157bd 45 void ahrs(void);
rulla 3:9244869157bd 46 void kalman_init(void);
rulla 3:9244869157bd 47 void showImage(void);
rulla 3:9244869157bd 48 void image_minmax(void);
rulla 3:9244869157bd 49 void processImage(void);
rulla 3:9244869157bd 50 void sdInit(void);
rulla 3:9244869157bd 51 void nrfpInit(void);
rulla 3:9244869157bd 52 void countdown(void);
rulla 3:9244869157bd 53
rulla 3:9244869157bd 54 extern "C" void mbed_reset();
rulla 3:9244869157bd 55
rulla 3:9244869157bd 56 double q[N_VAR]; //process noise covariance
rulla 3:9244869157bd 57 double rk[N_VAR]; //measurement noise covariance
rulla 3:9244869157bd 58 double value[N_VAR]; //value
rulla 3:9244869157bd 59 double p[N_VAR]; //estimation error covariance
rulla 3:9244869157bd 60 double k[N_VAR]; //kalman gain
rulla 3:9244869157bd 61
rulla 3:9244869157bd 62 int i=0, loopN=0, l=0;
rulla 3:9244869157bd 63 float mv=0.0,servoHead=0.0;
ms523 0:aabbf2286bf2 64
rulla 3:9244869157bd 65 int thr=5;
rulla 3:9244869157bd 66
rulla 3:9244869157bd 67 float xrot=0.0,yrot=0.0,zrot=0.0, vx=0.0,vy=0.0,vz=0.0;
rulla 3:9244869157bd 68 int ax=0,ay=0,az=0,ax0,ay0,az0;
rulla 3:9244869157bd 69 int mx=0,my=0,mz=0,mx0,my0,mz0;
rulla 3:9244869157bd 70 double xh=0.0, yh=0.0, head=0.0;
rulla 3:9244869157bd 71 double ca=0.0, cb=0.0, cc=0.0;
rulla 3:9244869157bd 72 double sa=0.0, sb=0.0,sc=0.0;
rulla 3:9244869157bd 73 double fx,fy,fz;
rulla 3:9244869157bd 74 uint16_t thri=3,thrr=3,thrg=4;
rulla 3:9244869157bd 75
rulla 3:9244869157bd 76 uint16_t max_red,max_blue, max_green,min_blue,min_green,min_red;
rulla 3:9244869157bd 77 uint16_t colour=0,r,g,b;
rulla 3:9244869157bd 78 int x=0,y=0, lineacc,cnt=0,x0[SIZEY];
rulla 3:9244869157bd 79 uint16_t bank0,bank1;
rulla 3:9244869157bd 80 int ok=0;
rulla 3:9244869157bd 81 uint16_t mr=0,mb=0,mg=0,id=0;
rulla 3:9244869157bd 82
rulla 3:9244869157bd 83 // FILE *out=fopen("/local/world.txt","w");
rulla 3:9244869157bd 84
rulla 3:9244869157bd 85 int main()
rulla 3:9244869157bd 86 {
rulla 3:9244869157bd 87 // if ( out == NULL ) error("Could not open file.\r\n");
rulla 3:9244869157bd 88 //fprintf(out,"v 0.0 0.0 0.0\n");
rulla 3:9244869157bd 89 pc.baud(115200);
rulla 3:9244869157bd 90 yaw=0.0;
rulla 3:9244869157bd 91 //////////////////////Camera and TFT init
rulla 3:9244869157bd 92 TFT.set_orientation(3);
rulla 3:9244869157bd 93 //TFT.background(Black);
rulla 3:9244869157bd 94 TFT.cls();
rulla 3:9244869157bd 95 cam.Reset() ;
rulla 3:9244869157bd 96 cam.InitQQVGA(SIZEX) ;
rulla 3:9244869157bd 97
rulla 3:9244869157bd 98 pc.baud(115200);
rulla 3:9244869157bd 99 kalman_init();
rulla 3:9244869157bd 100 accMagInit();
rulla 3:9244869157bd 101 //nrfpInit();
rulla 3:9244869157bd 102 //sdInit();
rulla 3:9244869157bd 103
rulla 3:9244869157bd 104 /////////////////////////////////// Loop control////////////////////////////////////////////////////
rulla 3:9244869157bd 105 int j=0;
ms523 0:aabbf2286bf2 106
rulla 3:9244869157bd 107 for(j=1; j>0; j--) {
rulla 3:9244869157bd 108 if(LABVIEW_MODE==0) pc.printf("Posizionamento %d s\n", j);
rulla 3:9244869157bd 109 tempo.start();
rulla 3:9244869157bd 110 while(tempo.read()<0.5) showImage();
rulla 3:9244869157bd 111 tempo.stop();
rulla 3:9244869157bd 112 tempo.reset();
rulla 3:9244869157bd 113 }
rulla 3:9244869157bd 114 //countdown();
rulla 3:9244869157bd 115
rulla 3:9244869157bd 116 image_minmax();
rulla 3:9244869157bd 117
rulla 3:9244869157bd 118 // while(1) {
rulla 3:9244869157bd 119 //tempo.start();
rulla 3:9244869157bd 120 ahrs();
rulla 3:9244869157bd 121 ///////////Image Acquisition and Processing
rulla 3:9244869157bd 122 // if((hd[0]+0.22)<hd[1] || (hd[0]-0.22)>hd[1]){ //Heading noise gate
rulla 3:9244869157bd 123 ok=0;
rulla 3:9244869157bd 124
rulla 3:9244869157bd 125 //if(loopN%20==0)
rulla 3:9244869157bd 126 TFT.cls();
rulla 3:9244869157bd 127 //Misura intensità max e min
rulla 3:9244869157bd 128 if(loopN%100==0 || loopN==0) image_minmax(); //Se il numero di pixel validi è piccolo
ms523 1:c98598814170 129
rulla 3:9244869157bd 130 // printf(" SCANNING.\n");
rulla 3:9244869157bd 131 while(1) {
rulla 3:9244869157bd 132
rulla 3:9244869157bd 133 ahrs();
rulla 3:9244869157bd 134 image_minmax();
rulla 3:9244869157bd 135 processImage();
rulla 3:9244869157bd 136 loopN++;
rulla 3:9244869157bd 137 yaw=kalman_update(12,mv);
rulla 3:9244869157bd 138 servoHead=mv*1.5768863175;
rulla 3:9244869157bd 139 // wait_ms(20);
rulla 3:9244869157bd 140 }
rulla 3:9244869157bd 141 /* for(mv=1.0; mv>0.0; mv-=0.01) {
rulla 3:9244869157bd 142
rulla 3:9244869157bd 143 ahrs();
rulla 3:9244869157bd 144 image_minmax();
rulla 3:9244869157bd 145 processImage();
rulla 3:9244869157bd 146 loopN++;
rulla 3:9244869157bd 147 yaw=kalman_update(12,mv);
rulla 3:9244869157bd 148 servoHead=mv*1.5768863175;
rulla 3:9244869157bd 149 wait_ms(20);
rulla 3:9244869157bd 150 }
rulla 3:9244869157bd 151 yaw=kalman_update(12,0.0);*/
rulla 3:9244869157bd 152 //fclose(out);
rulla 3:9244869157bd 153 // printf(" END\n");
rulla 3:9244869157bd 154 // tempo.stop();
rulla 3:9244869157bd 155 // printf("Time taken= %f seconds, OK=%d\n", tempo.read(),ok);
rulla 3:9244869157bd 156 // tempo.reset();
rulla 3:9244869157bd 157 // }//Fine while(1)
rulla 3:9244869157bd 158 // mbed_reset();
rulla 3:9244869157bd 159 }
ms523 0:aabbf2286bf2 160
rulla 3:9244869157bd 161
rulla 3:9244869157bd 162 double kalman_update(int index,double measurement)
rulla 3:9244869157bd 163 {
rulla 3:9244869157bd 164 //prediction update
rulla 3:9244869157bd 165 //omit x = x
rulla 3:9244869157bd 166 p[index]+= q[index];
rulla 3:9244869157bd 167 //measurement update
rulla 3:9244869157bd 168 k[index] = p[index] / (p[index] + rk[index]);
rulla 3:9244869157bd 169 value[index] += k[index] * (measurement - value[index]);
rulla 3:9244869157bd 170 p[index] *= (1 - k[index]);
rulla 3:9244869157bd 171 //fprintf(out,"%f %f \r\n",p,k);
rulla 3:9244869157bd 172 return value[index];
rulla 3:9244869157bd 173 }
ms523 0:aabbf2286bf2 174
rulla 3:9244869157bd 175 void kalman_init(void)
rulla 3:9244869157bd 176 {
rulla 3:9244869157bd 177 for(l=0; l<N_VAR; l++) {
rulla 3:9244869157bd 178 q[l]=0.001; //process noise covariance
rulla 3:9244869157bd 179 rk[l]=0.1; //measurement noise covariance
rulla 3:9244869157bd 180 p[l]=0.0; //estimation error covariance
rulla 3:9244869157bd 181 k[l]=0.0; //kalman gain
rulla 3:9244869157bd 182 value[l]=0.0;
rulla 3:9244869157bd 183 }
rulla 3:9244869157bd 184 }
rulla 2:4ebec58ffaca 185
rulla 3:9244869157bd 186 ///////////////////////////////////////////SD Card init
rulla 3:9244869157bd 187 void sdInit(void)
rulla 3:9244869157bd 188 {
rulla 2:4ebec58ffaca 189 /*
rulla 3:9244869157bd 190 mkdir("/sd/", 0777);
rulla 3:9244869157bd 191 FILE *fp = fopen("/sd/worldSD.obj", "w");
rulla 3:9244869157bd 192 if(fp == NULL) error("Could not open file for write\n");
rulla 3:9244869157bd 193 fprintf(fp, "World mapping");
rulla 3:9244869157bd 194 fclose(fp);
rulla 3:9244869157bd 195 */
rulla 3:9244869157bd 196 }
rulla 3:9244869157bd 197
rulla 3:9244869157bd 198 ////////////////////////////AHRS//////////////////
rulla 3:9244869157bd 199 void accMagInit(void)
rulla 3:9244869157bd 200 {
rulla 3:9244869157bd 201
rulla 3:9244869157bd 202 int breadings[3] = {0, 0, 0};
rulla 3:9244869157bd 203 int bhmcreadings[3];
rulla 3:9244869157bd 204 char buffer[3];
rulla 3:9244869157bd 205 // if(LABVIEW_MODE==0) pc.printf("Starting ADXL345 test...\n");
rulla 3:9244869157bd 206 // if(LABVIEW_MODE==0) pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
rulla 3:9244869157bd 207 accelerometer.setPowerControl(0x00);
rulla 3:9244869157bd 208 accelerometer.setDataFormatControl(0x0B);
rulla 3:9244869157bd 209 accelerometer.setDataRate(ADXL345_3200HZ);
rulla 3:9244869157bd 210 accelerometer.setPowerControl(0x08);
rulla 3:9244869157bd 211 compass.getAddress(buffer);
rulla 3:9244869157bd 212 // if(LABVIEW_MODE==0) pc.printf("Magnetic Compass Id=%c%c%c \n\r",buffer[0],buffer[1],buffer[2]);
rulla 3:9244869157bd 213 compass.setDefault();
rulla 3:9244869157bd 214 wait(.1);
rulla 3:9244869157bd 215 for(i=0; i<50; i++) {
rulla 3:9244869157bd 216 accelerometer.getOutput(breadings);
rulla 3:9244869157bd 217 //13-bit, sign extended values.
rulla 3:9244869157bd 218 compass.readData(bhmcreadings);
rulla 3:9244869157bd 219 wait(0.1);
rulla 3:9244869157bd 220 ax+=(int16_t)(breadings[0]);
rulla 3:9244869157bd 221 ay+=(int16_t)(breadings[1]);
rulla 3:9244869157bd 222 az+=(int16_t)(breadings[2]);
rulla 3:9244869157bd 223 mx+=(int16_t)(bhmcreadings[0]);
rulla 3:9244869157bd 224 my+=(int16_t)(bhmcreadings[1]);
rulla 3:9244869157bd 225 mz+=(int16_t)(bhmcreadings[2]);
rulla 3:9244869157bd 226 }
rulla 3:9244869157bd 227 ax0=ax/50;
rulla 3:9244869157bd 228 ay0=ay/50;
rulla 3:9244869157bd 229 az0=az/50;
rulla 3:9244869157bd 230 mx0=mx/50;
rulla 3:9244869157bd 231 my0=my/50;
rulla 3:9244869157bd 232 mz0=mz/50;
rulla 3:9244869157bd 233 ax=0;
rulla 3:9244869157bd 234 ay=0;
rulla 3:9244869157bd 235 az=0;
rulla 3:9244869157bd 236 mx=0;
rulla 3:9244869157bd 237 my=0;
rulla 3:9244869157bd 238 mz=0;
rulla 3:9244869157bd 239 }
rulla 3:9244869157bd 240
rulla 3:9244869157bd 241 void ahrs(void)
rulla 3:9244869157bd 242 {
rulla 3:9244869157bd 243 cnt=0;
rulla 3:9244869157bd 244 int accelero=0, deadr=0;
rulla 3:9244869157bd 245
rulla 3:9244869157bd 246 if(accelero==1) {
rulla 3:9244869157bd 247
rulla 3:9244869157bd 248 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;
rulla 3:9244869157bd 249 float aax=0.0,aay=0.0,aaz=0.0;
rulla 3:9244869157bd 250 int readings[3] = {0, 0, 0};
rulla 3:9244869157bd 251 int hmcreadings[3];
rulla 3:9244869157bd 252 accelerometer.getOutput(readings);
rulla 3:9244869157bd 253 compass.readData(hmcreadings);
rulla 3:9244869157bd 254 //13-bit, sign extended values.
rulla 3:9244869157bd 255 cnt++;
rulla 3:9244869157bd 256 ax+=(int16_t)(readings[0]);
rulla 3:9244869157bd 257 ay+=(int16_t)(readings[1]);
rulla 3:9244869157bd 258 az+=(int16_t)(readings[2]);
rulla 3:9244869157bd 259 ax-=ax0;
rulla 3:9244869157bd 260 ay-=ay0;
rulla 3:9244869157bd 261 az-=az0;
rulla 3:9244869157bd 262 mx+=(int16_t)(hmcreadings[0]);
rulla 3:9244869157bd 263 my+=(int16_t)(hmcreadings[1]);
rulla 3:9244869157bd 264 mz+=(int16_t)(hmcreadings[2]);
rulla 3:9244869157bd 265 mx-=mx0;
rulla 3:9244869157bd 266 my-=my0;
rulla 3:9244869157bd 267 mz-=mz0;
rulla 3:9244869157bd 268 wait(0.1);
rulla 3:9244869157bd 269
rulla 3:9244869157bd 270 xd=(double)(ax);
rulla 3:9244869157bd 271 yd=(double)(ay);
rulla 3:9244869157bd 272 zd=(double)(az);
rulla 3:9244869157bd 273 aax=kalman_update(9,xd); //Kalman to accelerometer
rulla 3:9244869157bd 274 aay=kalman_update(10,yd);
rulla 3:9244869157bd 275 aaz=kalman_update(11,-zd);
rulla 3:9244869157bd 276
rulla 3:9244869157bd 277 if(deadr==1) {
rulla 3:9244869157bd 278 x1+=kalman_update(3,aax);
rulla 3:9244869157bd 279 y1+= kalman_update(4,aay);
rulla 3:9244869157bd 280 z1+= kalman_update(5,aaz);
rulla 3:9244869157bd 281
rulla 3:9244869157bd 282 if(aax<=1.6420 && aax>=-1.6420 ) x1=0.0;
rulla 3:9244869157bd 283 if(aay<=1.2315 && aay>=-1.1325 ) y1=0.0;
rulla 3:9244869157bd 284 if(aaz<=8.374 && aaz>=-8.374 ) z1=0.0;
rulla 3:9244869157bd 285 x2+=kalman_update(6,x1);
rulla 3:9244869157bd 286 y2+=kalman_update(7,y1);
rulla 3:9244869157bd 287 z2+=kalman_update(8,z1);
rulla 3:9244869157bd 288 }
rulla 3:9244869157bd 289 double xAngle=-0.00609*aay;
rulla 3:9244869157bd 290 double yAngle=0.00609*aax;
rulla 3:9244869157bd 291
rulla 3:9244869157bd 292 double mmx=(double)(mx);
rulla 3:9244869157bd 293 double mmy=(double)(my);
rulla 3:9244869157bd 294 double mmz=(double)(mz);
rulla 3:9244869157bd 295
rulla 3:9244869157bd 296 fx=kalman_update(0,xAngle);
rulla 3:9244869157bd 297 fy=kalman_update(1,yAngle);
rulla 3:9244869157bd 298
rulla 3:9244869157bd 299 ca = cos(fx);//theta
rulla 3:9244869157bd 300 cb = cos(fy);//phi
rulla 3:9244869157bd 301 sa = sin(fx);
rulla 3:9244869157bd 302 sb = sin(fy);
rulla 3:9244869157bd 303
rulla 3:9244869157bd 304 double yh=mmz*sb-mmy*cb;
rulla 3:9244869157bd 305 double xh=+mmx*ca+mmy*sa*sb+mmz*sa*cb;
rulla 3:9244869157bd 306 double head=atan2(yh,xh);
rulla 3:9244869157bd 307 fz=kalman_update(2,head);
rulla 3:9244869157bd 308 }
rulla 3:9244869157bd 309
rulla 3:9244869157bd 310
rulla 3:9244869157bd 311 //if(accelero==0) {
rulla 3:9244869157bd 312 fz=servoHead;
rulla 3:9244869157bd 313 fz=pot.read();
rulla 3:9244869157bd 314 fz-=0.1;
rulla 3:9244869157bd 315 fz*=6.2831168863175;
rulla 3:9244869157bd 316 fz=3.14-fz;
rulla 2:4ebec58ffaca 317
rulla 3:9244869157bd 318 cc = cos(fz);
rulla 3:9244869157bd 319 sc = sin(fz);
rulla 3:9244869157bd 320 //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));
rulla 3:9244869157bd 321 cnt=0;
rulla 3:9244869157bd 322 ax=0;
rulla 3:9244869157bd 323 ay=0;
rulla 3:9244869157bd 324 az=0;
rulla 3:9244869157bd 325
rulla 3:9244869157bd 326
rulla 3:9244869157bd 327 }
rulla 3:9244869157bd 328
rulla 3:9244869157bd 329 /////////////////////CAMERA FUNCTIONS/////////////////////
rulla 3:9244869157bd 330 void image_minmax(void)
rulla 3:9244869157bd 331 {
rulla 3:9244869157bd 332 for(id=0;id<SIZEY;id++) x0[id]=0;
rulla 3:9244869157bd 333 max_red=0;max_blue=0; max_green=0;min_blue=200;min_green=200;min_red=200;
rulla 3:9244869157bd 334
rulla 2:4ebec58ffaca 335 cam.CaptureNext() ;
rulla 2:4ebec58ffaca 336 while (cam.CaptureDone()==0) ;
rulla 3:9244869157bd 337 cam.ReadStart();
rulla 3:9244869157bd 338
rulla 3:9244869157bd 339 for (y = 0; y < SIZEY; y++) {
rulla 3:9244869157bd 340 for ( x= 0; x < SIZEX; x++) {
rulla 3:9244869157bd 341 bank0 = cam.ReadOneByte();
rulla 3:9244869157bd 342 bank1 = cam.ReadOneByte();
rulla 3:9244869157bd 343 r=((bank1 & 0x1f)) ;
rulla 3:9244869157bd 344 g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
rulla 3:9244869157bd 345 b=((bank0 & 0xf8)>>3 );
ms523 0:aabbf2286bf2 346
rulla 3:9244869157bd 347 /* if(b>max_blue) {
rulla 3:9244869157bd 348 max_blue=b;
rulla 3:9244869157bd 349 if(max_green<g) max_green=g;
rulla 3:9244869157bd 350 if(max_red<r) max_red=r;
rulla 3:9244869157bd 351 if(min_blue>b) min_blue=b;
rulla 3:9244869157bd 352 if(min_green>g) min_green=g;
rulla 3:9244869157bd 353 if(min_red>r) min_red=r;
rulla 3:9244869157bd 354 mr+=r;
rulla 3:9244869157bd 355 mb+=b;
rulla 3:9244869157bd 356 mg+=g;
rulla 3:9244869157bd 357 id++;
rulla 3:9244869157bd 358 }*/
rulla 3:9244869157bd 359 if(r>max_red) {
rulla 3:9244869157bd 360 max_red=r;
rulla 3:9244869157bd 361 if(max_green<g) max_green=g;
rulla 3:9244869157bd 362 if(max_blue<b) max_blue=b;
rulla 3:9244869157bd 363 if(min_blue>b) min_blue=b;
rulla 3:9244869157bd 364 if(min_green>g) min_green=g;
rulla 3:9244869157bd 365 if(min_red>r) min_red=r;
rulla 3:9244869157bd 366 mr+=r;
rulla 3:9244869157bd 367 mb+=b;
rulla 3:9244869157bd 368 mg+=g;
rulla 3:9244869157bd 369 id++;
rulla 3:9244869157bd 370 }
rulla 3:9244869157bd 371 }
rulla 3:9244869157bd 372 }
rulla 3:9244869157bd 373 cam.ReadStop() ;
rulla 3:9244869157bd 374 }
rulla 3:9244869157bd 375
rulla 3:9244869157bd 376
ms523 0:aabbf2286bf2 377
rulla 3:9244869157bd 378 void processImage(void)
rulla 3:9244869157bd 379 {
rulla 3:9244869157bd 380 cnt=0;
rulla 3:9244869157bd 381 double xrot,yrot,zrot;
rulla 3:9244869157bd 382 cam.CaptureNext() ;
rulla 3:9244869157bd 383 while (cam.CaptureDone()==0) ;
rulla 3:9244869157bd 384 cam.ReadStart();
rulla 3:9244869157bd 385 for (y = 0; y < SIZEY; y++) {
rulla 3:9244869157bd 386 for ( x= 0; x < SIZEX; x++) {
rulla 3:9244869157bd 387 bank0 = cam.ReadOneByte();
rulla 3:9244869157bd 388 bank1 = cam.ReadOneByte();
rulla 3:9244869157bd 389 r=((bank1 & 0x1f)) ;
rulla 3:9244869157bd 390 g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
rulla 3:9244869157bd 391 b=((bank0 & 0xf8)>>3 );
rulla 3:9244869157bd 392 //if(b>=(mb-thri) && r<=(mr+thrr) ) {
rulla 3:9244869157bd 393 if(r>=(max_red-thrr) && b>=(max_blue-thri)) {
rulla 3:9244869157bd 394 lineacc+=x;
rulla 3:9244869157bd 395 cnt++;
rulla 3:9244869157bd 396 }
rulla 3:9244869157bd 397 }
rulla 3:9244869157bd 398 if(cnt>1) {
rulla 3:9244869157bd 399 x0[y]=lineacc/cnt;
ms523 1:c98598814170 400
rulla 3:9244869157bd 401 ok++;
rulla 3:9244869157bd 402 }
rulla 3:9244869157bd 403 lineacc=0;
rulla 3:9244869157bd 404 cnt=0;
rulla 3:9244869157bd 405 }
rulla 3:9244869157bd 406 cam.ReadStop() ;
rulla 3:9244869157bd 407
rulla 3:9244869157bd 408 for(y=0;y<SIZEY;y++){
rulla 3:9244869157bd 409 vy=(double)(SIZEX)-(double)x0[y];//vy*sc
rulla 3:9244869157bd 410 vx=(double)(SIZEX)-(double)(SIZEX)*cc;//vy*cc
rulla 3:9244869157bd 411 vx=vy*cc;//vy*cc
rulla 3:9244869157bd 412 yrot=vy*sc;
rulla 3:9244869157bd 413 zrot=y*0.6;
rulla 3:9244869157bd 414 TFT.pixel(x0[y],y,x0[y]*10);
rulla 3:9244869157bd 415
rulla 3:9244869157bd 416 // if(LABVIEW_MODE==0) pc.printf("v %f. %f. %f\n",xrot,yrot,zrot);
rulla 3:9244869157bd 417 if(LABVIEW_MODE==0) pc.printf("%f %f %f %f ",vx,vy,vz,fz);
rulla 3:9244869157bd 418 if(LABVIEW_MODE==1) pc.printf("%.3f,%.3f,%.3f\n",vx,yrot,zrot);
rulla 3:9244869157bd 419 }
ms523 0:aabbf2286bf2 420 }
rulla 3:9244869157bd 421
rulla 3:9244869157bd 422
rulla 3:9244869157bd 423 void showImage(void)
rulla 3:9244869157bd 424 {
rulla 3:9244869157bd 425 cam.CaptureNext() ;
rulla 3:9244869157bd 426 while (cam.CaptureDone()==0) ;
rulla 3:9244869157bd 427 cam.ReadStart();
rulla 3:9244869157bd 428 for (y = 0; y < SIZEY; y++) {
rulla 3:9244869157bd 429 for ( x= 0; x < SIZEX; x++) {
rulla 3:9244869157bd 430 bank0 = cam.ReadOneByte();
rulla 3:9244869157bd 431 bank1 = cam.ReadOneByte();
rulla 3:9244869157bd 432 r=((bank1 & 0x1f)) ;
rulla 3:9244869157bd 433 g= (((bank0<<3)&0x38 )|((bank1>>5)&0x7)) & 0x3f;
rulla 3:9244869157bd 434 b=((bank0 & 0xf8)>>3 );
rulla 3:9244869157bd 435 colour = (((r<<11)&0xF800)|((g<<5)&0x7E0)| (b&0x1f));
rulla 3:9244869157bd 436 TFT.pixel(x,y,colour);
rulla 3:9244869157bd 437 }
rulla 3:9244869157bd 438 }
rulla 3:9244869157bd 439 cam.ReadStop() ;
rulla 3:9244869157bd 440 }
rulla 3:9244869157bd 441
rulla 3:9244869157bd 442 void countdown(void)
rulla 3:9244869157bd 443 {
rulla 3:9244869157bd 444 int i;
rulla 3:9244869157bd 445 for(i=3; i>0; i--) {
rulla 3:9244869157bd 446 showImage();
rulla 3:9244869157bd 447 wait(1);
rulla 3:9244869157bd 448 if(LABVIEW_MODE==0) pc.printf("Acquisisco fra %d s\n",i);
rulla 3:9244869157bd 449 }
rulla 2:4ebec58ffaca 450 }