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
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@3:9244869157bd, 2017-10-03 (annotated)
- 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?
User | Revision | Line number | New 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 | } |