Mathieu Malone / PixyStereoCam

Uses multiple Pixy coordinates, communicated through SPI, to estimate the distance between the object and the parallel cameras.

Dependencies:   PixyLibrary

PixyStereoCam.cpp@1:b742e6df453e, 2014-08-12 (annotated)

Committer:
MBM
Date:
Tue Aug 12 11:05:44 2014 +0000
Revision:
1:b742e6df453e
Parent:
0:cac27bf8a28c 
This has been written based on Arduino code, provided by http://cmucam.org/projects/cmucam5/files

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MBM 0:cac27bf8a28c 1 //--------------------------------------------------------------------------------------------
MBM 0:cac27bf8a28c 2 //Original Property of: charmedlabs.com/pixystart -> arduino_pixy-x.y.z.zip
MBM 0:cac27bf8a28c 3 //
MBM 0:cac27bf8a28c 4 //Modifications made by: Mathieu Malone
MBM 0:cac27bf8a28c 5 //Modifications: Modified Arduino code to function with mbed development platform
MBM 0:cac27bf8a28c 6 //Output Method: This program uses "Serial pc(USBTX, USBRX)" in order to allow communication
MBM 0:cac27bf8a28c 7 // between the mbed platform and putty terminal through USB.
MBM 0:cac27bf8a28c 8 //
MBM 0:cac27bf8a28c 9 //Purpose: This program uses two CMUcam5 Pixy cameras in parallel, and allows the coordinates
MBM 0:cac27bf8a28c 10 // gathered from each camera to be used to estimate a distance 'z' from the cameras.
MBM 0:cac27bf8a28c 11 //
MBM 0:cac27bf8a28c 12 //Latest update by: Mathieu Malone
MBM 0:cac27bf8a28c 13 //Date of last update: August 12th, 2014
MBM 0:cac27bf8a28c 14 //--------------------------------------------------------------------------------------------
MBM 0:cac27bf8a28c 15
MBM 0:cac27bf8a28c 16 double Xp1; double Xp2; //Global variable keeping track of the X coordinates in TPixy#.h
MBM 0:cac27bf8a28c 17 double Yp1; double Yp2; //Global variable keeping track of the Y coordinates in TPixy#.h
MBM 0:cac27bf8a28c 18 double sig1; double sig2; //Global variable to keep track of object in view in TPixy#.h
MBM 0:cac27bf8a28c 19
MBM 0:cac27bf8a28c 20 #include "mbed.h"
MBM 0:cac27bf8a28c 21 #include "Pixy1.h"
MBM 0:cac27bf8a28c 22 #include "Pixy2.h"
MBM 0:cac27bf8a28c 23 #include "Motor.h"
MBM 0:cac27bf8a28c 24
MBM 0:cac27bf8a28c 25 #define pixel 1.2e-5 //pixel dimensions
MBM 0:cac27bf8a28c 26 #define f 2.8e-3 //focal length
MBM 0:cac27bf8a28c 27 #define s 7.75e-2 //distance between lenses
MBM 0:cac27bf8a28c 28 #define xc 160 //center coordinates for x-axis in pixels
MBM 0:cac27bf8a28c 29 #define pi 3.141592653589793
MBM 0:cac27bf8a28c 30
MBM 0:cac27bf8a28c 31 // Distance Calculation Variables
MBM 0:cac27bf8a28c 32 double X1; double X2;
MBM 0:cac27bf8a28c 33 double Theta1; double Theta2;
MBM 0:cac27bf8a28c 34 double s1; double s2; double z;
MBM 0:cac27bf8a28c 35
MBM 0:cac27bf8a28c 36 int main(){
MBM 0:cac27bf8a28c 37
MBM 0:cac27bf8a28c 38 //Right PIXY
MBM 0:cac27bf8a28c 39 Serial pc1(USBTX, USBRX);
MBM 0:cac27bf8a28c 40 Pixy1 pixy1;
MBM 0:cac27bf8a28c 41 SPI spi1(p5, p6, p7); // mosi, miso, sclk
MBM 0:cac27bf8a28c 42 spi1.format(16,0); //16 bits mode 0
MBM 0:cac27bf8a28c 43 spi1.frequency(100000);//set the frequency in Hz
MBM 0:cac27bf8a28c 44 pc1.printf("\r Starting #1...\n");
MBM 0:cac27bf8a28c 45
MBM 0:cac27bf8a28c 46 //Left PIXY
MBM 0:cac27bf8a28c 47 Serial pc2(USBTX, USBRX);
MBM 0:cac27bf8a28c 48 Pixy2 pixy2;
MBM 0:cac27bf8a28c 49 SPI spi2(p11, p12, p13);// mosi, miso, sclk
MBM 0:cac27bf8a28c 50 spi2.format(16,0); //16 bits mode 0
MBM 0:cac27bf8a28c 51 spi2.frequency(100000); //set the frequency in Hz
MBM 0:cac27bf8a28c 52 pc2.printf("\r Starting #2...\n");
MBM 0:cac27bf8a28c 53
MBM 0:cac27bf8a28c 54 while(1){
MBM 0:cac27bf8a28c 55
MBM 0:cac27bf8a28c 56 static int i1 = 0;
MBM 0:cac27bf8a28c 57 int j1;
MBM 0:cac27bf8a28c 58 uint16_t blocks1;
MBM 0:cac27bf8a28c 59 char buf1[32];
MBM 0:cac27bf8a28c 60
MBM 0:cac27bf8a28c 61 blocks1 = pixy1.getBlocks1();
MBM 0:cac27bf8a28c 62
MBM 0:cac27bf8a28c 63 if (blocks1)
MBM 0:cac27bf8a28c 64 {
MBM 0:cac27bf8a28c 65 i1++;
MBM 0:cac27bf8a28c 66
MBM 0:cac27bf8a28c 67 if (i1%50==0)
MBM 0:cac27bf8a28c 68 {
MBM 0:cac27bf8a28c 69 sprintf(buf1, "\r Detected #1 %d:\r\n", blocks1);
MBM 0:cac27bf8a28c 70 pc1.printf(buf1);
MBM 0:cac27bf8a28c 71 for (j1=0; j1<blocks1; j1++)
MBM 0:cac27bf8a28c 72 {
MBM 0:cac27bf8a28c 73 sprintf(buf1, " block #1 %d: ", j1);
MBM 0:cac27bf8a28c 74 printf(buf1);
MBM 0:cac27bf8a28c 75 pixy1.blocks1[j1].print1();
MBM 0:cac27bf8a28c 76 }
MBM 0:cac27bf8a28c 77 }
MBM 0:cac27bf8a28c 78 }
MBM 0:cac27bf8a28c 79
MBM 0:cac27bf8a28c 80 static int i2 = 0;
MBM 0:cac27bf8a28c 81 int j2;
MBM 0:cac27bf8a28c 82 uint16_t blocks2;
MBM 0:cac27bf8a28c 83 char buf2[32];
MBM 0:cac27bf8a28c 84
MBM 0:cac27bf8a28c 85 blocks2 = pixy2.getBlocks2();
MBM 0:cac27bf8a28c 86
MBM 0:cac27bf8a28c 87 if (blocks2)
MBM 0:cac27bf8a28c 88 {
MBM 0:cac27bf8a28c 89 i2++;
MBM 0:cac27bf8a28c 90
MBM 0:cac27bf8a28c 91 if (i2%50==0)
MBM 0:cac27bf8a28c 92 {
MBM 0:cac27bf8a28c 93 sprintf(buf2, "\r Detected #2 %d:\r\n", blocks2);
MBM 0:cac27bf8a28c 94 pc2.printf(buf2);
MBM 0:cac27bf8a28c 95 for (j2=0; j2<blocks2; j2++)
MBM 0:cac27bf8a28c 96 {
MBM 0:cac27bf8a28c 97 sprintf(buf2, " block #2 %d: ", j2);
MBM 0:cac27bf8a28c 98 printf(buf2);
MBM 0:cac27bf8a28c 99 pixy2.blocks2[j2].print2();
MBM 0:cac27bf8a28c 100 }
MBM 0:cac27bf8a28c 101 }
MBM 0:cac27bf8a28c 102 }
MBM 0:cac27bf8a28c 103
MBM 0:cac27bf8a28c 104 if (blocks2 || blocks1){
MBM 0:cac27bf8a28c 105 if ((Xp1 != 0) && (Xp2 != 0) && ((i1%50==0) || (i2%50==0))){
MBM 0:cac27bf8a28c 106 //pc1.printf("\r Xp1 = %f\r\n",Xp1); pc2.printf(" Xp2 = %f\r\n",Xp2);
MBM 0:cac27bf8a28c 107 X1 = (Xp1-xc)*pixel; X2 = (Xp2-xc)*pixel;
MBM 0:cac27bf8a28c 108 //pc1.printf(" X1 = %f\r\n",X1); pc2.printf(" X2 = %f\r\n",X2);
MBM 0:cac27bf8a28c 109 Theta1 = atan((f/X1)); Theta2 = atan((f/X2));
MBM 0:cac27bf8a28c 110 //pc1.printf(" Theta1 = %f\r\n",Theta1); pc2.printf(" Theta2 = %f\r\n",Theta2);
MBM 0:cac27bf8a28c 111 s1 = abs(s*(sin(pi-Theta2)/sin(Theta2-Theta1))); s2 = abs(s*(sin(Theta1)/sin(Theta2-Theta1)));
MBM 0:cac27bf8a28c 112 //pc1.printf(" s1 = %f\r\n",s1); pc2.printf(" s2 = %f\r\n",s2);
MBM 0:cac27bf8a28c 113 z = (s1+s2)/2;
MBM 0:cac27bf8a28c 114 pc1.printf("\r Distances is: %f meters \r\n", z);
MBM 0:cac27bf8a28c 115 }
MBM 0:cac27bf8a28c 116 }
MBM 0:cac27bf8a28c 117 }
MBM 0:cac27bf8a28c 118 }
MBM 0:cac27bf8a28c 119