Video Test 01
Video Test 02
Introduction
Among most of the devices have to be able to identify users believe that our project is still among the least developed in Mexico, which is in some part a challenge to achieve the identification of a person using this type of verification since there are many sources on which we rely to conduct our research, which even we can make this as one of the early research in development of such devices.
In everything that you want to try find the part where we realize that it is easier to allow access for multiple people to a certain place using iris recognition, this is intended to replace or provide an alternative to the identification of users, as the process is carried out using radio frequency credentials or HandKeys that recognize patterns of user's hand, which are already used in the CIC (Computing Research Center).
The research conducted for this project includes many theoretical sources on the Internet about robotic arm mobility with the use of mathematical formulas to calculate the movement of the arm and Internet sources code which was adapted only to make it work according to our needs.
Among the objectives we seek with the development of this project has the mark a breakthrough in the development of these and achieve with this foster the development of such projects by students of the IPN, as there are some that have not been realize that they have the level to do them.
Theory system architecture
Inverse Kinematics
Kinematics deals with the description of motion without regard to its causes. The objective of the inverse kinematics is to find the gesture to be taken by different joints for the end of the linkage reaches a specific position.
Background
2.3 Theory of devices to be used for project development
The system will try to have the camera on one end, and at the other end we have the robot arm. The gesture for an articulated system reaches a particular position is defined by the inverse kinematics. For the preparation of standard servo arm will use ¼ scale servo force and effect, the reason is because they have the inverted rotation, thus both servos receive the same pulse width to be placed in the desired position.
A servo is basically an electric motor which can only be moved at an angle of about 180 degrees (not provide full turns as normal engines). It has three wires, red is supply voltage (+5 v), black is ground (GND), and the yellow wire is the power by which they are required to accommodate servo what position (0 ° to 180 °), ie the control cable.
Inside the servo controller card tells the DC motor rotate few laps to accommodate the arrow in the position that has been asked. The variable resistor (also called "pot") is subject to the arrow, and measures to where this rotated at all times. This is how the controller card knows where to move the engine.
The desired position is given to the servo by means of pulses (PWM). All time must be a pulse signal present at the control cable to the servo hold its position. These "orders" are a series of pulses. Pulse duration indicates the rotation angle of the motor. Each servo has its operating margins, which correspond to the width of the maximum and minimum pulse the servo understand.
The following table shows some pulse widths that correspond to the angles of the servos.
Standard Servo Force
 | Operating Voltage: 4.8-6.0V
