Pedro SoaresGetúlio Igrejas
Published © LGPL

W2B - Whole WalaBot

This Walabot uses machine learning and a vertical robot to learn to differentiate water pipes and electrical wires behind the walls.

AdvancedFull instructions providedOver 5 days1,247
W2B - Whole WalaBot

Things used in this project

Hardware components

Raspberry Pi 3 Model B
Raspberry Pi 3 Model B
It is the system brain, for signal processing and data control.
×1
Camera Module
Raspberry Pi Camera Module
Raspberry Pi camera V2, It is used to get the homography about the wall plane.
×1
NEMA 17 Stepper Motor
OpenBuilds NEMA 17 Stepper Motor
They are used on the polar robot.
×2
USB-A to Micro-USB Cable
USB-A to Micro-USB Cable
5 meters - To connect the Walabot to the Raspberry Pi.
×1
USB-A to Mini-USB Cable
USB-A to Mini-USB Cable
5 meters - To connect the Nucleo F401RE to the Raspberry Pi.
×1
Nucleo-F401RE
To receive the motion commands from the Pi and control the IHM02A1 shield.
×1
X-NUCLEO-IHM02A1
Two axis stepper motor driver expansion board based on the L6470.
×1
SparkFun MPR121 shield - DEV-12013
×1
Helios Laser Dac
The gateway USB-ILDA for the laser control.
×1
Laserworld CS-1000RGB
The RGB laser projector w/ILDA interface.
×1
DB-25 M/F cable
To connect the laser gateway to the laser projector.
×1
USB-A to B Cable
USB-A to B Cable
To connect the laser gateway to the Raspberry Pi.
×1
6202 - ball bearing to the robot gondola
×1
some meters of 3.5mm ball chain for the robot arms
×1

Software apps and online services

OpenCV
OpenCV
To calculate the homography about the wall plane and to use the machine learning library.
MBED

Hand tools and fabrication machines

3D Printer (generic)
3D Printer (generic)
To make the enclosures, motor holders, mark holders, and the Walabot gondola.

Story

Read more

Custom parts and enclosures

Walabot gondola

It is used to carry the walabot on the robot.

Enclosure for the robot controller

It is the box for the NUCLEO F401, IMH02A1 and the touch keypad.

Gondola - oposite weigth

It is the gondola axis with a weight.

cap for walabot

It is a cap for walabot where you have to put the circular mark. Thus, the mark will be close to the wall reducing the homography error.

Code

mbed-robot.cpp

C/C++
Main file for the Nucleo F401. It is the robot server. It is to be compile in the mbed compiler.
/* IncludeS ------------------------------------------------------------------*/

/* mbed specific header files. */
#include "mbed.h"

/* Helper header files. */
#include "DevSPI.h"

/* Expansion Board specific header files. */
#include "XNucleoIHM02A1.h"

#include "mpr121.h"  // FOR THE TOUCH KEYPAD

#include <iostream>
#include <deque>
#include <string>
#include <sstream>


/* Definitions ---------------------------------------------------------------*/

/* Number of movements per revolution. */
#define MPR_1 4

/* Number of steps. */
#define STEPS_1 (200 * 128)   /* 1 revolution given a 200 steps motor configured at 1/128 microstep mode. */
#define STEPS_2 (STEPS_1 * 2)

// YOU NEED TO DEFINE THE STEPS FOR YOUR MOTOR IN ONE REVOLUTION
#define STEPS 200   

/* Delay in milliseconds. */
#define DELAY_1 1000
#define DELAY_2 2000
#define DELAY_3 5000


/* Buffer size */
#define BUFSIZE 1000

// 10 steps/s
#define MAXSPEED 10


/* Variables -----------------------------------------------------------------*/

/* Motor Control Expansion Board. */
XNucleoIHM02A1 *x_nucleo_ihm02a1;

/* Initialization parameters of the motors connected to the expansion board. */
L6470_init_t init[L6470DAISYCHAINSIZE] = {
    /* First Motor. */
    {
        12.0,                           /* Motor supply voltage in V. */
        STEPS,                           /* Min number of steps per revolution for the motor. */
        2.0,                           /* Max motor phase voltage in A. */
        2.2,                          /* Max motor phase voltage in V. */
        300.0,                         /* Motor initial speed [step/s]. */
        500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        MAXSPEED,                         /* Motor maximum speed [step/s]. */
        0.0,                           /* Motor minimum speed [step/s]. */
        602.7,                         /* Motor full-step speed threshold [step/s]. */
        2.2,                          /* Holding kval [V]. */
        2.2,                          /* Constant speed kval [V]. */
        2.2,                          /* Acceleration starting kval [V]. */
        2.2,                          /* Deceleration starting kval [V]. */
        61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        392.1569e-6,                   /* Start slope [s/step]. */
        643.1372e-6,                   /* Acceleration final slope [s/step]. */
        643.1372e-6,                   /* Deceleration final slope [s/step]. */
        0,                             /* Thermal compensation factor (range [0, 15]). */
        2.2 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        2.2 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        0x07, /* Step mode selection. */
        0xFF,                          /* Alarm conditions enable. */
        0x2E88                         /* Ic configuration. */
    },

    /* Second Motor. */
    {
        
        12.0,                           /* Motor supply voltage in V. */
        STEPS,                           /* Min number of steps per revolution for the motor. */
        2.0,                           /* Max motor phase voltage in A. */
        2.2,                          /* Max motor phase voltage in V. */
        300.0,                         /* Motor initial speed [step/s]. */
        500.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        500.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        MAXSPEED,                         /* Motor maximum speed [step/s]. */
        0.0,                           /* Motor minimum speed [step/s]. */
        602.7,                         /* Motor full-step speed threshold [step/s]. */
        2.2,                          /* Holding kval [V]. */
        2.2,                          /* Constant speed kval [V]. */
        2.2,                          /* Acceleration starting kval [V]. */
        2.2,                          /* Deceleration starting kval [V]. */
        61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        392.1569e-6,                   /* Start slope [s/step]. */
        643.1372e-6,                   /* Acceleration final slope [s/step]. */
        643.1372e-6,                   /* Deceleration final slope [s/step]. */
        0,                             /* Thermal compensation factor (range [0, 15]). */
        2.2 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        2.2 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        0x07, /* Step mode selection. */
        0xFF,                          /* Alarm conditions enable. */
        0x2E88                         /* Ic configuration. */
        
    }
};




Serial rpi(USBTX, USBRX);


std::deque<char> dq;

char stBuffer[BUFSIZE];

DigitalOut myled(LED1);

void callback()
{    
   // myled = !myled;
   dq.push_back( (char)rpi.getc() );
}




// Setup the i2c bus on PB_9 PB_8 (SDA, SCL)
I2C i2c(PB_9, PB_8);

// Setup the Mpr121:
// constructor(i2c object, i2c address of the mpr121)
Mpr121 mpr121(&i2c, Mpr121::ADD_VSS);


// Create the interrupt receiver object on pin PA_10
InterruptIn interrupt(PA_10);

// callback for the mpr121 interrupt
int key_code = 0;
int conv[9] = {9,6,3,8,5,2,7,4,1};
void fallInterrupt() {
    
    int value;
    int i;
    
    key_code = 0;
    
    value = mpr121.read(0x00);
    value += mpr121.read(0x01)<<8;
    
   
   if(value!=0)
   {
    int ix = log((double)value)/log((double)2);
   
    key_code = conv[ix]; 
      
    printf("%c\n", key_code+48);
   }    
  
}




/* Main ----------------------------------------------------------------------*/

int main()
{

    bool waitingForOrder = true;

    rpi.baud(19200);

    rpi.attach(&callback);
    
   // interruption for the touch keyboard
    interrupt.mode(PullUp);
    interrupt.fall(&fallInterrupt);
    
    

    /*----- Initialization. -----*/


    /* Initializing SPI bus. */
#ifdef TARGET_STM32F429
    DevSPI dev_spi(D11, D12, D13);
#else
    DevSPI dev_spi(D11, D12, D3);
#endif

    /* Initializing Motor Control Expansion Board. */
    x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);

    /* Building a list of motor control components. */
    L6470 **motors = x_nucleo_ihm02a1->get_components();

    /* Printing to the console. */
    // printf("Motor Control Application Example for 2 Motors\r\n\n");

     motors[0]->hard_stop();
     motors[1]->hard_stop();
   

    int i;
    int order=0;
    int dir0 = 0;
    int dir1 = 0;
    int steps0 = 0;
    int steps1 = 0;
    
   

    while(1) {

        
        i=0;
        if( !dq.empty() ) { // something was received
         
               
        std::fill(stBuffer, stBuffer+BUFSIZE, 0);
         
         
            do { // waiting for the End of the new command
                 
                 
                while(dq.empty())
                {
                  wait_us(102);
                }
                
                
                stBuffer[i] = dq.front();
                dq.pop_front();
               
                

            } while(stBuffer[i++] != 'E' && i<BUFSIZE); // End of string
            
            stBuffer[i]=0;
            
        
         
             // parsing the string
                string st(stBuffer);
                int pos = st.find('M');
                if(pos!=-1) //Mdir0#steps0#dir1#steps1E   M0#100#1#200E
                {
                      
                     std::stringstream ss(stBuffer+pos);
                  
                     char ch;  
                     ss >> ch >> dir0 >> ch >> steps0 >> ch >> dir1 >> ch >> steps1;  //Mdir0#steps0#dir1#steps1E
                     
                     waitingForOrder = false;
                     order = 'M';
                }
                  
        }  // if received


        if(waitingForOrder) {
            wait_ms(10);
            continue;
        }


        if(order == 'M') {
         
         int v0;
         int v1;
         double time;
         if(steps0 < steps1) // normalizing the speed in funtion of steps
         {    
            v1 = MAXSPEED;
            v0 = MAXSPEED * (1.0*steps0)/(1.0*steps1) ;
            time = (1.0*steps1)/MAXSPEED;         
         }
         else
         {      
            v0 = MAXSPEED;
            v1 = MAXSPEED * (1.0*steps1)/(1.0*steps0);
            time = (1.0*steps0)/MAXSPEED;
         }
         
                        
          myled = 1;
            
          motors[0]->prepare_run((StepperMotor::direction_t)dir0, v0);
            
          motors[1]->prepare_run((StepperMotor::direction_t)dir1, v1);

            
          wait_ms(10);

          x_nucleo_ihm02a1->perform_prepared_actions();
        
    
          wait_ms(time * 1000); // wait time while the motors are running 
        
          motors[0]->prepare_hard_stop();
          motors[1]->prepare_hard_stop();
        
          x_nucleo_ihm02a1->perform_prepared_actions();
          
          
          order = 0;
        
        
        printf("D\n"); // informing the raspberry pi about the end of this command
        
        myled = 0;
        
        } // if M
        
        waitingForOrder = true;
        
    }  // while

} // main

aFeatures.h

C/C++
Header file for descriptors
#pragma once

namespace signalFeatures {
	
	struct tofAmp{

        int tof;
        double Amp;

        };

	class AFeatures
	{

	private:
		double energy_;
		double mean_;
		double deviation_;
		double skewness_;
		double kurtosis_;
		double *p;
		int n;

		void signalEnergy();
		
		

	public:
		AFeatures();
		
		~AFeatures();

		double allEnergy();

		double mean();

		double deviation();

		double skewness();

		double kurtosis();

        struct tofAmp maxAmp();

		void getFeatures(double *v, int n, double &f1, double &f2, double &f3, double &f4);


	};

}

aFeatures.cpp

C/C++
Signal descriptors.
#include "aFeatures.h"
#include "math.h"

#include "stdio.h"

using namespace signalFeatures;

AFeatures::AFeatures()
{
	p = NULL;
}


AFeatures::~AFeatures()
{
	if(p != NULL)
	  delete []p;
}





void AFeatures::signalEnergy()
{
	for (int i = 0; i < n; i++)
	{
		p[i] *= p[i];
		p[i] = sqrt(p[i]);
	}
}



double AFeatures::allEnergy()
{
	double sum = 0;
	for (int i = 0; i < n; i++)
	{
		sum += p[i];
	}

	for (int i = 0; i < n; i++)
	{
		p[i] /= sum;

		
	}

	energy_ = sum;
	return sum;
}



double AFeatures::mean()
{
	double m = 0;
	for (int b = 0; b < n; b++)
	{
		m += b*(p[b]);
	}
	mean_ = m;
	return m;
}








struct tofAmp AFeatures::maxAmp()
{

	double m = 0;
	int t=0;
	
	struct tofAmp info;
	
	double max_ = p[0];
	for (int b = 1; b < n; b++)
	{
		if(max_ < p[b] )
		{m = p[b];
		 t = b;
		}
	}
	
	info.tof = t;
	info.Amp = m;
	
	return info;
}






double AFeatures::deviation()
{
	double d = 0;
	for (int b = 0; b < n; b++)
	{
		d += (b - mean_)*(b - mean_) * p[b];
	}

	d = sqrt(d);

	deviation_ = d;
	return d;

}


double AFeatures::skewness()
{
	double w = 0;
	for (int b = 0; b < n; b++)
	{
		w += (b - mean_)*(b - mean_)*(b - mean_) * p[b];

		 
	}

	w *= 1.0/(deviation_*deviation_*deviation_);

	skewness_ = w;
	return w;

}



double AFeatures::kurtosis()
{
	double k = 0;
	for (int b = 0; b < n; b++)
	{
		k += (b - mean_)*(b - mean_)*(b - mean_)*(b - mean_) * p[b];


	}

	k *= 1.0 / (deviation_*deviation_*deviation_*deviation_) -3.0;

	kurtosis_ = k;
	return k;

}


void AFeatures::getFeatures(double *v, int n, double &f1, double &f2, double &f3, double &f4)
{
	energy_ = 0;
	mean_ = 1;
	deviation_ = 0;
	skewness_ = 0;
	kurtosis_ = 0;

	this->n = n;

	if (p == NULL)
	{
		this->p = new double[n];

		for (int i = 0; i < n; i++)
			p[i] = v[i];

		

	}
	
	// keep this order
	signalEnergy();
	allEnergy();
	f1 = this->mean();	
	f2 = this->deviation();
	f3 = this->skewness();
	f4 = this->kurtosis();
	
}

antennasLoc.h

C/C++
Data about the antennas location.
double antennasLoc[][3] = 
{
 {-0.030000, 0.040000, -0.003000},
{-0.010000, 0.040000, -0.003000},
{0.010000, 0.040000, -0.003000},
{0.030000, 0.040000, -0.003000},
{-0.030000, 0.020000, -0.003000},
{-0.010000, 0.020000, -0.003000},
{0.010000, 0.020000, -0.003000},
{0.030000, 0.020000, -0.003000},
{-0.030000, 0.000000, -0.003000},
{-0.010000, 0.000000, -0.003000},
{0.010000, 0.000000, -0.003000},
{0.030000, 0.000000, -0.003000},
{-0.030000, -0.020000, -0.003000},
{-0.010000, -0.020000, -0.003000},
{0.010000, -0.020000, -0.003000},
{0.030000, -0.020000, -0.003000},
{-0.030000, -0.040000, -0.003000},
{0.030000, -0.040000, -0.003000},
};

CameraSys.h

C/C++
The header file to the camera system.
#ifndef CameraSys_H
#define CameraSys_H

#include <opencv2/opencv.hpp>
#include <iostream>
#include <stdlib.h>


#define BOARDWIDTH  6
#define BOARDHEIGHT 4
#define SQUARESCALE_MM 50
#define MARK_DIA_MM 42


using namespace cv;

class CameraSys{
	
	
	 VideoCapture *cap;
	 Mat gray, frame, grayCorrected;
	 Mat homography;
	 Mat colorCorrected;
	 Mat t, s;
	 
	 std::vector<Point2f> imagePoints;
	 
	 double sX;
	
	 std::vector<Point2f> boardPoints;
	 std::vector<Point2f> sourcePoints;
	 float radius[3];
	 
	 
	 int detectCircles();
	 int getHomography();
	 static bool  comp(Vec3f e1, Vec3f e2);
	 
	 void getCorrectedImage(Mat homography, Mat &correctedImage);
	
	 public:
	 CameraSys();
	 ~CameraSys();
	 
	 void capFrame();
	 
	 int getTrueTriangDistances(int &PL, int &PR, int &Base);
	 
	
	};
	
	
#endif	

CameraSys.cpp

C/C++
The class code to calculate the homography and detect the circular markers.
#include "CameraSys.h"



CameraSys::CameraSys()
{
	try{
	cap = new VideoCapture(0);
	
	
	if(!cap->isOpened())
	{
	   std::cout << "No Camera!\n" << std::endl;
       exit(1);
    }
    
    cap->set(CV_CAP_PROP_FRAME_WIDTH, 1920);
    cap->set(CV_CAP_PROP_FRAME_HEIGHT, 1080);
    
    
    
    // filling the board data
   for(int j=0; j<BOARDHEIGHT; j++)
    for(int i=0; i<BOARDWIDTH; i++)      
         boardPoints.push_back(Point2f((float)i, (float)j));
    
    
    }catch(std::bad_alloc &ba)
    {
		
		std::cout << "Bad alloc on Camera module!\n" << ba.what() << std::endl;
		exit(1);
	}
    
    
    
}



CameraSys::~CameraSys()
{
	if(cap != NULL)
        delete cap;		
}



void CameraSys::capFrame()
{
   
   *cap >> frame;
   
   flip(frame, frame, -1);	
	
   cvtColor(frame, gray, CV_BGR2GRAY);
   GaussianBlur(gray, gray, Size(7,7), 1.5, 1.5);	
    
  
   Mat gray2 = gray;
   resize(gray, gray2, Size(), 0.5, 0.5);
        
   imshow("window", gray2);
}





void CameraSys::getCorrectedImage(Mat homography, Mat &correctedImage)
{
	  double maxCols(0), maxRows(0), minCols(0), minRows(0);
      
      std::vector<Point2f> imageCorners;
      imageCorners.push_back(Point2f(0,0));
      imageCorners.push_back(Point2f(gray.cols-1,0));
      
      imageCorners.push_back(Point2f(gray.rows-1, gray.cols-1));
      imageCorners.push_back(Point2f(0, gray.rows-1));
      
      
      
      std::vector<Point2f> sceneCorners;
      
      perspectiveTransform(imageCorners, sceneCorners, homography);

 for(int i=0;i<sceneCorners.size();i++)
 {
   if(maxRows < sceneCorners.at(i).y)
        maxRows = sceneCorners.at(i).y;
   if(maxCols < sceneCorners.at(i).x)
        maxCols = sceneCorners.at(i).x;
 }

for(int i=0;i<sceneCorners.size();i++)
{
   if(minRows > sceneCorners.at(i).y)
        minRows = sceneCorners.at(i).y;
   if(minCols > sceneCorners.at(i).x)
        minCols = sceneCorners.at(i).x;
}
      
   double tm[3][3] =  {
     {1.0 , 0.0 , -minCols},
     { 0.0 , 1.0 , -minRows},
     { 0.0 , 0.0,    1.0    }
     };
     
     
     sX = gray.cols/(maxCols-minCols);
     
    double sm[3][3] =  {
     {1.0 , 0.0 , 0.0},
     { 0.0 , 1.0 , 0.0},
     { 0.0, 0.0,   1.0/sX}
     }; 
     
     Mat t(3,3, CV_64FC1, tm);
     Mat s(3,3, CV_64FC1, sm);
     
     Mat h =  s*t*homography;
      
    
    // Warp source image to destination based on homography
    warpPerspective(gray, correctedImage, h, gray.size() );
    
    imshow("correctedImage", correctedImage); 
	
}




int CameraSys::getHomography()
{
   	int patternFound;
   	
   	patternFound = findChessboardCorners(gray, Size(BOARDWIDTH, BOARDHEIGHT), imagePoints, CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_FAST_CHECK);
   	
   	 if(patternFound)
   	 {
	     printf("Pattern found!\n");
	     
	     cornerSubPix(gray, imagePoints, Size(11,11), Size(-1,-1), TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.1));
	     
	     homography = findHomography(imagePoints, boardPoints);	 
	     
	     
	     Mat chess = frame.clone();
	     
	     drawChessboardCorners(chess, Size(BOARDWIDTH,BOARDHEIGHT), imagePoints, patternFound);
	     waitKey(30);
	     
	     imshow("chess",chess);
      
      
      double maxCols(0), maxRows(0), minCols(0), minRows(0);
      
      std::vector<Point2f> imageCorners;
      imageCorners.push_back(Point2f(0,0));
      imageCorners.push_back(Point2f(gray.cols-1,0));
      
      imageCorners.push_back(Point2f(gray.rows-1, gray.cols-1));
      imageCorners.push_back(Point2f(0, gray.rows-1));
      
      
      
      std::vector<Point2f> sceneCorners;
      
      perspectiveTransform(imageCorners, sceneCorners, homography);

 for(int i=0;i<sceneCorners.size();i++)
{
   if(maxRows < sceneCorners.at(i).y)
        maxRows = sceneCorners.at(i).y;
   if(maxCols < sceneCorners.at(i).x)
        maxCols = sceneCorners.at(i).x;
}

for(int i=0;i<sceneCorners.size();i++)
{
   if(minRows > sceneCorners.at(i).y)
        minRows = sceneCorners.at(i).y;
   if(minCols > sceneCorners.at(i).x)
        minCols = sceneCorners.at(i).x;
}
      
   double tm[3][3] =  {
     {1.0 , 0.0 , -minCols},
     { 0.0 , 1.0 , -minRows},
     { 0.0 , 0.0,    1.0    }
     };
     
     
     sX = gray.cols/(maxCols-minCols);
     
    double sm[3][3] =  {
     {1.0 , 0.0 , 0.0},
     { 0.0 , 1.0 , 0.0},
     { 0.0, 0.0,   1.0/sX}
     }; 
     
     t = Mat(3,3, CV_64FC1, tm);
     s = Mat(3,3, CV_64FC1, sm);
     
     Mat h =  s*t*homography;
      
    
    // Warp source image to destination based on homography
    warpPerspective(gray, grayCorrected, h, gray.size() );
    

    imshow("grayCorrected", grayCorrected); 
      
	 
	     return 1;
	 }
	 else
	     return 0;
	
}



bool CameraSys::comp(Vec3f e1, Vec3f e2)
{
	return e1[0] < e2[0];  // testing
}



int CameraSys::detectCircles()
{
	capFrame();
	
	std::vector<Vec3f> circles;
	HoughCircles(grayCorrected, circles, CV_HOUGH_GRADIENT, 1, gray.rows/8, 90, 30, 3, 30); // image, circles, CV_HOUGH_GRADIENT, resolution, min distance, canny 2.threshold, min voting, min size, max size  
	
	 
	 
	 cvtColor(grayCorrected, colorCorrected, CV_GRAY2BGR);
	
	if(circles.size() == 3 )
	{
	    std::sort(circles.begin(), circles.end(), comp);
	    
	    for(int i=0; i<3; i++)
	    {
		   int x,y;
		   
		   x = circles[i][0];
		   y = circles[i][1];
		   radius[i] = circles[i][2];
		   Point center(x,y);
		   
		   Point2f Pf((float)center.x, (float)center.y);
		   
		   
		   
		   sourcePoints.push_back(Pf);
		   
			   
		   circle(colorCorrected, center, radius[i], Scalar(0,255, 0), 3, 8, 0);
		   
		   
		}
		
		
		
		resize(colorCorrected, colorCorrected, Size(), 0.5, 0.5);
		
		imshow("Calibration", colorCorrected);
	    
	    return 1;
	}
	else
	  return 0;
}



int CameraSys::getTrueTriangDistances(int &PL, int &PR, int &Base)
{
	    
	if(getHomography() == 0)
	    return 0;
	    
	    
	if(detectCircles() == 0)
	    return 0;
	    
	
	if(!homography.empty())
	{
	 std::vector<Point2f> destinationPoints;
	 
	 
	 double si[3][3] =  {
     {1.0 , 0.0 , 0.0},
     { 0.0 , 1.0 , 0.0},
     { 0.0, 0.0,   sX}
     }; 
     
     Mat smi(3,3, CV_64FC1, si);
		
	 perspectiveTransform(sourcePoints, destinationPoints, smi);
	  
	 
	 PL = sqrtf((pow(destinationPoints[0].x-destinationPoints[1].x,2)+pow((destinationPoints[0].y+95.0/SQUARESCALE_MM)-destinationPoints[1].y,2)))*SQUARESCALE_MM;
	 PR = sqrtf((pow(destinationPoints[1].x-destinationPoints[2].x,2)+pow(destinationPoints[1].y-(destinationPoints[2].y+95.0/SQUARESCALE_MM),2)))*SQUARESCALE_MM;
	 Base = sqrtf((pow(destinationPoints[0].x-destinationPoints[2].x,2)+pow(destinationPoints[0].y-destinationPoints[2].y,2)))*SQUARESCALE_MM;
	 
	 
	 
	 printf("calculado %d\n",PL);
	 return 1;
    }
    else
     return 0;
	
}