STD Address: Opposite to clockwise / pulse that travels 1500-1900 μ sec.
Puesto Torque: 6.0V: ≧ 3.5 kg.com (49 oz. / In), 4.8V: ≧ 3.2 kg.com (44.8 oz. / In)
Operating Speed: 6.0V: 0.19 sec / 60 ° at no charge, 4.8V: 0.23 sec / 60 ° at no charge
Weight: 39.2g (1.37 oz.)
Size: 40.6 x 20.0 x 38.9 mm |
Controlling our servos with different arm angles each q we can do this would be brought in different places detected.
The axes of motion are set by servo motors, which are controlled by the leading card installed called The R / C Servo Controller II. This card is based on a PIC microcontroller that connects to the serial port of the PC. In this case we use the PIC16F628A microcontroller as compared with pic16f84 has twice USART program memory and 4 MHz internal clock, which simplifies the circuit and reduces the cost.
FLASH is a CMOS microcontroller 8-bit RISC architecture capable of operating at clock frequencies up to 20 MHz, easy to program and capsules available in DIP (Dual row of pins) and SOIC (surface mount square shaped) 18 pin. It has an internal 4 MHz oscillator circuit Power-on reset to eliminate the need for external components and expand to 16 the number of pins that can be used as lines I / O (Input / Output, Input / Output) purpose Overall, unlike the PIC16F84 that only use 13 pins as inputs or outputs in the two ports. Additionally, not forgetting his Harvard architecture RISC instructions, the PIC16F628 provides a data memory EEPROM 128x8 (128 bytes), a program memory FLASH 2024x14 (2K with 14 bits per location), a data memory RAM general purpose of 224x8, CCP module (capture / compare / PWM) a USART, 3 analog comparators, a voltage reference and three programmable timers. These and other features make it ideal for automotive, industrial, and consumer electronics, as well as programmable tools and equipment of all kinds. The pin of the PIC16F628 is identical to the PIC16F627, except that the latter has a program memory FLASH 1024x14. It is also identical to the PIC16F84, PIC16F628 except that you can have three I / O lines additional port A (RA7, RA6, RA5, the latter can only be input) and some I / O pins are multiplexed with alternate function for the various peripheral devices that supports the chip. For example, RB1 line also functions as USART reception (RX). Generally, when a peripheral is enabled, the line can not be used as an I / O pin general purpose.
Using the code
Using PICBASIC, we write some lines for PIC16F628A, this will only put a serial sign in a port for servomotor:
INCLUDE "bs2defs.bas"
symbol pos = b3
symbol servo = b4
trisa = %11111111
trisb = %00000000
serpin VAR porta.0 'serial input pin
start:
serin serpin,N2400,pos 'get serial input from PC
move:
pulsout portb.0,pos ' send data to position servos
pause 10
goto start ' do it again
Here is a diagram that describes the phisical model of the integration between servomotor and pic.
After understading inverse kinematics we write some lines to resolve the formula of Kinematics:
private void Dibujar(object sender, PaintEventArgs e)
{
Graphics g = e.Graphics;
Pen blackPen = new Pen(Color.Black, 3);
g.DrawEllipse(blackPen, PuntoLinea01.X, PuntoLinea01.Y - 5, 10, 10);
g.DrawLine(Pens.Gray, 0, PuntoMedio.Y, PuntoInicio.X, PuntoMedio.Y);
g.DrawLine(Pens.Blue, PuntoMedio.X, 0, PuntoMedio.X, PuntoInicio.Y);
g.DrawLine(Pens.Purple, PuntoMedio, PuntoLinea01);
double angulo = Math.Atan2(PuntoLinea02.Y - 0, PuntoLinea02.X - PuntoMedio.X) * 180 / Math.PI;
label1.Text = angulo.ToString();
double angulo2 = Math.Atan2(PuntoLinea01.Y-PuntoLinea02.Y ,PuntoLinea01.X-PuntoLinea02.X) * 180 / Math.PI;
label2.Text = angulo2.ToString();
#region gradocentral
double hipotenusaAnguloCentral= (PuntoLinea01.Y - PuntoMedio.Y) / -(PuntoLinea01.X - PuntoMedio.X);
double gradosangulo = Math.Atan2((PuntoLinea01.Y - PuntoMedio.Y), (PuntoLinea01.X - PuntoMedio.X)) * 180 / Math.PI;
gradosangulo = -gradosangulo;
label3.Text = gradosangulo.ToString();
#endregion
double hipotenusa = Math.Sqrt(Math.Pow(getXReal(PuntoLinea01.X),2)+Math.Pow(getYReal(PuntoLinea01.Y),2));
double MitadHipotenusa = hipotenusa / 2;
double LongitudMotor01;
double LongitudMotor02;
double LongitudMotor03;
if (!checkBox3.Checked)
{
LongitudMotor01 = hipotenusa * PorcentajeMotor01;
LongitudMotor02 = hipotenusa * PorcentajeMotor02;
LongitudMotor03 = hipotenusa * PorcentajeMotor03;
}
else
{
LongitudMotor01 = 100;
LongitudMotor02 = 50;
LongitudMotor03 = 15;
}
double alturaMotor01 = LongitudMotor01 * (Math.Sin((gradosangulo*Math.PI)/180));
double baseMotor01 =Math.Sqrt( Math.Pow(LongitudMotor01,2)-Math.Pow(alturaMotor01,2));
double alturaMotor02 = LongitudMotor02 * (Math.Sin((gradosangulo * Math.PI) / 180));
double baseMotor02 = Math.Sqrt(Math.Pow(LongitudMotor02, 2) - Math.Pow(alturaMotor02, 2));
double alturaMotor03 = LongitudMotor03 * (Math.Sin((gradosangulo * Math.PI) / 180));
double baseMotor03 = Math.Sqrt(Math.Pow(LongitudMotor03, 2) - Math.Pow(alturaMotor03, 2));
float Motor01Medio = PuntoMedio.X + (float)baseMotor01;
float Motor02Medio = Motor01Medio + (float)baseMotor02;
float Motor03Medio = Motor02Medio + (float)baseMotor03;
label4.Text = (alturaMotor01/LongitudMotor01).ToString();
g.DrawLine(Pens.Red, Motor01Medio, PuntoMedio.Y, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01);
g.DrawLine(Pens.RoyalBlue, Motor02Medio, PuntoMedio.Y, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02+alturaMotor01));
g.DrawLine(Pens.Plum, Motor03Medio, PuntoMedio.Y, Motor03Medio, PuntoMedio.Y - (float)(alturaMotor03+alturaMotor02+alturaMotor01));
double anguloMotor01 = Math.Asin(LongitudMotor01/hipotenusa)*180/Math.PI;
double anguloMotoro01Base;
if(checkBox2.Checked)
anguloMotoro01Base = anguloMotor01+gradosangulo;
else
anguloMotoro01Base = anguloMotor01;
double alturaAnguloMotor01 = LongitudMotor01 * (Math.Sin((anguloMotoro01Base * Math.PI) / 180));
double baseAnguloMotor01 = Math.Sqrt(Math.Pow(LongitudMotor01, 2) - Math.Pow(alturaAnguloMotor01, 2));
PointFloat PuntoFinalMotor01 = new PointFloat((float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01);
g.DrawLine(Pens.Black, PuntoMedio.X, PuntoMedio.Y, PuntoFinalMotor01.X, PuntoFinalMotor01.Y);
g.DrawLine(Pens.Green, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01);
g.DrawLine(Pens.Yellow, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
g.DrawLine(Pens.Tomato, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor01Medio, PuntoMedio.Y);
g.DrawLine(Pens.IndianRed, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
double pendiente01 = getPendiente(Motor01Medio, PuntoMedio.Y - (float)alturaMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
g.DrawLine(Pens.Green, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01);
double pendiente02 = getPendiente((float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
label9.Text = getAnguloEntreRecta(pendiente01, pendiente02).ToString();
double distancia = getDistanciaEntreDosPuntos(PuntoFinalMotor01.X,PuntoFinalMotor01.Y,PuntoLinea01.X,PuntoLinea01.Y);
double distancia2 = getDistanciaEntreDosPuntos(PuntoMedio.X, PuntoMedio.Y, PuntoFinalMotor01.X, PuntoFinalMotor01.Y);
double anguloMotor02 = getAnguloEntreRecta( pendiente02,pendiente01);
double alturaAnguloMotor02 = LongitudMotor02 * CalcularSeno(anguloMotor02);
double baseAnguloMotor02 = Math.Sqrt(Math.Pow(LongitudMotor02, 2) - Math.Pow(alturaAnguloMotor02, 2));
PointFloat PuntoFinalMotor02 = new PointFloat((float)baseAnguloMotor02 + PuntoMedio.X + (float)baseAnguloMotor01, PuntoMedio.Y - (float)alturaAnguloMotor02-(float)alturaAnguloMotor01);
label6.Text = getDistanciaEntreDosPuntos(PuntoFinalMotor01.X, PuntoFinalMotor01.Y, PuntoFinalMotor02.X, PuntoFinalMotor02.Y).ToString();
label4.Text = getDistanciaEntreDosPuntos(Motor01Medio, PuntoMedio.Y - (float)alturaMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01)).ToString();
g.DrawLine(Pens.Black, PuntoFinalMotor01.X, PuntoFinalMotor01.Y, PuntoLinea01.X, PuntoLinea01.Y);
lblAnguloMotor01.Text = getAnguloEntreDosRectas(PuntoFinalMotor01.X, PuntoFinalMotor01.Y, PuntoMedio.X, PuntoMedio.Y) + "";
lblAnguloMotor02.Text = anguloMotor02 + "";
double dLineaCentral = getPendiente(PuntoFinalMotor02.X, PuntoFinalMotor02.Y, PuntoLinea01.X, PuntoLinea01.Y);
double dLineaMotor01 = getPendiente(PuntoMedio.X,PuntoMedio.Y, PuntoLinea01.X,PuntoLinea01.Y);
MotorDeAngulos.setAnguloMotor01( AnguloMotor.ConvertirAngulo01RealAEstandar(Convert.ToDouble(lblAnguloMotor01.Text)));
MotorDeAngulos.setAnguloMotor02( AnguloMotor.ConvertirAngulo02RealAEstandar(Convert.ToDouble(lblAnguloMotor02.Text)));
label12.Text = MotorDeAngulos.getAnguloMotor03().ToString();
ActualizarMotor();
}
public double getPendiente(double x1, double y1, double x2, double y2)
{
return (y2 - y1) / (x2 - x1);
}
public double getAnguloEntreRecta(double m1, double m2){
double div = (m2 - m1) / (1 + (m1 * m2));
return CalcularATan(div);
}
public double getAnguloEntreDosRectas(double x1, double y1, double x2, double y2) {
return Math.Atan2((y1 - y2), (x1 - x2)) * 180 / Math.PI;
}
public double getDistanciaEntreDosPuntos(double x1, double y1, double x2, double y2)
{
return Math.Sqrt(Math.Pow((x1 - x2), 2.0) + Math.Pow((y1 - y2), 2.0));
}
public double CalcularSeno(double valor)
{
double temp = (valor * Math.PI)/180;
return Math.Sin(temp);
}
public double CalcularASeno(double valor)
{
double temp = Math.Asin(valor);
return (valor * 180) / Math.PI;
}
public double CalcularATan(double valor)
{
double temp = Math.Atan(valor);
return (valor * 180) / Math.PI;
}
public double getXReal(double valor)
{
return valor - PuntoMedio.X;
}
public double getYReal(double valor)
{
return valor - PuntoMedio.Y;
}
Here is the result of INVERSE KINEMATICS in C#:
After getting the angles, we should convert this measure to Signal using RS232 protocol:
using System;
using System.Collections.Generic;
using System.Text;
using System.Collections;
namespace puertoserie.xiller.serie.angulo
{
public class Angulos
{
private static double ConvertirRangoAnguloASenal(double angulo){
double maxRangoAngulo = (double)Rangos.RangoAngulo;
double maxRangoSenal = (double)Rangos.RangoSenal;
double temp= maxRangoAngulo / maxRangoSenal;
int ciclo = 0;
for (double x = (double)Rangos.AnguloInicio; x <= (double)Rangos.AnguloFin; x+=temp)
{
if (x == angulo)return ciclo;
if (x > angulo)return ciclo;
ciclo++;
}
return ciclo;
}
public static double ConvertirAnguloASenal(double angulo)
{
double valor=ConvertirRangoAnguloASenal(angulo);
return valor + 50;
}
public static double ConvertirSenalAAngulo(double senal)
{
double temp = (double)Rangos.RangoSenal / (double)Rangos.RangoAngulo;
int ciclo=0;
for (double x = (double)Rangos.SenalInicio; x <= (double)Rangos.SenalFin; x += temp)
{
if (x == senal) return ciclo;
if (x > senal) return ciclo;
ciclo++;
}
return ciclo;
}
}
}
using System;
using System.Collections.Generic;
using System.Text;
namespace puertoserie.xiller.serie
{
public class Motor
{
private double ConvertirRangoAnguloASenal(double angulo)
{
double temp = this.rangoAngulo / this.rangoSenal;
int ciclo = 0;
for (double x = this.anguloInicio; x <= this.anguloFin; x += temp)
{
if (x == angulo) return ciclo;
if (x > angulo) return ciclo;
ciclo++;
}
return ciclo;
}
public double ConvertirAnguloASenal(double angulo)
{
double valor = ConvertirRangoAnguloASenal(angulo);
return valor + 50;
}
public double ConvertirSenalAAngulo(double senal)
{
double temp = this.rangoSenal / this.rangoAngulo;
int ciclo = 0;
for (double x = this.senalInicio; x <= this.senalFin; x += temp)
{
if (x == senal) return ciclo;
if (x > senal) return ciclo;
ciclo++;
}
return ciclo;
}
public Motor(double aInicio,double aRango,double aFin,double sInicio,double sRango,double sFin) {
this.anguloInicio = aInicio;
this.rangoAngulo = aRango;
this.anguloFin = aFin;
this.senalInicio = sInicio;
this.rangoSenal = sRango;
this.senalFin = sFin;
}
public Motor() { }
private double anguloInicio = 0;
public double AnguloInicio
{
get { return anguloInicio; }
set { anguloInicio = value; }
}
private double rangoAngulo = 180;
public double RangoAngulo
{
get { return rangoAngulo; }
set { rangoAngulo = value; }
}
private double anguloFin = 180;
public double AnguloFin
{
get { return anguloFin; }
set { anguloFin = value; }
}
private double senalInicio = 50;
public double SenalInicio
{
get { return senalInicio; }
set { senalInicio = value; }
}
private double rangoSenal = 200;
public double RangoSenal
{
get { return rangoSenal; }
set { rangoSenal = value; }
}
private double senalFin = 250;
public double SenalFin
{
get { return senalFin; }
set { senalFin = value; }
}
}
}
Other posibility is using Wiimote implementation, so using some characteristics of the controller we write some lines:
private void panel1_Paint(object sender, PaintEventArgs e)
{
if (wiiEstados.B == true)
{
Graphics g = e.Graphics;
control.MaxPanelX = panel1.Width;
control.MaxPanelY = panel1.Height;
label1.Text = wiiEstados.X.ToString();
label2.Text = wiiEstados.Y.ToString();
label3.Text = wiiEstados.Z.ToString();
control.dWiiMoteSetSpeedX = wiiEstados.X - control.dWiiOffsetX;
control.nSign = Math.Sign(control.dWiiMoteSetSpeedX);
control.dWiiMoteSSXQuadratic = Math.Pow(control.dWiiMoteSetSpeedX, 2.0) * control.nSign;
control.dWiiMotePosX = (control.dWiiMotePosX + (control.dWiiMoteSSXQuadratic * control.dSamplePeriod) * control.nSpeedGain);
if (control.dWiiMotePosX >= 1) control.dWiiMotePosX = 1;
if (control.dWiiMotePosX <= -1) control.dWiiMotePosX = -1;
control.dMousePosX = control.dWiiMotePosX;
control.CoordenadaXFinal = (control.MaxPanelX / 2) * (control.dMousePosX + 1);
control.dWiiMoteSetSpeedY = wiiEstados.Y - control.dWiiOffsetY;
control.nSign = Math.Sign(control.dWiiMoteSetSpeedY);
control.dWiiMoteSSYQuadratic = Math.Pow(control.dWiiMoteSetSpeedY, 2.0) * control.nSign;
control.dWiiMotePosY = (control.dWiiMotePosY + (control.dWiiMoteSSYQuadratic * control.dSamplePeriod) * control.nSpeedGain);
if (control.dWiiMotePosY >= 1) control.dWiiMotePosY = 1;
if (control.dWiiMotePosY <= -1) control.dWiiMotePosY = -1;
control.dMousePosY = control.dWiiMotePosY;
control.CoordenadaYFinal = (control.MaxPanelY / 2) * (control.dMousePosY + 1);
Pen blackPen = new Pen(Color.Black, 3);
setValorTrack02(control.CoordenadaXFinal);
setValorTrack01(control.CoordenadaYFinal);
g.DrawEllipse(blackPen, (float)control.CoordenadaXFinal, (float)control.CoordenadaYFinal, 10 + (wiiEstados.Z * 10), 10 + (wiiEstados.Z * 10));
}
}
private void setValorTrack01(double valor) {
int conv = Convert.ToInt16(Math.Round(valor));
if (conv >= 0 && conv <= wPanel.PanelWidth) {
trackBar1.Value = conv + 50;
setValorMotor(1, trackBar1.Value);
}
}
private void setValorTrack02(double valor)
{
int conv = Convert.ToInt16(Math.Round(valor));
if (conv >= 0 && conv <= wPanel.PanelHeight)
{
trackBar2.Value = conv + 50;
setValorMotor(3,trackBar2.Value);
}
}
private void setValorMotor(int motor , int valor) {
almacen.setCantidad(motor, valor);
switch (motor) {
case 0:
this.label15.Text = Angulos.ConvertirSenalAAngulo((double)trackBar3.Value).ToString() + "°";
break;
case 1:
this.label14.Text = Angulos.ConvertirSenalAAngulo((double)trackBar1.Value).ToString() + "°";
break;
case 2:
this.label14.Text = Angulos.ConvertirSenalAAngulo((double)trackBar1.Value).ToString() + "°";
break;
case 3:
this.label8.Text = Angulos.ConvertirSenalAAngulo((double)trackBar2.Value).ToString() + "°";
break;
}
}
References
- The Image Processing Handbook, John C. Russ, Fifth Edition, 2007 Publisher Taylor & Francis.
- OpenCV Bookseller, http://opencvlibrary.sourceforge.net/FaceDetection
- OpenCVDotNet Bookseller, http://code.google.com/p/opencvdotnet/
- AForge.NET, http://code.google.com/p/aforge/
- Speech SDK 5.1
- Eye Detection Cascades, by Modesto Castrillón Santana, (OpenCV Developers Community)
University:
Instituto Politécnico Nacional - Unidad Profesional Interdisciplinaria de Ingeniería y Ciencias Sociales y Administrativas (UPIICSA – Unidad Profesional Interdisciplinaria de Ingeniería y Ciencias Sociales y Administrativas IPN)
History
- First Draft 28/07/2013
- Second Draft:11/05/2021