HeliosDac.h

C/C++
The header file to the Helios API.
/*
Driver API for Helios Laser DAC class, HEADER
By Gitle Mikkelsen
gitlem@gmail.com

Dependencies:
Libusb 1.0 (GNU Lesser General Public License, see libusb.h)

Standard: C++14
git repo: https://github.com/Grix/helios_dac.git

BASIC USAGE:
1.	Call OpenDevices() to open devices, returns number of available devices.
2.	To send a new frame, first call GetStatus(). If the function returns ready (1), 
	then you can call WriteFrame(). The status should be polled until it returns ready. 
	It can and sometimes will fail to return ready on the first try.
3.  To stop output, use Stop(). To restart output you must send a new frame as described above.
4.	When the DAC is no longer needed, destroy the instance (destructors will free everything and close the connection)
*/

#pragma once

#include "libusb.h"
#include <cstring>
#include <cstdint>
#include <thread>
#include <mutex>
#include <vector>
#include <memory>
#include <chrono>
#include <future>

#define HELIOS_SDK_VERSION	5

#define HELIOS_MAX_POINTS	0x1000
#define HELIOS_MAX_RATE		0xFFFF
#define HELIOS_MIN_RATE		7

#define HELIOS_SUCCESS		1		
#define HELIOS_ERROR		-1		//functions return this if something went wrong
	
#define HELIOS_FLAGS_DEFAULT			0
#define HELIOS_FLAGS_START_IMMEDIATELY	(1 << 1)
#define HELIOS_FLAGS_SINGLE_MODE		(1 << 2)

//usb properties
#define HELIOS_VID	0x1209
#define HELIOS_PID	0xE500
#define EP_BULK_OUT	0x02
#define EP_BULK_IN	0x81
#define EP_INT_OUT	0x06
#define EP_INT_IN	0x83

#ifdef _DEBUG
#define LIBUSB_LOG_LEVEL LIBUSB_LOG_LEVEL_WARNING
#else
#define LIBUSB_LOG_LEVEL LIBUSB_LOG_LEVEL_NONE
#endif

//point data structure
typedef struct
{
	std::uint16_t x; //12 bit (from 0 to 0xFFF)
	std::uint16_t y; //12 bit (from 0 to 0xFFF)
	std::uint8_t r;	//8 bit	(from 0 to 0xFF)
	std::uint8_t g;	//8 bit (from 0 to 0xFF)
	std::uint8_t b;	//8 bit (from 0 to 0xFF)
	std::uint8_t i;	//8 bit (from 0 to 0xFF)
} HeliosPoint;

class HeliosDac
{
public:

	HeliosDac();
	~HeliosDac();

	//unless otherwise specified, functions return HELIOS_SUCCESS if OK, and HELIOS_ERROR if something went wrong.

	//initializes drivers, opens connection to all devices.
	//Returns number of available devices.
	//NB: To re-scan for newly connected DACs after this function has once been called before, you must first call CloseDevices()
	int OpenDevices();

	//closes and frees all devices
	int CloseDevices();

	//writes and outputs a frame to the speficied dac
	//devNum: dac number (0 to n where n+1 is the return value from OpenDevices() )
	//pps: rate of output in points per second
	//flags: (default is 0)
	//	Bit 0 (LSB) = if 1, start output immediately, instead of waiting for current frame (if there is one) to finish playing
	//	Bit 1 = if 1, play frame only once, instead of repeating until another frame is written
	//	Bit 2-7 = reserved
	//points: pointer to point data. See point structure declaration earlier in this document
	//NB! This function is asynchronous, so the points buffer must not be freed immediately after calling this function. It is safe to free
	//the points buffer once the status (GetStatus()) has returned to 1.
	//numOfPoints: number of points in the frame
	int WriteFrame(unsigned int devNum, unsigned int pps, std::uint8_t flags, HeliosPoint* points, unsigned int numOfPoints);

	//Gets status of DAC, 1 means DAC is ready to receive frame, 0 means it is not
	int GetStatus(unsigned int devNum);

	//Returns firmware version of DAC
	int GetFirmwareVersion(unsigned int devNum);

	//Gets name of DAC (populates name with max 32 characters)
	int GetName(unsigned int devNum, char* name);

	//Sets name of DAC (name must be max 31 characters incl. null terminator)
	int SetName(unsigned int devNum, char* name);

	//Stops output of DAC until new frame is written (NB: blocks for 100ms)
	int Stop(unsigned int devNum);

	//Sets shutter level of DAC
	int SetShutter(unsigned int devNum, bool level);

	//Erase the firmware of the DAC, allowing it to be updated by accessing the SAM-BA bootloader
	int EraseFirmware(unsigned int devNum);

private:

	class HeliosDacDevice
	{
	public:

		HeliosDacDevice(libusb_device_handle*);
		~HeliosDacDevice();
		int SendFrame(std::uint8_t* buffer, unsigned int bufferSize);
		int GetStatus();
		int GetFirmwareVersion();
		char* GetName();
		int SetName(char* name);
		int SetShutter(bool level);
		int Stop();
		int EraseFirmware();

	private:

		void DoFrame(std::uint8_t* buffer, unsigned int bufferSize);
		int SendControl(std::uint8_t* buffer, unsigned int bufferSize);

		struct libusb_transfer* interruptTransfer = NULL;
		struct libusb_device_handle* usbHandle;
		std::mutex frameLock;
		int firmwareVersion = 0;
		char name[32];
		bool closed = true;
	};

	std::vector<std::unique_ptr<HeliosDacDevice>> deviceList;
	std::mutex threadLock;
	bool inited = false;
};

HeliosDac.cpp

C/C++
The Helios API.
/*
Driver API for Helios Laser DAC class, SOURCE
By Gitle Mikkelsen
gitlem@gmail.com

Dependencies:
Libusb 1.0 (GNU Lesser General Public License, see libusb.h)

Standard: C++14
git repo: https://github.com/Grix/helios_dac.git

See header HeliosDac.h for function and usage documentation
*/


#include "HeliosDac.h"

HeliosDac::HeliosDac()
{
	inited = false;
}

HeliosDac::~HeliosDac()
{
	CloseDevices();
}

int HeliosDac::OpenDevices()
{
	if (inited)
		return deviceList.size();

	int result = libusb_init(NULL);
	if (result < 0)
		return result;

	libusb_set_debug(NULL, LIBUSB_LOG_LEVEL);

	libusb_device** devs;
	ssize_t cnt = libusb_get_device_list(NULL, &devs);
	if (cnt < 0)
		return (int)cnt;

	std::lock_guard<std::mutex> lock(threadLock);

	unsigned int devNum = 0;
	for (int i = 0; i < cnt; i++)
	{
		struct libusb_device_descriptor devDesc;
		int result = libusb_get_device_descriptor(devs[i], &devDesc);
		if (result < 0)
			continue;

		if ((devDesc.idProduct != HELIOS_PID) || (devDesc.idVendor != HELIOS_VID))
			continue;

		libusb_device_handle* devHandle;
		result = libusb_open(devs[i], &devHandle);
		if (result < 0)
			continue;

		result = libusb_claim_interface(devHandle, 0);
		if (result < 0)
			continue;

		result = libusb_set_interface_alt_setting(devHandle, 0, 1);
		if (result < 0)
			continue;

		//successfully opened, add to device list
		deviceList.push_back(std::make_unique<HeliosDacDevice>(devHandle));

		devNum++;
	}

	libusb_free_device_list(devs, 1);

	if (devNum > 0)
		inited = true;

	return devNum;
}

int HeliosDac::CloseDevices()
{
	if (!inited)
		return HELIOS_ERROR;

	std::lock_guard<std::mutex> lock(threadLock);
	inited = false;
	deviceList.clear(); //various destructors will clean all devices

	libusb_exit(NULL);

	return 1;
}

int HeliosDac::WriteFrame(unsigned int devNum, unsigned int pps, std::uint8_t flags, HeliosPoint* points, unsigned int numOfPoints)
{
	if (!inited)
		return 0;

	if ((points == NULL) || (numOfPoints > HELIOS_MAX_POINTS) || (pps > HELIOS_MAX_RATE) || (pps < HELIOS_MIN_RATE))
		return 0;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL; 
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();

	if (dev == NULL)
		return 0;

	//prepare frame buffer
	std::uint8_t frameBuffer[HELIOS_MAX_POINTS * 7 + 5];
	unsigned int bufPos = 0;
	for (unsigned int i = 0; i < numOfPoints; i++)
	{
		frameBuffer[bufPos++] = (points[i].x >> 4);
		frameBuffer[bufPos++] = ((points[i].x & 0x0F) << 4) | (points[i].y >> 8);
		frameBuffer[bufPos++] = (points[i].y & 0xFF);
		frameBuffer[bufPos++] = points[i].r;
		frameBuffer[bufPos++] = points[i].g;
		frameBuffer[bufPos++] = points[i].b;
		frameBuffer[bufPos++] = points[i].i;
	}
	frameBuffer[bufPos++] = (pps & 0xFF);
	frameBuffer[bufPos++] = (pps >> 8);
	frameBuffer[bufPos++] = (numOfPoints & 0xFF);
	frameBuffer[bufPos++] = (numOfPoints >> 8);
	frameBuffer[bufPos++] = flags;

	return dev->SendFrame(frameBuffer, bufPos);
}

int HeliosDac::GetStatus(unsigned int devNum)
{
	if (!inited)
		return -1;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL;
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();
	if (dev == NULL)
		return -1;

	return dev->GetStatus();
}

int HeliosDac::GetFirmwareVersion(unsigned int devNum)
{
	if (!inited)
		return -1;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL;
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();
	if (dev == NULL)
		return -1;

	return dev->GetFirmwareVersion();
}

int HeliosDac::GetName(unsigned int devNum, char* name)
{
	if (!inited)
		return HELIOS_ERROR;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL;
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();
	if (dev == NULL)
		return HELIOS_ERROR;

	char* dacName = dev->GetName();
	if (strcmp(dacName,"")==0)
	{
		memcpy(name, "Helios ", 8);
		name[7] = (char)((int)(devNum >= 10) + 48);
		name[8] = (char)((int)(devNum % 10) + 48);
		name[9] = '\0';
	}
	else
	{
		memcpy(name, dacName, 32);
	}
	return HELIOS_SUCCESS;
}

int HeliosDac::SetName(unsigned int devNum, char* name)
{
	if (!inited)
		return HELIOS_ERROR;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL;
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();
	if (dev == NULL)
		return HELIOS_ERROR;

	return dev->SetName(name);
}

int HeliosDac::Stop(unsigned int devNum)
{
	if (!inited)
		return HELIOS_ERROR;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL;
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();
	if (dev == NULL)
		return HELIOS_ERROR;

	return dev->Stop();
}

int HeliosDac::SetShutter(unsigned int devNum, bool level)
{
	if (!inited)
		return HELIOS_ERROR;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL;
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();
	if (dev == NULL)
		return HELIOS_ERROR;

	return dev->SetShutter(level);
}

int HeliosDac::EraseFirmware(unsigned int devNum)
{
	if (!inited)
		return HELIOS_ERROR;

	std::unique_lock<std::mutex> lock(threadLock);
	HeliosDacDevice* dev = NULL;
	if (devNum < deviceList.size())
		dev = deviceList[devNum].get();
	lock.unlock();
	if (dev == NULL)
		return HELIOS_ERROR;

	return dev->EraseFirmware();
}


/// -----------------------------------------------------------------------
/// HELIOSDACDEVICE START
/// -----------------------------------------------------------------------

HeliosDac::HeliosDacDevice::HeliosDacDevice(libusb_device_handle* handle)
{
	closed = true;
	usbHandle = handle;
	std::this_thread::sleep_for(std::chrono::milliseconds(100));
	std::lock_guard<std::mutex>lock(frameLock);

	int actualLength = 0;
	//bool ok = false;
	std::uint8_t ctrlBuffer0[32];
	//catch any lingering transfers
	while (libusb_interrupt_transfer(usbHandle, EP_INT_IN, ctrlBuffer0, 32, &actualLength, 5) == LIBUSB_SUCCESS);

	//get firmware version
	firmwareVersion = 0;
	bool repeat = true;
	for (int i = 0; ((i < 2) && repeat); i++) //retry command if necessary
	{
		std::uint8_t ctrlBuffer[2] = { 0x04, 0 };
		int transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_OUT, ctrlBuffer, 2, &actualLength, 32);
		if ((transferResult == LIBUSB_SUCCESS) && (actualLength == 2))
		{
			for (int j = 0; ((j < 3) && repeat); j++) //retry response getting if necessary
			{
				std::uint8_t ctrlBuffer2[32];
				transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_IN, ctrlBuffer2, 32, &actualLength, 32);
				if (transferResult == LIBUSB_SUCCESS)
				{
					if (ctrlBuffer2[0] == 0x84)
					{
						firmwareVersion = ((ctrlBuffer2[1] << 0) |
							(ctrlBuffer2[2] << 8) |
							(ctrlBuffer2[3] << 16) |
							(ctrlBuffer2[4] << 24));
						repeat = false;
					}
				}
			}
		}
	}


	//send sdk firmware version
	repeat = true;
	for (int i = 0; ((i < 2) && repeat); i++) //retry command if necessary
	{
		std::uint8_t ctrlBuffer3[2] = { 0x07, HELIOS_SDK_VERSION };
		int transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_OUT, ctrlBuffer3, 2, &actualLength, 32);
		if ((transferResult == LIBUSB_SUCCESS) && (actualLength == 2))
			repeat = false;
	}

	closed = false;
}

//sends a raw frame buffer (implemented as bulk transfer) to a dac device
//returns 1 if success
int HeliosDac::HeliosDacDevice::SendFrame(std::uint8_t* bufferAddress, unsigned int bufferSize)
{
	if (closed)
		return HELIOS_ERROR;

	if (GetStatus() == 1)
	{
		std::thread statusHandlerThread(&HeliosDac::HeliosDacDevice::DoFrame, this, bufferAddress, bufferSize);
		statusHandlerThread.detach();
		return HELIOS_SUCCESS;
	}
	else
		return HELIOS_ERROR;
}

void HeliosDac::HeliosDacDevice::DoFrame(std::uint8_t* buffer, unsigned int bufferSize)
{
	std::lock_guard<std::mutex> lock(frameLock);

	//send frame
	int actualLength = 0;
	int transferResult = libusb_bulk_transfer(usbHandle, EP_BULK_OUT, buffer, bufferSize, &actualLength, 128);

	if ((transferResult == LIBUSB_SUCCESS) && (actualLength == bufferSize))
	{	
		//wait for status
		bool quit = false;
		while (!quit && !closed)
		{
			std::uint8_t ctrlBuffer[32] = { 0x03, 0 };
			if (SendControl(ctrlBuffer, 2) == HELIOS_SUCCESS)
			{
				std::uint8_t ctrlBuffer2[32];
				transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_IN, ctrlBuffer2, 32, &actualLength, 32);
				if (transferResult == LIBUSB_SUCCESS)
				{
					if (ctrlBuffer2[0] == 0x83) //STATUS ID
					{
						if (ctrlBuffer2[1] == 1)
						{
							quit = true; //lock will be freed on returning, effectively setting status to 1
						}
					}
				}
			}
		}
	}
	else //get status and then try again
	{
		bool quit = false;
		while (!quit && !closed)
		{
			std::uint8_t ctrlBuffer[32] = { 0x03, 0 };
			if (SendControl(ctrlBuffer, 2) == HELIOS_SUCCESS)
			{
				std::uint8_t ctrlBuffer2[32];
				transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_IN, ctrlBuffer2, 32, &actualLength, 32);
				if (transferResult == LIBUSB_SUCCESS)
				{
					if (ctrlBuffer2[0] == 0x83) //STATUS ID
					{
						if (ctrlBuffer2[1] == 1)
						{
							quit = true; //lock will be freed on returning, effectively setting status to 1
						}
					}
				}
			}
		}

		//send frame
		int actualLength = 0;
		int transferResult = libusb_bulk_transfer(usbHandle, EP_BULK_OUT, buffer, bufferSize, &actualLength, 256);

		if ((transferResult == LIBUSB_SUCCESS) && (actualLength == bufferSize))
		{
			//wait for status
			bool quit = false;
			while (!quit && !closed)
			{
				std::uint8_t ctrlBuffer[32] = { 0x03, 0 };
				if (SendControl(ctrlBuffer, 2) == HELIOS_SUCCESS)
				{
					std::uint8_t ctrlBuffer2[32];
					transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_IN, ctrlBuffer2, 32, &actualLength, 32);
					if (transferResult == LIBUSB_SUCCESS)
					{
						if (ctrlBuffer2[0] == 0x83) //STATUS ID
						{
							if (ctrlBuffer2[1] == 1)
							{
								quit = true; //lock will be freed on returning, effectively setting status to 1
							}
						}
					}
				}
			}
		}
	}
}

//Gets firmware version of DAC
int HeliosDac::HeliosDacDevice::GetFirmwareVersion()
{
	if (closed)
		return HELIOS_ERROR;

	return firmwareVersion;
}

//Gets firmware version of DAC
char* HeliosDac::HeliosDacDevice::GetName()
{
	if (closed)
		return "";

	std::lock_guard<std::mutex> lock(frameLock);

	bool gotName = false;
	
	bool repeat = true;
	for (int i = 0; ((i < 2) && repeat); i++) //retry command if necessary
	{
		int actualLength = 0;
		std::uint8_t ctrlBuffer4[32] = { 0x05, 0 };
		int transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_OUT, ctrlBuffer4, 2, &actualLength, 32);

		if ((transferResult == LIBUSB_SUCCESS) && (actualLength == 2))
		{
			for (int j = 0; ((j < 3) && repeat); j++) //retry response if necessary
			{
				std::uint8_t ctrlBuffer5[32];
				transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_IN, ctrlBuffer5, 32, &actualLength, 32);

				if (transferResult == LIBUSB_SUCCESS)
				{
					if (ctrlBuffer5[0] == 0x85)
					{
						ctrlBuffer5[31] = '\0';
						memcpy(name, &ctrlBuffer5[1], 31);
						gotName = true;
						repeat = false;
					}
				}
			}
		}
	}

	//if the above failed, fallback name:
	if (!gotName)
	{
		return "";
	}

	return name;
}

//Gets status of DAC, 1 means DAC is ready to receive frame, 0 means it's not
int HeliosDac::HeliosDacDevice::GetStatus()
{
	if (closed)
		return HELIOS_ERROR;

	std::unique_lock<std::mutex> lock(frameLock, std::try_to_lock);
	return lock.owns_lock();
}

//Set shutter level of DAc
int HeliosDac::HeliosDacDevice::SetShutter(bool level)
{
	if (closed)
		return HELIOS_ERROR;

	std::lock_guard<std::mutex> lock(frameLock);

	std::uint8_t txBuffer[2] = { 0x02, level };
	if (SendControl(txBuffer, 2) == HELIOS_SUCCESS)
		return HELIOS_SUCCESS;
	else
		return HELIOS_ERROR;
}

//Stops output of DAC
int HeliosDac::HeliosDacDevice::Stop()
{
	if (closed)
		return HELIOS_ERROR;

	std::lock_guard<std::mutex> lock(frameLock);

	std::uint8_t txBuffer[2] = { 0x01, 0 };
	if (SendControl(txBuffer, 2) == HELIOS_SUCCESS)
	{
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
		return HELIOS_SUCCESS;
	}
	else
		return HELIOS_ERROR;
}

//Sets persistent name of DAC
int HeliosDac::HeliosDacDevice::SetName(char* name)
{
	if (closed)
		return HELIOS_ERROR;

	std::lock_guard<std::mutex> lock(frameLock);

	std::uint8_t txBuffer[32] = { 0x06 };
	memcpy(&txBuffer[1], name, 30);
	txBuffer[31] = '\0';
	if (SendControl(txBuffer, 32) == HELIOS_SUCCESS)
		return HELIOS_SUCCESS;
	else
		return HELIOS_ERROR;
}

//Erases the firmware of the DAC, allowing it to be updated
int HeliosDac::HeliosDacDevice::EraseFirmware()
{
	if (closed)
		return HELIOS_ERROR;

	std::lock_guard<std::mutex> lock(frameLock);

	std::uint8_t txBuffer[2] = { 0xDE, 0 };
	if (SendControl(txBuffer, 2) == HELIOS_SUCCESS)
	{
		closed = true;
		return HELIOS_SUCCESS;
	}
	else
		return HELIOS_ERROR;
}

//sends a raw control signal (implemented as interrupt transfer) to a dac device
//returns 1 if successful
int HeliosDac::HeliosDacDevice::SendControl(std::uint8_t* bufferAddress, unsigned int length)
{
	if ((bufferAddress == NULL) || (length > 32))
		return HELIOS_ERROR;

	int actualLength = 0;
	int transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_OUT, bufferAddress, length, &actualLength, 16);

	if (transferResult == LIBUSB_SUCCESS)
	{
		if (actualLength == length)
			return HELIOS_SUCCESS;
		else
			return HELIOS_ERROR;
	}
	else
	{
		if ((transferResult == LIBUSB_ERROR_NO_DEVICE) || (transferResult == LIBUSB_ERROR_NOT_FOUND) || (transferResult == LIBUSB_ERROR_IO)) //critical errors
		{
			for (int i = 0; i < 5; i++) //try again a few times, if not successful, the device is probably plugged out so close it
			{
				int transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_OUT, bufferAddress, length, &actualLength, 16);
				if (transferResult == LIBUSB_SUCCESS)
				{
					if (actualLength == length)
						return HELIOS_SUCCESS;
					else
						return HELIOS_ERROR;
				}
				else
				{
					if ((transferResult != LIBUSB_ERROR_NO_DEVICE) && (transferResult != LIBUSB_ERROR_NOT_FOUND) && (transferResult != LIBUSB_ERROR_IO))
						return HELIOS_ERROR;
				}
			}
			closed = true;
			return HELIOS_ERROR;
		}

		//if another error (likely timeout), try again once just in case
		transferResult = libusb_interrupt_transfer(usbHandle, EP_INT_OUT, bufferAddress, length, &actualLength, 32);
		if ((transferResult == LIBUSB_SUCCESS) && (actualLength == length))
			return HELIOS_SUCCESS;
		else
			return HELIOS_ERROR;
	}
}

HeliosDac::HeliosDacDevice::~HeliosDacDevice()
{
	closed = true;
	std::lock_guard<std::mutex>lock(frameLock); //wait until all threads have closed

	libusb_close(usbHandle);
}

RobotSys.h

C/C++
The header file to the robot client.
#ifndef RobotSys_H
#define RobotSys_H

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <math.h>
#include <wiringSerial.h>

#define STEPS 200
#define PI 3.1415926535
//mm
#define RADIUS (28/2.0)




class RobotSys{
	
	double oldPL, oldPR, Base;
	int fd;
    	
	void initSerial();
	void sendCommand(int Ldir, int Lsteps, int Rdir, int Rsteps);
	
	public:
	RobotSys();
	~RobotSys();
	
	void setParameters(double PL, double PR, double base);
	void motorsAction(int x2go, int y2go, int same);
	float heightTriang(float a, float b, float c);
	
	char receiveCommand();
	
};
#endif

RobotSys.cpp

C/C++
The code for the robot client.
#include "RobotSys.h"




RobotSys::RobotSys()
{
	
	oldPL = 1;
   	oldPR = 1;
   	
   	Base =  1;  
	
   	initSerial();	
}


RobotSys::~RobotSys()
{
	serialClose(fd);
}



void RobotSys::setParameters(double PL, double PR, double base)
{
	oldPL = PL;
   	oldPR = PR;
   	Base = base;  
   	
}



void RobotSys::initSerial()
{
	
	
	fd = serialOpen("//dev//ttyACM0", 19200);
	
	if(fd < 1)  {
		std::cout << "No robot, no USB!!!" << std::endl;
		exit(1);
	}
	
}


void RobotSys::sendCommand(int Ldir, int Lsteps, int Rdir, int Rsteps)
{

    char buff[100];
    sprintf(buff, "M%d#%d#%d#%dE", Ldir, Lsteps, Rdir, Rsteps);

	serialPuts(fd, buff);
	
}


char RobotSys::receiveCommand()
{
	char r;
	
	if( (r = serialDataAvail(fd)) > 0 )
	{
		
		return serialGetchar(fd);
	}
	else
	{
		if(r==-1)
		      printf("Error on serialDataAvail (errno) = %d\n", errno);
		
		return -1;	
	}
}






void RobotSys::motorsAction(int x2go, int y2go, int same)
{
	
	    int b2 = Base - x2go;
	  
	    int  deltaPL, deltaPR;
	    int newPL, newPR;
	    int Ldir, Rdir;
	    int Lsteps, Rsteps;
	    
	    newPL = sqrt(x2go*x2go + y2go*y2go);
	    newPR = sqrt(b2*b2 + y2go*y2go);
	    
	    deltaPL = newPL - oldPL;
	    deltaPR = newPR - oldPR;
	    

	    if(deltaPL < 0)
	     {
		     deltaPL *= -1;
		     Ldir = 0;	    
		 }
		 else
		     Ldir = 1;
		     
		     
	     
		if(deltaPR < 0)
	     {
		     deltaPR *= -1;
		     Rdir = 1;	    
		 }
		 else
		     Rdir = 0;     
	    
	    
	    
	    Lsteps = deltaPL * STEPS / (2 * PI * RADIUS);
	    Rsteps = deltaPR * STEPS / (2 * PI * RADIUS);
	    
	    oldPL = newPL;
	    oldPR = newPR;
	     
	    sendCommand(Ldir, Lsteps, Rdir, Rsteps); 
}



float RobotSys::heightTriang(float p1, float p2, float base)
{
	
	float s = (p1+p2+base)/2.0;
	
	float a = sqrt( s*(s-p1)*(s-p2)*(s-base) );
	
	a = a/(0.5 * base);
	
	
   	return a;
}

WalabotSys.h

C/C++
The header for the walabotSys class.
#ifndef WalabotSys_H
#define WalabotSys_H

#define __LINUX__

#include <WalabotAPI.h>

#include <iostream>
#include <stdio.h>

#include <string>

#include <opencv2/opencv.hpp>
#include <opencv2/ml.hpp>

#include "aFeatures.h"


// 18 antennas on this board version
#define N_ANTENNAS 18

// for better generalization: considering 50 pairs of antennas
#define NPAIRS 50



using namespace cv;
using namespace signalFeatures;
using namespace cv::ml;



#define CHECK_WALABOT_RESULT(result, func_name)					\
{																\
	if (result != WALABOT_SUCCESS)								\
	{															\
		unsigned int extended = Walabot_GetExtendedError();	\
		const char* errorStr = Walabot_GetErrorString();		\
		std::cout << std::endl << "Error at " __FILE__ << ":"	\
                  << std::dec << __LINE__ << " - "				\
				  << func_name << " result is 0x" << std::hex	\
                  << result << std::endl;						\
																\
		std::cout << "Error string: " << errorStr << std::endl; \
																\
		std::cout << "Extended error: 0x" << std::hex			\
                  << extended << std::endl << std::endl;		\
																\
		std::cout << "Press enter to continue ...";				\
		std::string dummy;										\
		std::getline(std::cin, dummy);							\
		return;												\
	}															\
}



class WalabotSys{
	
	
	
	
	
	// --------------------
	// Variable definitions
	// --------------------
	

	AntennaPair *p;
	std::vector<AntennaPair> *pairsVector;
	int numPairs;

	// Walabot_GetStatus - output parameters
	//APP_STATUS appStatus;
	//double calibrationProcess; // Percentage of calibration completed, if status is STATUS_CALIBRATING

							   // Walabot_GetImagingTargets - output parameters
	ImagingTarget* targets;
	int numTargets;

	// Walabot_GetRawImageSlice - output parameters
	int*	rasterImage;
	int		sizeX;
	int		sizeY;
	double	sliceDepth;
	double	power;
	
	double zArenaMin;
	double zArenaMax;
	double zArenaRes;

	double xArenaMin;
	double xArenaMax;
	double xArenaRes;

	double yArenaMin;
	double yArenaMax;
	double yArenaRes;
	
	// matrix used by the Support-Vector-Machine
	Mat patterns;
	Mat labels;
	
	// pointer to the Support-Vector-Machine
	Ptr<SVM> svm;
	bool trained;
	
		
	
	void PrintImagingTargets(ImagingTarget*  targets, int numTargets);
	
	static int localWalabot_GetAntennaLocation(int a, double &x, double &y, double &z);
	
	static void antennasDistance(int a1, int a2, double &dist);
	
	static bool compAntennasLocAsc(AntennaPair p1, AntennaPair p2);
	
	static bool compAntennasLocDesc(AntennaPair p1, AntennaPair p2);
	
	void getTheCurrentPattern(std::vector<AntennaPair> vAnt, int nPairs, Mat &patt);
	
	public:
	WalabotSys();
	
	void classOneExample();
	void classTwoExample();
	void classThreeExample();
	
	
	void trainSVM();
	bool  svmTrained();
	int getSVMresponse();
	
	void triggerWalabot();
	
	
	
	void stopWalabot();
	
	
};



#endif

WalabotSys.cpp

C/C++
The code to get the signals from antennas, to train the SVM and to classify the signals.
#include "WalabotSys.h"
#include "antennasLoc.h"

WALABOT_RESULT res;

void WalabotSys::PrintImagingTargets(ImagingTarget*  targets, int numTargets)
{
	int targetIdx;

	system("clear");
	if (numTargets > 0)
	{
		for (targetIdx = 0; targetIdx < numTargets; targetIdx++)
		{
			printf("Target #%d:\n\t\t type %s,\n\t\t angleDeg = %lf,\n\t\t xPosCm = %lf,\n\t\t yPosCm = %lf,\n\t\t zPosCm = %lf,\n\t\t wPosCm = %lf,\n\t\t amplitude = %lf\n ",
				targetIdx,
				targets[targetIdx].type == TARGET_TYPE_PIPE ? "Pipe" : "Unknown",
				targets[targetIdx].angleDeg,
				targets[targetIdx].xPosCm,
				targets[targetIdx].yPosCm,
				targets[targetIdx].zPosCm,
				targets[targetIdx].widthCm,
				targets[targetIdx].amplitude);
		}
	}
	else
	{
		printf("No targets detected\n");
	}
}





int WalabotSys::localWalabot_GetAntennaLocation(int a, double &x, double &y, double &z)
{
	if(a>=1 && a<=N_ANTENNAS)
	{
	  x = antennasLoc[a-1][0];
	  y = antennasLoc[a-1][1];
	  z = antennasLoc[a-1][2];	
	  return 0;
	}
	else
	  return 14;
	
	}




void WalabotSys::antennasDistance(int a1, int a2, double &dist)
{
	double x1, y1, z1;
	res = (WALABOT_RESULT)localWalabot_GetAntennaLocation(a1, x1, y1, z1);
	CHECK_WALABOT_RESULT(res, "Walabot_GetAntennaPairs");

	double x2, y2, z2;
	res = (WALABOT_RESULT)localWalabot_GetAntennaLocation(a2, x2, y2, z2);
	CHECK_WALABOT_RESULT(res, "Walabot_GetAntennaPairs");

	dist = sqrt((x1-x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
}


bool WalabotSys::compAntennasLocAsc(AntennaPair p1, AntennaPair p2)
{

	double d1, d2;

	antennasDistance(p1.txAntenna, p1.rxAntenna, d1);

	antennasDistance(p2.txAntenna, p2.rxAntenna, d2);

	return (d1 < d2);

}


bool WalabotSys::compAntennasLocDesc(AntennaPair p1, AntennaPair p2)
{

	double d1, d2;

	antennasDistance(p1.txAntenna, p1.rxAntenna, d1);

	antennasDistance(p2.txAntenna, p2.rxAntenna, d2);

	return (d1 >= d2);

}



void WalabotSys::getTheCurrentPattern(std::vector<AntennaPair> vAnt, int nPairs, Mat &patt)
{
	int numS = 75;

	double *signal = new double[4096];
	double *timeAxis = new double[4096];
	
	Mat pattern;
	//pattern.create(nPairs*1, 1, CV_32F);

	//patt.create(1, nPairs * 1, CV_32F);

	for (int i = 0; i < nPairs; i++)
	{
		res = Walabot_GetSignal(vAnt[i].txAntenna, vAnt[i].rxAntenna, &signal, &timeAxis, &numS);
		CHECK_WALABOT_RESULT(res, "Walabot_GetSignal");

		//for (int h = 0; h < 100; h++)
		//printf("signal = %lf\n", signal[h]);

		//waitKey(0);

		AFeatures *aFeatures = new AFeatures();

		double f1, f2, f3, f4;

		aFeatures->getFeatures(signal, numS, f1, f2, f3, f4);

		pattern.push_back((float)f1);
		pattern.push_back((float)f2);
		pattern.push_back((float)f3);
		pattern.push_back((float)f4);

		

		delete aFeatures;
	}

	//printf("num pairs = %d num cols = %d num rows = %d \n", nPairs, pattern.cols, pattern.rows);

	Mat aux = pattern.t();

	

	aux.copyTo(patt);

}




WalabotSys::WalabotSys()
{

	// ------------------------
	// Initialize configuration
	// ------------------------
	double zArenaMin = 3;
	double zArenaMax = 8;
	double zArenaRes = 0.5;

	double xArenaMin = -4;
	double xArenaMax = 4;
	double xArenaRes = 0.5;

	double yArenaMin = -6;
	double yArenaMax = 4;
	double yArenaRes = 0.5;
	
	
	
	p = new AntennaPair[200];
	
	   
	        svm = StatModel::load<SVM>("SVM.xml");
	        trained = true;
	   
	      if( svm.empty() )
		  {
		    std::cout << "SVM is not trained!" << std::endl;
	        svm = SVM::create();
		    svm->setType(SVM::C_SVC);
		    svm->setKernel(SVM::RBF);
		    svm->setTermCriteria(TermCriteria(TermCriteria::MAX_ITER, 1000000, 1e-8));
		    trained = false;
	      }
		
	    
	    
	    
	// ----------------------
	// Sample Code Start Here
	// ----------------------

	/*
	For an image to be received by the application, the following need to happen :
	1) Connect
	2) Configure
	3) Calibrate
	4) Start
	5) Trigger
	6) Get action
	7) Stop/Disconnect
	*/

	// Configure Walabot database install location (for windows)
	res = Walabot_SetSettingsFolder((char *)"//var//lib//walabot//");
	CHECK_WALABOT_RESULT(res, "Walabot_SetSettingsFolder");

	//	1) Connect : Establish communication with Walabot.
	//	==================================================
	res = Walabot_ConnectAny();
	CHECK_WALABOT_RESULT(res, "Walabot_ConnectAny");

	//  2) Configure : Set scan profile and arena
	//	=========================================

	// Set Profile - short range . 
	//			Walabot recording mode is configure with the following attributes:
	//			-> Distance scanning through air; 
	//			-> lower - resolution images for a fast capture rate (useful for tracking quick movement)
	res = Walabot_SetProfile(PROF_SHORT_RANGE_IMAGING);
	CHECK_WALABOT_RESULT(res, "Walabot_SetProfile");

	// Set arena by Cartesian coordinates, with arena resolution :
	res = Walabot_SetArenaX(xArenaMin, xArenaMax, xArenaRes);
	CHECK_WALABOT_RESULT(res, "Walabot_SetArenaX");

	res = Walabot_SetArenaY(yArenaMin, yArenaMax, yArenaRes);
	CHECK_WALABOT_RESULT(res, "Walabot_SetArenaY");

	res = Walabot_SetArenaZ(zArenaMin, zArenaMax, zArenaRes);
	CHECK_WALABOT_RESULT(res, "Walabot_SetArenaZ");

	// Walabot filtering disable 
	res = Walabot_SetDynamicImageFilter(FILTER_TYPE_NONE);
	CHECK_WALABOT_RESULT(res, "Walabot_SetDynamicImageFilter");

	//	3) Start: Start the system in preparation for scanning.
	//	=======================================================
	res = Walabot_Start();
	CHECK_WALABOT_RESULT(res, "Walabot_Start");

	// calibrates scanning to ignore or reduce the signals
	res = Walabot_StartCalibration();
	CHECK_WALABOT_RESULT(res, "Walabot_StartCalibration");

	//	4) Trigger: Scan(sense) according to profile and record signals to be available
	//	for processing and retrieval.
	//	================================================================================
	

	
	res = Walabot_GetAntennaPairs(&p, &numPairs);
	CHECK_WALABOT_RESULT(res, "Walabot_GetAntennaPairs");

	/*
		for (int i = 0; i < nump; i++)
		{
			printf("pair[%d] = %d(tx), %d(rx)\n", i, p[i].txAntenna, p[i].rxAntenna);
		}
		*/
	//	double dist;
		//antDistance(2, 3, dist);
		//antDistance(8, 16, dist);
		//antDistance(3, 18, dist);


		pairsVector = new std::vector<AntennaPair>(p, p + numPairs);
		std::sort(pairsVector->begin(), pairsVector->end(), compAntennasLocDesc); // compAntennasLocAsc = dist ascendent ;   compAntennasLocDesc = dist descendent

		/*
		for (int i = 0; i < nump; i++)
		{
			printf("pair[%d] = %d(tx), %d(rx)\n", i, myvector[i].txAntenna, myvector[i].rxAntenna);
		}
		*/


	//trained = false;
		
	
	
}





void WalabotSys::classOneExample()
{
	        Mat pattern;
	
	        getTheCurrentPattern(*pairsVector, NPAIRS, pattern);
			Mat patternN;
			normalize(pattern, patternN, -1, 1, NORM_MINMAX, CV_32F);
			patterns.push_back(patternN);

			

			Mat line = Mat(1, 1, CV_32S);
			line.at<int>(0, 0) = 0;

			labels.push_back(line);
			printf("new sample stored!");
}



void WalabotSys::classTwoExample()
{
	
	        Mat pattern;
	        
	        getTheCurrentPattern(*pairsVector, NPAIRS, pattern);
			Mat patternN;
			normalize(pattern, patternN, -1, 1,NORM_MINMAX, CV_32F);
			patterns.push_back(patternN);

			Mat line = Mat(1, 1, CV_32S);
			line.at<int>(0, 0) = 1;

			labels.push_back(line);
		
			printf("new sample stored!");
}


void WalabotSys::classThreeExample()
{
	
	        Mat pattern;
	
	        getTheCurrentPattern(*pairsVector, NPAIRS, pattern);
			Mat patternN;
			normalize(pattern, patternN, -1, 1, NORM_MINMAX, CV_32F);
			patterns.push_back(patternN);

			Mat line = Mat(1, 1, CV_32S);
			line.at<int>(0, 0) = 2;

			labels.push_back(line);

			printf("new sample stored!");
}




void WalabotSys::trainSVM()  // save and load the data??????????????
{
	    
	    

		Ptr<ml::TrainData> trainData = ml::TrainData::create(patterns, ml::ROW_SAMPLE, labels);
			

		std::cout << "Starting the SVM learning..." << std::endl;
		svm->trainAuto(trainData, 2);

		//svm->train(patterns, ROW_SAMPLE, labels);
		
		svm->save("SVM.xml");
		
		printf("Trained!");
		trained = true;
	
}



bool WalabotSys::svmTrained()
{
	return trained;
}




int WalabotSys::getSVMresponse()
{
	Mat pattern;
	getTheCurrentPattern(*pairsVector, NPAIRS, pattern);
			Mat patternN;
			normalize(pattern, patternN, -1, 1, NORM_MINMAX, CV_32F);
			Mat res;   // output
			float r = svm->predict(patternN, res, StatModel::RAW_OUTPUT);
			printf("CLASS=%f r = %f \n", res.at<float>(0, 0), r);

			if (res.at<float>(0, 0) < 0.6)
			{
				printf("class 1\n");
				return 1;
			}
			else if (res.at<float>(0, 0) <1.5)
			{
				printf("class 2\n");
				return 2;
			}
			else
				if (res.at<float>(0, 0) <2.5)
				{
					printf("class 3\n");
					return 3;
	            }
	            
	            return 0;
}




void WalabotSys::triggerWalabot()
{
	
        // Trigger: Scan(sense) according to profile and record signals to be 
		//	available for processing and retrieval.
		//	====================================================================
		res = Walabot_Trigger();
		CHECK_WALABOT_RESULT(res, "Walabot_Trigger");

		//	Get action : retrieve the last completed triggered recording 
		//	================================================================
	/* res = Walabot_GetImagingTargets(&targets, &numTargets);
	CHECK_WALABOT_RESULT(res, "Walabot_GetImagingTargets");

	res = Walabot_GetRawImageSlice(&rasterImage, &sizeX, &sizeY, &sliceDepth, &power);
		CHECK_WALABOT_RESULT(res, "Walabot_GetRawImageSlice");

		//	******************************
		//	TODO: add processing code here
		//	******************************
		PrintImagingTargets(targets, numTargets);
////////////////////////
		Mat t = Mat(sizeY, sizeX, CV_32S, rasterImage);

		t = abs(t);

		t.convertTo(t, CV_8U);

		resize(t, t, Size(200,200));
		imshow("RawImageSlice", t);	
	*/
}





void WalabotSys::stopWalabot()
{
	//  Stop and Disconnect.
	//	======================
	res = Walabot_Stop();
	CHECK_WALABOT_RESULT(res, "Walabot_Stop");

	res = Walabot_Disconnect();
	CHECK_WALABOT_RESULT(res, "Walabot_Disconnect");
	
}

mpr121.h

C/C++
Header file for touch keypad.
/*
Copyright (c) 2011 Anthony Buckton (abuckton [at] blackink [dot} net {dot} au)

 
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
 
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.

   Parts written by Jim Lindblom of Sparkfun
   Ported to mbed by A.Buckton, Feb 2011
*/

#ifndef MPR121_H
#define MPR121_H

//using namespace std;

class Mpr121 
{

public:
    // i2c Addresses, bit-shifted
    enum Address { ADD_VSS = 0xb4,// ADD->VSS = 0x5a <-wiring on Sparkfun board
                   ADD_VDD = 0xb6,// ADD->VDD = 0x5b
                   ADD_SCL = 0xb8,// ADD->SDA = 0x5c
                   ADD_SDA = 0xba // ADD->SCL = 0x5d
                 };

    // Real initialiser, takes the i2c address of the device.
    Mpr121(I2C *i2c, Address i2cAddress);
    
    bool getProximityMode();
    
    void setProximityMode(bool mode);
    
    int readTouchData();
               
    unsigned char read(int key);
    
    int write(int address, unsigned char value);
    int writeMany(int start, unsigned char* dataSet, int length);

    void setElectrodeThreshold(int electrodeId, unsigned char touchThreshold, unsigned char releaseThreshold);
        
protected:
    // Configures the MPR with standard settings. This is permitted to be overwritten by sub-classes.
    void configureSettings();
    
private:
    // The I2C bus instance.
    I2C *i2c;

    // i2c address of this mpr121
    Address address;
};


// MPR121 Register Defines
#define    MHD_R        0x2B
#define    NHD_R        0x2C
#define    NCL_R        0x2D
#define    FDL_R        0x2E
#define    MHD_F        0x2F
#define    NHD_F        0x30
#define    NCL_F        0x31
#define    FDL_F        0x32
#define    NHDT         0x33
#define    NCLT         0x34
#define    FDLT         0x35
// Proximity sensing controls
#define    MHDPROXR     0x36
#define    NHDPROXR     0x37
#define    NCLPROXR     0x38
#define    FDLPROXR     0x39
#define    MHDPROXF     0x3A
#define    NHDPROXF     0x3B
#define    NCLPROXF     0x3C
#define    FDLPROXF     0x3D
#define    NHDPROXT     0x3E
#define    NCLPROXT     0x3F
#define    FDLPROXT     0x40
// Electrode Touch/Release thresholds
#define    ELE0_T       0x41
#define    ELE0_R       0x42
#define    ELE1_T       0x43
#define    ELE1_R       0x44
#define    ELE2_T       0x45
#define    ELE2_R       0x46
#define    ELE3_T       0x47
#define    ELE3_R       0x48
#define    ELE4_T       0x49
#define    ELE4_R       0x4A
#define    ELE5_T       0x4B
#define    ELE5_R       0x4C
#define    ELE6_T       0x4D
#define    ELE6_R       0x4E
#define    ELE7_T       0x4F
#define    ELE7_R       0x50
#define    ELE8_T       0x51
#define    ELE8_R       0x52
#define    ELE9_T       0x53
#define    ELE9_R       0x54
#define    ELE10_T      0x55
#define    ELE10_R      0x56
#define    ELE11_T      0x57
#define    ELE11_R      0x58
// Proximity Touch/Release thresholds
#define    EPROXTTH     0x59
#define    EPROXRTH     0x5A
// Debounce configuration
#define    DEB_CFG      0x5B
// AFE- Analogue Front End configuration
#define    AFE_CFG      0x5C 
// Filter configuration
#define    FIL_CFG      0x5D
// Electrode configuration - transistions to "active mode"
#define    ELE_CFG      0x5E

#define GPIO_CTRL0      0x73
#define GPIO_CTRL1      0x74
#define GPIO_DATA       0x75
#define    GPIO_DIR     0x76
#define    GPIO_EN      0x77
#define    GPIO_SET     0x78
#define GPIO_CLEAR      0x79
#define GPIO_TOGGLE     0x7A
// Auto configration registers
#define    AUTO_CFG_0   0x7B
#define    AUTO_CFG_U   0x7D
#define    AUTO_CFG_L   0x7E
#define    AUTO_CFG_T   0x7F

// Threshold defaults
// Electrode touch threshold
#define    E_THR_T      0x0F + 5   
// Electrode release threshold 
#define    E_THR_R      0x0A + 5   
// Prox touch threshold
#define    PROX_THR_T   0x02
// Prox release threshold
#define    PROX_THR_R   0x02

#endif

mpr121.cpp

C/C++
The code for touch keypad.
/*
Copyright (c) 2011 Anthony Buckton (abuckton [at] blackink [dot} net {dot} au)
 
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
 
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <mbed.h>
#include <sstream>
#include <string>
#include <list>

#include <mpr121.h>
    
Mpr121::Mpr121(I2C *i2c, Address i2cAddress)
{
    this->i2c = i2c;
    
    address = i2cAddress;
           
    // Configure the MPR121 settings to default
    this->configureSettings();
}

   
void Mpr121::configureSettings()
{
    // Put the MPR into setup mode
    this->write(ELE_CFG,0x00);
    
    // Electrode filters for when data is > baseline
    unsigned char gtBaseline[] = {
         0x01,  //MHD_R
         0x01,  //NHD_R 
         0x00,  //NCL_R
         0x00   //FDL_R
         };
         
    writeMany(MHD_R,gtBaseline,4);   
                 
     // Electrode filters for when data is < baseline   
     unsigned char ltBaseline[] = {
        0x01,   //MHD_F
        0x01,   //NHD_F
        0xFF,   //NCL_F
        0x02    //FDL_F
        };
        
    writeMany(MHD_F,ltBaseline,4);
        
    // Electrode touch and release thresholds
    unsigned char electrodeThresholds[] = {
        E_THR_T, // Touch Threshhold
        E_THR_R  // Release Threshold
        };

    for(int i=0; i<12; i++){
        int result = writeMany((ELE0_T+(i*2)),electrodeThresholds,2);
    }   

    // Proximity Settings
    unsigned char proximitySettings[] = {
        0xff,   //MHD_Prox_R
        0xff,   //NHD_Prox_R
        0x00,   //NCL_Prox_R
        0x00,   //FDL_Prox_R
        0x01,   //MHD_Prox_F
        0x01,   //NHD_Prox_F
        0xFF,   //NCL_Prox_F
        0xff,   //FDL_Prox_F
        0x00,   //NHD_Prox_T
        0x00,   //NCL_Prox_T
        0x00    //NFD_Prox_T
        };
    writeMany(MHDPROXR,proximitySettings,11);

    unsigned char proxThresh[] = {
        PROX_THR_T, // Touch Threshold
        PROX_THR_R  // Release Threshold
        };
    writeMany(EPROXTTH,proxThresh,2); 
       
    this->write(FIL_CFG,0x34); // for 10 samples in Filter 2   
    
    // Set the electrode config to transition to active mode
    this->write(ELE_CFG,0x0c);
    
    this->write(AFE_CFG,0x90); // Filter 1: 18 samples and 16uA
    
    
    this->write(DEB_CFG,0x21); // Debounce: T 2x R 1x
}

void Mpr121::setElectrodeThreshold(int electrode, unsigned char touch, unsigned char release){
    
    if(electrode > 11) return;
    
    // Get the current mode
    unsigned char mode = this->read(ELE_CFG);
    
    // Put the MPR into setup mode
    this->write(ELE_CFG,0x00);
    
    // Write the new threshold
    this->write((ELE0_T+(electrode*2)), touch);
    this->write((ELE0_T+(electrode*2)+1), release);
    
    //Restore the operating mode
    this->write(ELE_CFG, mode);
    
    
    
}
    
    
unsigned char Mpr121::read(int key){

    unsigned char data[2];
    
    //Start the command
    i2c->start();

    // Address the target (Write mode)
    int ack1= i2c->write(address);

    // Set the register key to read
    int ack2 = i2c->write(key);

    // Re-start for read of data
    i2c->start();

    // Re-send the target address in read mode
    int ack3 = i2c->write(address+1);

    // Read in the result
    data[0] = i2c->read(0); 

    // Reset the bus        
    i2c->stop();

    return data[0];
}


int Mpr121::write(int key, unsigned char value){
    
    //Start the command
    i2c->start();

    // Address the target (Write mode)
    int ack1= i2c->write(address);

    // Set the register key to write
    int ack2 = i2c->write(key);

    // Read in the result
    int ack3 = i2c->write(value); 

    // Reset the bus        
    i2c->stop();
    
    return (ack1+ack2+ack3)-3;
}


int Mpr121::writeMany(int start, unsigned char* dataSet, int length){
    //Start the command
    i2c->start();

    // Address the target (Write mode)
    int ack= i2c->write(address);
    if(ack!=1){
        return -1;
    }
    
    // Set the register key to write
    ack = i2c->write(start);
    if(ack!=1){
        return -1;
    }

    // Write the date set
    int count = 0;
    while(ack==1 && (count < length)){
        ack = i2c->write(dataSet[count]);
        count++;
    } 
    // Stop the cmd
    i2c->stop();
    
    return count;
}
      

bool Mpr121::getProximityMode(){
    if(this->read(ELE_CFG) > 0x0c)
        return true;
    else
        return false;
}

void Mpr121::setProximityMode(bool mode){
    this->write(ELE_CFG,0x00);
    if(mode){
        this->write(ELE_CFG,0x30); //Sense proximity from ALL pads
    } else {
        this->write(ELE_CFG,0x0c); //Sense touch, all 12 pads active.
    }
}


int Mpr121::readTouchData(){
    return this->read(0x00);
}

Laser.h

C/C++
This is the header to the laserSys class.
#include "HeliosDac.h"
#include <opencv2/opencv.hpp>

using namespace cv;

class Laser
{
	HeliosDac helios;
	int numDevices;
	
	public:
	Laser();
	~Laser();
	
	void projectDetection(Mat imgC2, Mat imgC3);
	void projectSquare(Mat imgC2);
	void projectorStop();

	
};

Laser.cpp

C/C++
laserSys class
#include "Laser.h"
#include <iostream>


#define LaserMaxWidth 3000


//connect to DACs and output frames
Laser::Laser()
{

   numDevices = helios.OpenDevices();
   
   if(numDevices < 1)
      std::cout << "No Laser!" << std::endl;
      
      std::cout << "Num of Laser devices:" << numDevices<< std::endl;
}


void Laser::projectorStop()
{
	helios.Stop(0);
}


void Laser::projectSquare(Mat imgC2)
{
	
	int nRowsC2 = imgC2.rows;
	int nColsC2 = imgC2.cols;
	
	
	
	float ratio = nColsC2/nRowsC2;
	
	int LaserMaxHeight = LaserMaxWidth / ratio;
	
	HeliosPoint frame[1][5];
	
	
	
		frame[0][0].x = 0;
	   
		frame[0][0].y = 0;
		frame[0][0].r = 0;
		frame[0][0].g = 0;
		frame[0][0].b = 0xFF;
		frame[0][0].i = 0x00;
		
		frame[0][1].x = 0;
	   	frame[0][1].y = LaserMaxHeight;
		frame[0][1].r = 0;
		frame[0][1].g = 0;
		frame[0][1].b = 0xFF;
		frame[0][1].i = 0x00;
		
		frame[0][2].x = LaserMaxWidth;
	   	frame[0][2].y = LaserMaxHeight;
		frame[0][2].r = 0;
		frame[0][2].g = 0;
		frame[0][2].b = 0xFF;
		frame[0][2].i = 0x00;
		
		frame[0][3].x = LaserMaxWidth;
	   	frame[0][3].y = 0;
		frame[0][3].r = 0;
		frame[0][3].g = 0;
		frame[0][3].b = 0xFF;
		frame[0][3].i = 0x00;
		
		frame[0][4].x = 0;
		frame[0][4].y = 0;
		frame[0][4].r = 0;
		frame[0][4].g = 0;
		frame[0][4].b = 0xFF;
		frame[0][4].i = 0x80;
		
		helios.WriteFrame(0, 100, 0, &frame[0][0], 5);
		
	
	}



void Laser::projectDetection(Mat imgC2, Mat imgC3)
{
	
	int nRowsC2 = imgC2.rows;
	int nColsC2 = imgC2.cols;
	
	float ratio = nColsC2/nRowsC2;
	
	int LaserMaxHeight = LaserMaxWidth / ratio;
	
	
	
	Mat colorC2 = Mat::zeros(nRowsC2, nColsC2, CV_8UC3);
	Mat colorC3 = Mat::zeros(nRowsC2, nColsC2, CV_8UC3);
	
	std::vector<Vec4i> lines;
	std::vector<Vec4i> lines2;
    
    HoughLinesP( imgC2, lines, 1, CV_PI/360.0, 7, 8, 2 );
    HoughLinesP( imgC3, lines2, 1, CV_PI/360.0, 7, 8, 2 );
    
    
    HeliosPoint frame[1][10000];
    
    
    
    int l;
    int x1;
    int y1;
    int x2;
    int y2;
    int xlast;
    int ylast;
    int min;
    int max;
    
    
    int f = 0;
   
    
    f=0;
 
    {
		
		
    for( size_t i = 0; i < lines.size();i++ )
    {
		
			
		x1 = (int)(-(LaserMaxWidth/nColsC2)*lines[i][0]+LaserMaxWidth);
		y1 = (int)(-(LaserMaxHeight/nRowsC2)*lines[i][1]+LaserMaxHeight);
				
		x2 = (int)(-(LaserMaxWidth/nColsC2)*lines[i][2]+LaserMaxWidth);
		y2 = (int)(-(LaserMaxHeight/nRowsC2)*lines[i][3]+LaserMaxHeight);
		
				
		
		if (x1 > x2){
			max = x1;
			min = x2;
			}
			else
			{
				max = x2;
				min = x1;
				}
		if (min != max)
		{
		for (l=min;l<=max;l++)
		{
			
		
		   frame[0][f].x = l;
		   
	   
		   frame[0][f].y = (int)((y2-y1)/(x2-x1)*(l-x1)+y1);
		   frame[0][f].r = 0;
		   frame[0][f].g = 0xFF;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x00;
		   f++;
		   
		   
	   
	    }
	}
	else //vertical line
	{
		if (y2 > y1)
		{
			max = y2;
			min = y1;
			}
			else
			{
				max = y1;
				min = y2;
				}
		for (l=min;l<=max;l++)
		{
			
		
		   frame[0][f].x = x2;
		   
	   
		   frame[0][f].y = l;
		   frame[0][f].r = 0;
		   frame[0][f].g = 0xFF;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x00;
		   f++;
		   
		   
	   
	    }
		
		}
	   
	   //Stop the laser between segments with gaps;
	   
	   if (i+1 < lines.size())
	   {
		  x2 = (int)(-(LaserMaxWidth/nColsC2)*lines[i+1][0]+LaserMaxWidth);
		  y2 = (int)(-(LaserMaxHeight/nRowsC2)*lines[i+1][1]+LaserMaxHeight);
		  
		  xlast = frame[0][f-1].x;
		  ylast = frame[0][f-1].y;
		  
		  
		  
		  if (xlast > x2){
			max = xlast;
			min = x2;
			}
			else
			{
				max = x2;
				min = xlast;
				}

		if (min != max)
		{
		 
		 for (l=min;l<=max;l++)
		{
		   frame[0][f].x = l;
		   
		   frame[0][f].y = (int)((y2-ylast)/(x2-xlast)*(l-xlast)+ylast);
		   frame[0][f].r = 0;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x40;
		   f++;
		   
	    }
	  }
	  else //vertical line
	  {
		  if (y2 > ylast)
			{
			max = y2;
			min = ylast;
			}
			else
			{
				max = ylast;
				min = y2;
				}
		
		for (l=min;l<=max;l++)
		{
			
		
		   frame[0][f].x = x2;
		   
	   
		   frame[0][f].y = l;
		   frame[0][f].r = 0;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x40;
		   f++;
		   
		   
	   
	    }
		  
		  }
  }
	  
	  //Go to frame begin and mark last point of the frame
	  
	  if (i+1 == lines.size())
	  {
		x2 = frame[0][f-1].x;
		x1 = frame[0][0].x;
		
		y2 = frame[0][f-1].y;
		y1 = frame[0][0].y;
		
		if (x2 == x1)
		{
			if (y2 > y1)
			{
				max = y2;
				min = y1;
				}
				else
				{
					max = y1;
					min = y2;
					}
			for (l=min;l<=max;l++)
			{
				frame[0][f].y = l;
				frame[0][f].x = x1;
				frame[0][f].r = 0;
				frame[0][f].g = 0;
				frame[0][f].b = 0;
				frame[0][f].i = 0x40;
				f++;
				}
			}
			else
			{
				if (x2 > x1)
				{
				max = x2;
				min = x1;
				}
				else
				{
					max = x1;
					min = x2;
					}
			for (l=min;l<=max;l++)
			{
				frame[0][f].y = (int)((y2-y1)/(x2-x1)*(l-x1)+y1);
				frame[0][f].x = l;
				frame[0][f].r = 0;
				frame[0][f].g = 0;
				frame[0][f].b = 0;
				frame[0][f].i = 0x40;
				f++;
				}
				
			}
		
	  if (lines2.size() == 0)
	  {
		frame[0][f].y = frame[0][f-1].y;
		frame[0][f].x = frame[0][f-1].x;
		frame[0][f].r = 0;
		frame[0][f].g = 0;
		frame[0][f].b = 0;
		frame[0][f].i = 0x80;
		
		
		
		  }
		  
		  else
		  {
			   x2 = (int)(-(LaserMaxWidth/nColsC2)*lines2[0][0]+LaserMaxWidth);
			   y2 = (int)(-(LaserMaxHeight/nRowsC2)*lines2[0][1]+LaserMaxHeight);
		  
		       xlast = frame[0][f-1].x;
		       ylast = frame[0][f-1].y;
		       
		       
		   if (xlast > x2){
			max = xlast;
			min = x2;
			}
			else
			{
				max = x2;
				min = xlast;
				}

		if (min != max)
		{
		 
		 for (l=min;l<=max;l++)
		{
		   frame[0][f].x = l;
		   
		   frame[0][f].y = (int)((y2-ylast)/(x2-xlast)*(l-xlast)+ylast);
		   frame[0][f].r = 0;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x40;
		   f++;
		   
	    }
	  }
	  else //vertical line
	  {
		  if (y2 > ylast)
			{
			max = y2;
			min = ylast;
			}
			else
			{
				max = ylast;
				min = y2;
				}
		
		for (l=min;l<=max;l++)
		{
			
		
		   frame[0][f].x = x2;
		   
	   
		   frame[0][f].y = l;
		   frame[0][f].r = 0;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x40;
		   f++;
		   
		   
	   
	    }
		  
		  }
		       
			  
			  }
		
		
		
		
		
		
		//k=k+2;
		
        line( colorC2, Point(lines[i][0], lines[i][1]), Point(lines[i][2], lines[i][3]), Scalar(0,0,255), 1, 8 );
    }
}
}
    
    
     for( size_t i = 0; i < lines2.size(); i++ )
    {
		
			
		x1 = (int)(-(LaserMaxWidth/nColsC2)*lines2[i][0]+LaserMaxWidth);
		y1 = (int)(-(LaserMaxHeight/nRowsC2)*lines2[i][1]+LaserMaxHeight);
				
		x2 = (int)(-(LaserMaxWidth/nColsC2)*lines2[i][2]+LaserMaxWidth);
		y2 = (int)(-(LaserMaxHeight/nRowsC2)*lines2[i][3]+LaserMaxHeight);
		
				
		
		if (x1 > x2){
			max = x1;
			min = x2;
			}
			else
			{
				max = x2;
				min = x1;
				}
		if (min != max)
		{
		for (l=min;l<=max;l++)
		{
			
		
		   frame[0][f].x = l;
		   
	   
		   frame[0][f].y = (int)((y2-y1)/(x2-x1)*(l-x1)+y1);
		   frame[0][f].r = 0xFF;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x00;
		   f++;
		   
		   
	   
	    }
	}
	else //vertical line
	{
		if (y2 > y1)
		{
			max = y2;
			min = y1;
			}
			else
			{
				max = y1;
				min = y2;
				}
		for (l=min;l<=max;l++)
		{
			
		
		   frame[0][f].x = x2;
		   
	   
		   frame[0][f].y = l;
		   frame[0][f].r = 0xFF;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x00;
		   f++;
		   
		   
	   
	    }
		
		}
	   
	   //Stop the laser between segments with gaps;
	   
	   if (i+1 < lines.size())
	   {
		  x2 = (int)(-(LaserMaxWidth/nColsC2)*lines2[i+1][0]+LaserMaxWidth);
		  y2 = (int)(-(LaserMaxHeight/nRowsC2)*lines2[i+1][1]+LaserMaxHeight);
		  
		  xlast = frame[0][f-1].x;
		  ylast = frame[0][f-1].y;
		  
		  
		  
		  if (xlast > x2){
			max = xlast;
			min = x2;
			}
			else
			{
				max = x2;
				min = xlast;
				}

		if (min != max)
		{
		 
		 for (l=min;l<=max;l++)
		{
		   frame[0][f].x = l;
		   
		   frame[0][f].y = (int)((y2-ylast)/(x2-xlast)*(l-xlast)+ylast);
		   frame[0][f].r = 0;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x40;
		   f++;
		   
	    }
	  }
	  else //vertical line
	  {
		  if (y2 > ylast)
			{
			max = y2;
			min = ylast;
			}
			else
			{
				max = ylast;
				min = y2;
				}
		
		for (l=min;l<=max;l++)
		{
			
		
		   frame[0][f].x = x2;
		   
	   
		   frame[0][f].y = l;
		   frame[0][f].r = 0;
		   frame[0][f].g = 0;
		   frame[0][f].b = 0;
		   frame[0][f].i = 0x40;
		   f++;
		   
		   
	   
	    }
		  
		  }
  }
	  
	  //Go to frame begin and mark last point of the frame
	  
	  if (i+1 == lines.size())
	  {
		x2 = frame[0][f-1].x;
		x1 = frame[0][0].x;
		
		y2 = frame[0][f-1].y;
		y1 = frame[0][0].y;
		
		if (x2 == x1)
		{
			if (y2 > y1)
			{
				max = y2;
				min = y1;
				}
				else
				{
					max = y1;
					min = y2;
					}
			for (l=min;l<=max;l++)
			{
				frame[0][f].y = l;
				frame[0][f].x = x1;
				frame[0][f].r = 0;
				frame[0][f].g = 0;
				frame[0][f].b = 0;
				frame[0][f].i = 0x40;
				f++;
				}
			}
			else
			{
				if (x2 > x1)
				{
				max = x2;
				min = x1;
				}
				else
				{
					max = x1;
					min = x2;
					}
			for (l=min;l<=max;l++)
			{
				frame[0][f].y = (int)((y2-y1)/(x2-x1)*(l-x1)+y1);
				frame[0][f].x = l;
				frame[0][f].r = 0;
				frame[0][f].g = 0;
				frame[0][f].b = 0;
				frame[0][f].i = 0x40;
				f++;
				}
				
			}
		
	
		frame[0][f].y = frame[0][f-1].y;
		frame[0][f].x = frame[0][f-1].x;
		frame[0][f].r = 0;
		frame[0][f].g = 0;
		frame[0][f].b = 0;
		frame[0][f].i = 0x80;
		
		
		  }
		
		
		
        line( colorC3, Point(lines2[i][0], lines2[i][1]), Point(lines2[i][2], lines2[i][3]), Scalar(0,0,255), 1, 8 );
    }
    
    waitKey(100);
	
	
		if(lines.size() != 0 || lines2.size() != 0)
			{
				
				helios.WriteFrame(0, 50000, 0, &frame[0][0], f);
				
				
			}
	
	
	imshow("C2 Line Detection", colorC2);
	imshow("C3 Line Detection", colorC3);
	
	
}




Laser::~Laser()
{
  helios.CloseDevices();	
}

W2B.cpp

C/C++
The entry file.
#include "CameraSys.h"
#include "RobotSys.h"
#include "WalabotSys.h"
#include "Laser.h"
#include <unistd.h>
#include <iostream> 
#include <sys/ioctl.h> 
#include <fcntl.h> 
#include <linux/kd.h> 


#include <sys/resource.h>

void beep()
{ 
  int freq = 2000; // freq in hz 
  int len = 250; // len in ms 

  int fd = open("//dev//snd//pcmC0D1p", O_WRONLY); 
  ioctl(fd, KIOCSOUND, (int)(1193180/freq)); 
  usleep(1000*len); 
  ioctl(fd, KIOCSOUND, 0); 
  close(fd); 
}  


int maxABC(int a, int b, int c){

  int max = a;
  int i=1;
  
  if(b > max)
  {
      max = b;
      i = 2;
   }   
   
  if(c > max)
  {
      max = c;
      i=3;    
  }
  
  if(a == b && a == c)
    return 1;
    
  return i;
}




int main()
{
 
  setpriority(PRIO_PROCESS, 0, -15);
 
 
  Laser *ls = new Laser();
 
  CameraSys *cs = new CameraSys();
  
  RobotSys * rs = new RobotSys();
  
  WalabotSys * ws = new WalabotSys();
  
  
  
  
  
  int PL=0, PR=0, Base=0;
  

  int ch = -1;
  
  namedWindow("window");
  
  
  char cmdTouch;
  while(1)
  {
	  
	  
	ws->triggerWalabot();  
	  
	cmdTouch = rs->receiveCommand();
	
	printf("CMDTOUCH = %c \n", cmdTouch);
	  
	if (ch == '1' || cmdTouch == '1') // class one
		{
		   ws->classOneExample();
		}

		if (ch == '2' || cmdTouch == '2') // class two
		{
			ws->classTwoExample(); 
		}



		if (ch == '3' || cmdTouch == '3') // class three
		{
			ws->classThreeExample();
			
		}




		if (ch == 't' || cmdTouch == '7' )
		{
			// Train the SVM
			ws->trainSVM();	
		
		}


		if (ws->svmTrained())
		{		
            ws->getSVMresponse();		
		}  
		
		
		cs->capFrame();
		
		
		if( ch == 'c' || cmdTouch == '8')
		{
		   if( cs->getTrueTriangDistances(PL, PR, Base) == 1 )
		   {
		   
		   
		   rs->setParameters(PL, PR, Base);	
		   
		   
	       }
	       else
	       std::cout << "Calibration failed!" << std::endl; 
		}
		
		
		if(ch=='s' || cmdTouch == '9')
		{
		 
		     
		     printf("PL PR B: %d _ %d _ %d \n", PL, PR, Base);
		         
		    int Height = (int)rs->heightTriang((float)PL, (float)PR, (float)Base);
		    
		    
		    printf("Height/Base: %d - %d\n",Height,Base);
		    
		    
		    Mat outImageC2 = Mat::zeros(Height/10+1, Base/10+1, CV_8U);
		    Mat outImageC3 = Mat::zeros(Height/10+1, Base/10+1, CV_8U);
		    Mat map = Mat::zeros(Height/10+1, Base/10+1, CV_8U);
		    
		    int j, hInc;
		    
		    int vInc = 30; // mm
		    int horizOffset = 220; // mm
		    int vertiOffset =370; // mm
		    
		    int c=2;
		    j = 0 + horizOffset;
		    for(int i=vertiOffset; i<Height; i += vInc)  // vertical inc
		    {
				
				    if(i>vertiOffset)
				    {
				
				    rs->motorsAction(j, i, 0);
				    
				    
				     printf("j=%d i=%d \n", j, i);
				    
				    map.at<uchar>(i/10, j/10) = 255;
				    
				    
				    imshow("map", map);
				    
				    waitKey(30);
				    
				    
				  
				   
				    char ch;
				    do{
				         ch = rs->receiveCommand();
				         waitKey(30);
				    }
				    while(ch != 'D'&& ch != '5');
				    
				    
				    
				    if(ch == '5')
				    {
						std::cout << "STOP!"<< std::endl;
						
						break;
						}

				    
				    
				    waitKey(1000);
				    ws->triggerWalabot();
				    waitKey(1000);
				    ws->triggerWalabot(); 
				    waitKey(1000);
				    ws->triggerWalabot();   
				    
				    if(ws->svmTrained())
		            {		
                        
                        int r = ws->getSVMresponse();
                        if(r == 2){
                            outImageC2.at<uchar>(i/10,j/10) = 255;
                            beep();
						}
                        else
                            if(r == 3){
                               outImageC3.at<uchar>(i/10,j/10) = 255;
                               beep();
                               beep();
						   }
                        		
		            }
		            
				    }
				
				
				
				if(i>vertiOffset)
				     c++;
				
				
				if(c%2 == 0) // alternating the horizontal scanning
				{
					//j = 0 + horizOffset;
					hInc = 10; // horizontal inc  mm
				}
				else
				{
					//j = Base -1 - horizOffset;
					hInc = -10;  // horizontal inc  mm
				}
				
				
				
		      for( ; j>=horizOffset && j<Base - horizOffset; j += hInc)
		      {
				    
				   // if(i>vertiOffset)
				   // rs->motorsAction(j, i, 1);
				    //else
				    rs->motorsAction(j, i, 0);
				    
				    printf("j=%d i=%d \n", j, i);
				    
				    
				    
				    
				    imshow("map", map);
				    
				    waitKey(30);
				    
				   
				   
				    char ch;
				    do{
				         ch = rs->receiveCommand();
				         waitKey(30);
				    }
				    while(ch != 'D' && ch != '5');
				    
				    if(ch == '5')
				    {
						std::cout << "STOP!"<< std::endl;
						i=Height;
						break;
						}
				    
				    map.at<uchar>(i/10, j/10) = 255; 
				    
				   
				    
				    if(ws->svmTrained())
		            {		
                         int cRs[3] = {0,0,0};
                        for(int tr=0; tr<3; tr++)
                        {
							
						   waitKey(1000);
				           ws->triggerWalabot(); 	
							
                           int r = ws->getSVMresponse();
				           
				           cRs[r-1]++;
				           
				        
				        }
                       
                       int r = maxABC(cRs[0], cRs[1], cRs[2]);
                       
                       if(r == 2){
                            outImageC2.at<uchar>(i/10,j/10) = 255;
                            beep();
						}
                        else
                           if(r == 3){
                               outImageC3.at<uchar>(i/10,j/10) = 255;
                               beep();
                               beep();
						   }
                        		
		            }  
		            
		           imshow("C2", outImageC2);
		           imshow("C3", outImageC3);
				    
			  }   // for j
			  
			  j -= hInc;
			  
		  } // for i
		  
		  
		  int nRowsC2 = outImageC2.rows;
			int nColsC2 = outImageC2.cols;
		  
		  Mat colorC2 = Mat::zeros(nRowsC2, nColsC2, CV_8UC3);
	      Mat colorC3 = Mat::zeros(nRowsC2, nColsC2, CV_8UC3);
	
		 std::vector<Vec4i> lines;
		 HoughLinesP( outImageC2, lines, 1, CV_PI/180, 10, 10, 2 );
		 for( size_t i = 0; i < lines.size(); i++ )
         {
		
		    line( colorC2, Point(lines[i][0], lines[i][1]), Point(lines[i][2], lines[i][3]), Scalar(0,0,255), 1, 8 );
	    }
		  
		  std::vector<Vec4i> lines2;
		  HoughLinesP( outImageC3, lines2, 1, CV_PI/180, 10, 10, 2 );
		 for( size_t i = 0; i < lines2.size(); i++ )
         {
		
		    line( colorC3, Point(lines2[i][0], lines2[i][1]), Point(lines2[i][2], lines2[i][3]), Scalar(0,0,255), 1, 8 );
	     }
		  
		  
		  
		  
		  imshow("houghC2", colorC2);
		  imshow("houghC3", colorC3);
		  
		  
		  ls->projectDetection(outImageC2, outImageC3);
		  
	    } // if start
	    
	    
	    
			
		if(ch == 'g' || cmdTouch == '4')
	    {
			int Height = (int)rs->heightTriang((float)PL, (float)PR, (float)Base);
			Mat outImageC2 = Mat::zeros(Height/10+1, Base/10+1, CV_8U);
			printf("Height - %d e base %d\n",Height/10,Base/10);
			ls->projectSquare(outImageC2);
		}
		
		if(ch == 'p' || cmdTouch == '6' )
	    {
			ls->projectorStop();
		}
	    
	
	printf("new trigger\r\n");
	  
	if((ch = waitKey(30)) == 27) break;  
  
  
  } // while
  
  
  delete ws;
  delete rs;	
  delete cs;
  delete ls;
  
}

Credits

Pedro Soares
2 projects • 2 followers
PhD in electronics. More than 15 years projecting electronic boards and software.
Getúlio Igrejas
2 projects • 1 follower

Comments