2014

MECATRONICA

EQUIPOS DE LA MATERIA
TOPICOS SELECTOS DE
MECATRONICA
-MECATRONICA -

MECATRONICA
OBJETIVO
Que el grupo de la materia Tpicos Selectos de Mecatrnica sea capaz de desarrollar el funcionamiento
completo de un omni phantom.

MARCO TEORICO
ENCODER

Un encoder es un sensor electro-opto-mecnico que unido a un eje, proporciona informacin de

la posicin angular. Su fin, es actuar como un dispositivo de realimentacin en sistemas de
control integrado.

Tipos de Encoders

Encoder incremntal (Incremental Encoder)

Este tipo de encoder se caracteriza porque determina su posicin, contando el numero de

impulsos que se generan cuando un rayo de luz, es atravesado por marcas opacas en la
superficie de un disco unido al eje.

En el estator hay como mnimo dos pares de fotorreceptor pticos, escalados un nmero entero
de pasos ms de paso. Al girar el rotor genera una seal cuadrada, el escalado hace que las
seales tengan un desfase de de periodo si el rotor gira en un sentido y de si gira en el
sentido contrario, lo que se utiliza para discriminar el sentido de giro.

Un simple sistema lgico permite determinar desplazamientos a partir de un origen, a base de

contar impulsos de un canal y determinar el sentido de giro a partir del desfase entre los dos
canales. Algunos encoders pueden disponer de un canal adicional que genere un pulso por
vuelta y la lgica puede dar nmero de vueltas ms fraccin de vuelta.

2
-MECATRONICA -

La resolucin del encoder depende del nmero de impulsos por revolucin.

Encoder absoluto (Absolute Encoder)

En el encoder absoluto, el disco contiene varias bandas dispuestas en forma de coronas

circulares concntricas, dispuestas de tal forma que en sentido radial el rotor queda dividido en
sectores, con marcas opacas y transparentes codificadas en cdigo Gray.

El estator tiene un fotorreceptor por cada bit representado en el disco. El valor binario obtenido
de los fotorreceptores es nico para cada posicin del rotor y representa su posicin absoluta.
Se utiliza el cdigo Gray en lugar de un binario clsico porque en cada cambio de sector slo
cambia el estado de una de las bandas, evitando errores por falta de alineacin de los
captadores.

Para un encoder con n bandas en el disco, el rotor permite 2^n combinaciones, por lo cual la
resolucin ser 360 dividido entre los 2^n sectores; Por ejemplo para encoders de 12 y 16 bits
se obtiene una resolucin angular de 0.0879 y 0.00549 respectivamente.

Resolucin angular = 360/2^n

Generalmente, los encoders incremntales proporcionan mayor resolucin a un costo ms bajo

que los encoders absolutos. Adems, su electrnica es mas simple ya que tienen menos lneas
de salida.

Tpicamente un encoder incrmental solo tiene cuatro lneas: 2 de cuadratura, una de poder y
una tierra. Un encoder absoluto tiene una lnea de salida por cada bit, una lnea de poder y la
tierra.

3
-MECATRONICA -

En esta prctica se seleccion usar un Encoder incremental por la naturaleza de nuestro

dispositivo, seleccionando el Encoders Bourns Ace 128 proporcionado por el docente:
DATASHEET

4
-MECATRONICA -

ACELEROMETRO

Se denomina acelermetro a cualquier instrumento destinado a medir aceleraciones. Esto no es

necesariamente la misma que la aceleracin de coordenadas (cambio de la velocidad del dispositivo en el
espacio), sino que es el tipo de aceleracin asociada con el fenmeno de peso experimentado por una masa
de prueba que se encuentra en el marco de referencia del dispositivo. Un ejemplo en el que este tipo de
aceleraciones son diferentes es cuando un acelermetro medir un valor sentado en el suelo, ya que las
masas tienen un peso, a pesar de que no hay cambio de velocidad. Sin embargo, un acelermetro en cada
gravitacional libre hacia el centro de la Tierra medir un valor de cero, ya que, a pesar de que su velocidad es
cada vez mayor, est en un marco de referencia en el que no tiene peso.

Actualmente es posible construir acelermetros de tres ejes (X,Y,Z) en un slo chip de silicio, incluyendo en
el mismo la parte electrnica que se encarga de procesar las seales.

En el desarrollo de este proyecto se eligi el acelermetro modelo Minimu9 de pololu:

El Pololu minimu-9 es una unidad de medicin inercial (IMU), que viene equipado con un L3G4200D
giroscopio de 3 ejes y un LSM303DLH 3 ejes acelermetro y magnetmetro de 3 ejes en un pequeo 0.9 "x
0.6" tablero. Una interfaz IC accede a nueve rotaciones independientes, aceleracin y mediciones
magnticas que se pueden utilizar para calcular la orientacin absoluta del sensor.

El L3G4200D y la LSM303 tienen muchas opciones configurables, incluyendo sensibilidades dinmicamente

seleccionables para el giroscopio, acelermetro, y magnetmetro, as como una eleccin de velocidades de
datos de salida para cada sensor. Los dos CI se puede acceder a travs de una interfaz IC / TWI compartida,
permitiendo que todos los tres sensores que deben abordarse de forma individual a travs de una sola lnea
de reloj y una sola lnea de datos. Las nueve rotaciones independientes, aceleracin y lecturas magnticas (a
veces llamados 9DOF) proporcionan todos los datos necesarios para hacer un sistema de referencia de
actitud y rumbo (AHRS). El giroscopio se puede utilizar para realizar un seguimiento de forma muy precisa la
rotacin en un corto plazo de tiempo, mientras que el acelermetro y la brjula pueden ayudar a compensar
la deriva del girocomps con el tiempo, proporcionando un marco de referencia absoluto. Los ejes
respectivos de los dos chips estn alineados en el tablero para facilitar estos clculos fusin de sensores.

Caractersticas

- Dimensiones: 0.9 "x 0.6" x 0.1 "(23 x 15 x 3 mm)

- Peso sin pines del cabezal: 0,9 g (0,03 oz)
- Tensin de trabajo: 02.06 a 05.05 V
- Corriente de alimentacin: 10 mA
- Formato de salida (IC):
- Gyro: una lectura de 16 bits por eje
- Acelermetro: uno de 12 bits de lectura (justificado a la izquierda) por eje
- Magnetmetro: uno de 12 bits de lectura (justificado a la derecha) por eje
- Rango de sensibilidad (configurable):
- Gyro: 250, 500, o 2,000 / s
- Acelermetro: 2, 4, o 8 g
- Magnetmetro: 1.3, 1.9, 2.5, 4.0, 4.7, 5,6 o 8,1 gauss

5
-MECATRONICA -

MATERIAL
- Encoders
- Sensor acelerometro
- Resistencias
- Conectores
- Cautin
- Estao
- Arduino
- Conectores hembra
- Cable plano

6
-MECATRONICA -

DESARROLLO
En este reporte se explicara en dos bloques para mejor una mejor explicacin del proyecto (Hardware y
Software)

HARDWARE

En esta seccin se busca que nuestro omni phantom se mueva de la siguiente manera:

(logrando registrar los desplazamientos)

Para ello se plantea incrustar a presin en las siguientes zonas los encoder:

7
-MECATRONICA -

Para la lectura de posicionamiento en grados del modelo mecnico, utilizaremos el Encoder Marca BOURNS
Modelo ACE-128. Absolute Contacting Encoder (ACE) para cada articulacin.

El rango es de 0 a 127 posiciones. Donde cada posicin corresponde a 2.8125 grados.

Diagrama esquemtico de conexin para Encoder

Se disea diagrama para los encoders:

Vcc
10K

R8

10K

R7

10K

R6

10K

R5

10K

R4

10K

R3

10K

R2

10K

R1
8

1
GND

GND
4
3

2
1

Vcc
8
7
6
5
4
3
2
1
6
5
4
3
2
1

10
9
8
7
6
5
4
3
2
1
SIL-100-06

J2

SIL-100-10

J1
SIL-100-06

J3
1
2
3
4
5
6
5
6

7
8

Diseo en PCB de la placa a imprimir

8
-MECATRONICA -

Diseo realizado en 3D del placa a realizar

Imprimiendo las hojas

para el Serigrafiado

Se plancha las hojas

sobre la placa

Se obtienen las pequeas

placas para los Encoders

9
-MECATRONICA -

Diagrama de conexin de 4 Encoder en puertos de Arduino.

Una vez soldados los componentes, se soldan las conexiones en pines para mejor manipulacin de
informacion y dispositivo

10
-MECATRONICA -

El dispositivo queda de la siguiente manera con los encoders ya posicionados:

11
-MECATRONICA -

En la la punta de nuestro omni phantom se es necesaria una pieza de agarre para nuestro sensor el cual nos
brindara otros dos grados de movilidad para una mejor manipulacin de nuestro dispositivo.

El diseo es el siguiente:

PIEZA 1 PIEZA 2

PIEZA 3 ENSAMBLE

12
-MECATRONICA -

Simulacion fisica de la pieza:

13
-MECATRONICA -

Pieza fisica

14
-MECATRONICA -

SOFTWARE:

CODIGO DE ARDUINO PARA LOS ENCODER

int M [128] [2]= {

{0, 127}, {1, 63}, {2, 62}, {3, 58}, {4, 56}, {5, 184}, {6, 152}, {7, 24}, {8, 8}, {9, 72},
{10, 73}, {11, 77}, {12, 79}, {13, 15}, {14, 47}, {15, 175}, {16, 191}, {17, 159}, {18, 31}, {19, 29},
{20, 28}, {21, 92}, {22, 76}, {23, 12}, {24, 4}, {25, 36}, {26, 164}, {27, 166}, {28, 167}, {29, 135},
{30, 151}, {31, 215}, {32, 223}, {33, 207}, {34, 143}, {35, 142}, {36, 14}, {37, 46}, {38, 38}, {39, 6},
{40, 2}, {41, 18}, {42, 82}, {43, 83}, {44, 211}, {45, 195}, {46, 203}, {47, 235}, {48, 239}, {49, 231},
{50, 199}, {51, 71}, {52, 7}, {53, 23}, {54, 19}, {55, 3}, {56, 1}, {57, 9}, {58, 41}, {59, 169},
{60, 233}, {61, 225}, {62, 229}, {63, 245}, {64, 247}, {65, 243}, {66, 227}, {67, 163}, {68, 131}, {69, 139},
{70, 137}, {71, 129}, {72, 128}, {73, 132}, {74, 148}, {75, 212}, {76, 244}, {77, 240}, {78, 242}, {79, 250},
{80, 251}, {81, 249}, {82, 241}, {83, 209}, {84, 193}, {85, 197}, {86, 196}, {87, 192}, {88, 64}, {89, 66},
{90, 74}, {91, 106}, {92, 122}, {93, 120}, {94, 121}, {95, 125}, {96, 253}, {97, 252}, {98, 248}, {99, 232},
{100, 224}, {101, 226}, {102, 98}, {103, 96}, {104, 32}, {105, 33}, {106, 37}, {107, 53}, {108, 61}, {109, 60},
{110, 188}, {111, 190}, {112, 254}, {113, 126}, {114, 124}, {115, 116}, {116, 112}, {117, 113}, {118, 49}, {119, 48},
{120, 16}, {121, 144}, {122, 146}, {123, 154}, {124, 158}, {125, 30}, {126, 94}, {127, 95}

};
float L1 = 14;
float L2 = 11.5;
float h1 = 7;
float h2 = 14;
float th1;
float gam1;
float gam2;
float gam3;
class Puerto
{ public:
int Pin;
int PinAcarreo;
int Resultado;
int Resultado2;
int k;
int r;
int i;
int PinIniObj;
Puerto(int PortIni,int PortEnd){
for (i=PortIni;i<=PortEnd;i++){
pinMode(i,INPUT);
}
Pin=0;
PinAcarreo=0;
Resultado=0;
Resultado2;
k=0;
i=0;
PinIniObj=PortIni;
}
int Leer(){
for(i=0; i<8;i++){

15
-MECATRONICA -

Pin=digitalRead(PinIniObj+i);
PinAcarreo=Pin<<i;
Resultado=Resultado|PinAcarreo;
}
for(k=0;k<=127;k++){
if (Resultado==M[k][1]){
Resultado2=M[k][0];
r=map(Resultado2,0,127,0,359);
return (r);
}
}
Resultado=0;
}
};
void end_effector(int th1,int gam1,int gam2,int gam3){
float Link1 = (cos(th1)*(L1*cos(gam1 + gam2) + h2*sin(gam1) + L2*cos(gam1 + gam2 + gam3)));
float Link2 = (sin(th1)*(L1*cos(gam1 + gam2) + h2*sin(gam1) + L2*cos(gam1 + gam2 + gam3)));
float Link3 = (h1 - L1*sin(gam1 + gam2) + h2*cos(gam1) - L2*sin(gam1 + gam2 + gam3));
Serial.print(th1);
Serial.print(" ");
Serial.print(gam1);
Serial.print(" ");
Serial.print(gam2);
Serial.print(" ");
Serial.print(gam3);
Serial.print(" ");
Serial.print(Link1);
Serial.print(" ");
Serial.print(Link2);
Serial.print(" ");
Serial.println(Link3);
}
//------------------- OBJETOS-------------------------------
Puerto portA = Puerto (22,29);
Puerto portB = Puerto (30,37);
Puerto portC = Puerto (38,45);
Puerto portD = Puerto (46,53);
//----------------------------------------------------------
void setup()
{
Serial.begin(9600);
}
void loop()
{
int th1=portA.Leer();
int gam1=portB.Leer();
int gam2=portC.Leer();
int gam3=portD.Leer();
end_effector(th1,gam1,gam2,gam3);
}

16
-MECATRONICA -

CODIGO DE ARDUINO COMPLETO

int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer

#include <Wire.h>
// LSM303 accelerometer: 8 g sensitivity
// 3.9 mg/digit; 1 g = 256
#define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer

#define ToRad(x) ((x)*0.01745329252) // *pi/180

#define ToDeg(x) ((x)*57.2957795131) // *180/pi

// L3G4200D gyro: 2000 dps full scale

// 70 mdps/digit; 1 dps = 0.07
#define Gyro_Gain_X 0.07 //X axis Gyro gain
#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians
for second
#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians
for second
#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians
for second

// LSM303 magnetometer calibration constants; use the Calibrate example from

// the Pololu LSM303 library to find the right values for your board
#define M_X_MIN -421
#define M_Y_MIN -639
#define M_Z_MIN -238
#define M_X_MAX 424
#define M_Y_MAX 295
#define M_Z_MAX 472

#define Kp_ROLLPITCH 0.02

#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002

/*For debugging purposes*/

//OUTPUTMODE=1 will print the corrected data,
//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
#define OUTPUTMODE 1

//#define PRINT_DCM 0 //Will print the whole direction cosine matrix

#define PRINT_ANALOGS 0 //Will print the analog raw data

17
-MECATRONICA -

#define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw

#define STATUS_LED 13

float G_Dt=0.02; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible

long timer=0; //general purpuse timer

long timer_old;
long timer24=0; //Second timer used to print values
int AN[6]; //array that stores the gyro and accelerometer data
int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors

int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;
int magnetom_x;
int magnetom_y;
int magnetom_z;
float c_magnetom_x;
float c_magnetom_y;
float c_magnetom_z;
float MAG_Heading;

float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector

float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0,0,0};//Omega Proportional correction
float Omega_I[3]= {0,0,0};//Omega Integrator
float Omega[3]= {0,0,0};

// Euler angles
float roll;
float pitch;
float yaw;

float errorRollPitch[3]= {0,0,0};

float errorYaw[3]= {0,0,0};

unsigned int counter=0;

byte gyro_sat=0;

float DCM_Matrix[3][3]= {

18
-MECATRONICA -

{
1,0,0 }
,{
0,1,0 }
,{
0,0,1 }
};
float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here

float Temporary_Matrix[3][3]={
{
0,0,0 }
,{
0,0,0 }
,{
0,0,0 }
};

int M [128] [2]= {

{0, 127}, {1, 63}, {2, 62}, {3, 58}, {4, 56}, {5, 184}, {6, 152}, {7, 24}, {8, 8}, {9, 72},
{10, 73}, {11, 77}, {12, 79}, {13, 15}, {14, 47}, {15, 175}, {16, 191}, {17, 159}, {18, 31}, {19, 29},
{20, 28}, {21, 92}, {22, 76}, {23, 12}, {24, 4}, {25, 36}, {26, 164}, {27, 166}, {28, 167}, {29, 135},
{30, 151}, {31, 215}, {32, 223}, {33, 207}, {34, 143}, {35, 142}, {36, 14}, {37, 46}, {38, 38}, {39, 6},
{40, 2}, {41, 18}, {42, 82}, {43, 83}, {44, 211}, {45, 195}, {46, 203}, {47, 235}, {48, 239}, {49, 231},
{50, 199}, {51, 71}, {52, 7}, {53, 23}, {54, 19}, {55, 3}, {56, 1}, {57, 9}, {58, 41}, {59, 169},
{60, 233}, {61, 225}, {62, 229}, {63, 245}, {64, 247}, {65, 243}, {66, 227}, {67, 163}, {68, 131}, {69,
139},
{70, 137}, {71, 129}, {72, 128}, {73, 132}, {74, 148}, {75, 212}, {76, 244}, {77, 240}, {78, 242}, {79,
250},
{80, 251}, {81, 249}, {82, 241}, {83, 209}, {84, 193}, {85, 197}, {86, 196}, {87, 192}, {88, 64}, {89,
66},
{90, 74}, {91, 106}, {92, 122}, {93, 120}, {94, 121}, {95, 125}, {96, 253}, {97, 252}, {98, 248}, {99,
232},
{100, 224}, {101, 226}, {102, 98}, {103, 96}, {104, 32}, {105, 33}, {106, 37}, {107, 53}, {108, 61},
{109, 60},
{110, 188}, {111, 190}, {112, 254}, {113, 126}, {114, 124}, {115, 116}, {116, 112}, {117, 113}, {118,
49}, {119, 48},
{120, 16}, {121, 144}, {122, 146}, {123, 154}, {124, 158}, {125, 30}, {126, 94}, {127, 95}

};
float h1 = 14;
float L1 = 14;
float L2 = 14;
float L3 = 11.5;

19
-MECATRONICA -

int th1;
int gam1;
int gam2;
int gam3;

class Puerto
{ public:

int Pin;
int PinAcarreo;
int Resultado;
int Resultado2;
int k;
int r;
int i;
int PinIniObj;

Puerto(int PortIni,int PortEnd){

for (i=PortIni;i<=PortEnd;i++){
pinMode(i,INPUT);
}
Pin=0;
PinAcarreo=0;
Resultado=0;
Resultado2;
k=0;
i=0;
PinIniObj=PortIni;
}

int Leer(){
for(i=0; i<8;i++){
Pin=digitalRead(PinIniObj+i);
PinAcarreo=Pin<<i;
Resultado=Resultado|PinAcarreo;
}
for(k=0;k<=127;k++){
if (Resultado==M[k][1]){
Resultado2=M[k][0];
r=map(Resultado2,0,127,0,359);
return (r);
}
}
Resultado=0;

20
-MECATRONICA -

}
};

void end_effector(int th1,int gam1,int gam2,int gam3){

Serial.print(th1);
Serial.print(" ");
Serial.print(gam1);
Serial.print(" ");
Serial.print(gam2);
Serial.print(" ");
Serial.print(gam3);
Serial.print(" ");

float th1m = (float)th1*3.1459/180;

float gam1m = (float)gam1*3.1459/180;
float gam2m = (float)gam2*3.1459/180;
float gam3m = (float)gam3*3.1459/180;

float Link1 = cos(th1m)*(L2*sin(gam1m + gam2m) + L1*sin(gam1m) + L3*sin(gam1m + gam2m + gam3m));

float Link2 = sin(th1m)*(L2*sin(gam1m + gam2m) + L1*sin(gam1m) + L3*sin(gam1m + gam2m + gam3m));

float Link3 = h1 + L2*cos(gam1m + gam2m) + L1*cos(gam1m) + L3*cos(gam1m + gam2m + gam3m);

Serial.print(Link1);
Serial.print(" ");
Serial.print(Link2);
Serial.print(" ");
Serial.println(Link3);
}
//------------------- OBJETOS-------------------------------
Puerto portA = Puerto (22,29);
Puerto portB = Puerto (30,37);
Puerto portC = Puerto (38,45);
Puerto portD = Puerto (46,53);
//----------------------------------------------------------

void setup()
{
Serial.begin(115200);
pinMode (STATUS_LED,OUTPUT); // Status LED

21
-MECATRONICA -

I2C_Init();

Serial.println("Pololu MinIMU-9 + Arduino AHRS");

digitalWrite(STATUS_LED,LOW);
delay(1500);

Accel_Init();
Compass_Init();
Gyro_Init();

delay(20);

for(int i=0;i<32;i++) // We take some readings...

{
Read_Gyro();
Read_Accel();
for(int y=0; y<6; y++) // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}

for(int y=0; y<6; y++)

AN_OFFSET[y] = AN_OFFSET[y]/32;

AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];

//Serial.println("Offset:");
for(int y=0; y<6; y++)
Serial.println(AN_OFFSET[y]);

delay(2000);
digitalWrite(STATUS_LED,HIGH);

timer=millis();
delay(20);
counter=0;
}

void loop() //Main Loop

{
if((millis()-timer)>=20) // Main loop runs at 50Hz
{
counter++;
timer_old = timer;

22
-MECATRONICA -

timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro
integration time)
else
G_Dt = 0;

// *** DCM algorithm

// Data adquisition
Read_Gyro(); // This read gyro data
Read_Accel(); // Read I2C accelerometer

if (counter > 5) // Read compass data at 10Hz... (5 loop runs)

{
counter=0;
Read_Compass(); // Read I2C magnetometer
Compass_Heading(); // Calculate magnetic heading
}

// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***

printdata();
}
///////////////////////////////////////////////////////PHANTOM
th1=portA.Leer();
gam1=portB.Leer();
gam2=portC.Leer();
gam3=portD.Leer();

end_effector(th1,gam1,gam2,gam3);

/////////////// A PARTIR DE AQUI SON PARTES DE LA LIBRERIA DEL ACELEROMETRO

void Compass_Heading()
{
float MAG_X;
float MAG_Y;
float cos_roll;
float sin_roll;

23
-MECATRONICA -

float cos_pitch;
float sin_pitch;

cos_roll = cos(roll);
sin_roll = sin(roll);
cos_pitch = cos(pitch);
sin_pitch = sin(pitch);

// adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) -
SENSOR_SIGN[6]*0.5;
c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) -
SENSOR_SIGN[7]*0.5;
c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) -
SENSOR_SIGN[8]*0.5;

// Tilt compensated Magnetic filed X:

MAG_X =
c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
// Tilt compensated Magnetic filed Y:
MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
// Magnetic Heading
MAG_Heading = atan2(-MAG_Y,MAG_X);
}
void Normalize(void)
{
float error=0;
float temporary[3][3];
float renorm=0;

error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19

Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19

Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19

Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19

Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19

Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20

renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21

Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);

renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21

Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);

24
-MECATRONICA -

renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21

Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
}

/**************************************************/
void Drift_correction(void)
{
float mag_heading_x;
float mag_heading_y;
float errorCourse;
//Compensation the Roll, Pitch and Yaw drift.
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_magnitude;
float Accel_weight;

//*****Roll and Pitch***************

// Calculate the magnitude of the accelerometer vector

Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] +
Accel_Vector[2]*Accel_Vector[2]);
Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //

Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of

reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);

Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);

//*****YAW***************
// We make the gyro YAW drift correction based on compass magnetic heading

mag_heading_x = cos(MAG_Heading);
mag_heading_y = sin(MAG_Heading);
errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating
YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation
of the aircraft, depeding the position.

25
-MECATRONICA -

Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.

Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
}
/**************************************************/
/*
void Accel_adjust(void)
{
Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
}
*/
/**************************************************/

void Matrix_update(void)
{
Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw

Accel_Vector[0]=accel_x;
Accel_Vector[1]=accel_y;
Accel_Vector[2]=accel_z;

Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term

Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term

//Accel_adjust(); //Remove centrifugal acceleration. We are not using this function in this version - we
have no speed measurement

#if OUTPUTMODE==1
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
#else // Uncorrected data (no drift correction)
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z

26
-MECATRONICA -

Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
Update_Matrix[2][2]=0;
#endif

Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c

for(int x=0; x<3; x++) //Matrix Addition (update)

{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}

void Euler_angles(void)
{
pitch = -asin(DCM_Matrix[2][0]);
roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}
#include <L3G.h>
#include <LSM303.h>

L3G gyro;
LSM303 compass;

void I2C_Init()
{
Wire.begin();
}

void Gyro_Init()
{
gyro.init();
gyro.writeReg(L3G_CTRL_REG4, 0x20); // 2000 dps full scale
gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
}

void Read_Gyro()

27
-MECATRONICA -

{
gyro.read();

AN[0] = gyro.g.x;
AN[1] = gyro.g.y;
AN[2] = gyro.g.z;
gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
}

void Accel_Init()
{
compass.init();
compass.enableDefault();
switch (compass.getDeviceType())
{
case LSM303::device_D:
compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011
break;
case LSM303::device_DLHC:
compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode
break;
default: // DLM, DLH
compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11
}
}

// Reads x,y and z accelerometer registers

void Read_Accel()
{
compass.readAcc();

AN[3] = compass.a.x >> 4; // shift left 4 bits to use 12-bit representation (1 g = 256)
AN[4] = compass.a.y >> 4;
AN[5] = compass.a.z >> 4;
accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
}

void Compass_Init()
{
// doesn't need to do anything because Accel_Init() should have already called compass.enableDefault()
}

28
-MECATRONICA -

void Read_Compass()
{
compass.readMag();

magnetom_x = SENSOR_SIGN[6] * compass.m.x;

magnetom_y = SENSOR_SIGN[7] * compass.m.y;
magnetom_z = SENSOR_SIGN[8] * compass.m.z;
}
void printdata(void)
{
Serial.print("!");

#if PRINT_EULER == 1
Serial.print("ANG:");
Serial.print(ToDeg(roll));
Serial.print(",");
Serial.print(ToDeg(pitch));
Serial.print(",");
Serial.print(ToDeg(yaw));
#endif
#if PRINT_ANALOGS==1
Serial.print(",AN:");
Serial.print(AN[0]); //(int)read_adc(0)
Serial.print(",");
Serial.print(AN[1]);
Serial.print(",");
Serial.print(AN[2]);
Serial.print(",");
Serial.print(AN[3]);
Serial.print (",");
Serial.print(AN[4]);
Serial.print (",");
Serial.print(AN[5]);
Serial.print(",");
Serial.print(c_magnetom_x);
Serial.print (",");
Serial.print(c_magnetom_y);
Serial.print (",");
Serial.print(c_magnetom_z);
#endif
/*#if PRINT_DCM == 1
Serial.print (",DCM:");
Serial.print(convert_to_dec(DCM_Matrix[0][0]));
Serial.print (",");

29
-MECATRONICA -

Serial.print(convert_to_dec(DCM_Matrix[0][1]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[0][2]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[1][0]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[1][1]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[1][2]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[2][0]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[2][1]));
Serial.print (",");
Serial.print(convert_to_dec(DCM_Matrix[2][2]));
#endif*/
Serial.println();

long convert_to_dec(float x)
{
return x*10000000;
}
float Vector_Dot_Product(float vector1[3],float vector2[3])
{
float op=0;

for(int c=0; c<3; c++)

{
op+=vector1[c]*vector2[c];
}

return op;
}

//Computes the cross product of two vectors

void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
{
vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
}

//Multiply the vector by a scalar.

30
-MECATRONICA -

void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)

{
for(int c=0; c<3; c++)
{
vectorOut[c]=vectorIn[c]*scale2;
}
}

void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])

{
for(int c=0; c<3; c++)
{
vectorOut[c]=vectorIn1[c]+vectorIn2[c];
}
}

void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])

{
float op[3];
for(int x=0; x<3; x++)
{
for(int y=0; y<3; y++)
{
for(int w=0; w<3; w++)
{
op[w]=a[x][w]*b[w][y];
}
mat[x][y]=0;
mat[x][y]=op[0]+op[1]+op[2];

float test=mat[x][y];
}
}
}

31
-MECATRONICA -

CODIGO EN MATLAB PARA SIMULACION DEL POSICIONAMIENTO DEL BRAZO.

ga3 = ga3m%ga3m*t/tmax;
clear all
close all % punto origen del robot, primer joint
clc T0 = obtenT(0,0,0,0,0,th1);
%Segundo joint (rotacion en y)
Rob = line ([0 0], [0 0], [0 0]); T1 = obtenT(0,0,14,0,ga1,0);
grid on %tercer joint (rotacion en y)
Ls = 100 T2 = obtenT(0,0,14,0,ga2,0);
axis (Ls*[-1 1 -1 1 0 2]) %cuarto joint (rotacion en x)
set(Rob,'Marker','o') T3 = obtenT(0,0,14,0,ga3,0);
set(Rob,'MarkerSize',5) %quinto joint (rotacion en y)
set(Rob,'MarkerEdgeColor','k') T4 = obtenT(0,0,11.5,0,0,0);
set(Rob,'MarkerFaceColor','r')
set(Rob,'LineWidth',2) T01 = T0*T1;
set(Rob,'Color','b') T02 = T01*T2;
T03 = T02*T3;
%rotacin en z T04 = T03*T4;
th1m = 10
%rotacin en y set(Rob,'XData',[T0(1,4) T01(1,4) T02(1,4)
ga1m = 90 T03(1,4) T04(1,4)]);
ga2m = 90 set(Rob,'YData',[T0(2,4) T01(2,4) T02(2,4)
ga3m = -90 T03(2,4) T04(2,4)]);
tmax = 100; set(Rob,'ZData',[T0(3,4) T01(3,4) T02(3,4)
T03(3,4) T04(3,4)]);
for t = 0:0.05:tmax

th1 = 0%th1m*t/tmax*8; drawnow

ga1 = ga1m%ga1m*t/tmax*4;
ga2 = ga2m%ga2m*t/tmax*2; end

32
-MECATRONICA -

MODELO MATEMATICO
%T4 = obtenT( L2, 0, 0, 0, 0, 0,1);
clear all
close all
clc %T04 = T0*T1*T2*T3*T4
%Ts = simple(T04)
%%Esta bien pero debo considerar los angulos en %Pos = Ts(1:3,4)
los que se encuentran
%%los links, ya que sino considera que estan T0 = obtenT( 0, 0, 0, 0, 0,th1,1);
desplazados el linea recta. T1 = obtenT( 0, 0, h1, 0, gam1, 0,1);
syms h1 h2 real T2 = obtenT( 0, 0, L1, 0, gam2, 0,1);
syms L1 L2 L3 real T3 = obtenT( 0, 0, L2, 0, gam3, 0,1);
syms th1 gam1 gam2 gam3 real T4 = obtenT( 0, 0, L3, 0, 0, 0,1);

%T0 = obtenT( 0, 0, 0, 0, 0,th1,1);

%T1 = obtenT( 0, 0, h1, 0, gam1, 0,1); T04 = T0*T1*T2*T3*T4
%T2 = obtenT( h2, 0, 0, 0, gam2, 0,1); Ts = simple(T04)
%T3 = obtenT( L1, 0, 0, 0, gam3, 0,1); Pos = Ts(1:3,4)

33
-MECATRONICA -

OBSERVACIONES Y CONCLUSIONES
Para la correcta realizacin de este proyecto, se cuenta con la colaboracin de cada uno de los equipos que
forman el grupo de tpicos selectos de Mecatrnica. Donde se definieron partes necesarias para lograr la
lectura de la posicin del brazo.

En orden del cumplimiento de dicha tarea, se colocaron encoders absolutos en cada articulacin del brazo.
Para lo cual se necesit ajustar el orificio, para que al mover el brazo el movimiento se transmita al eje del
encoder. Sumando un total de 4 articulaciones. La interpretacin de cada posicin es ofrecida por el
software de Arduino.

Primeramente se dise una pieza que agregara dos grados de libertad al brazo, con la finalidad de recrear
los movimientos de un Omniphantom, dando la capacidad al proyecto de tener ms fluidez a la hora de
realizar sus movimientos y cubrir todos los grados de libertad en los ejes x, y, z. Sin embargo por cuestiones
de suministro no fue posible imprimir en 3D dicha pieza. Lo cual nos oblig a recurrir a un modelo
experimental en papel cascaron.

En adicin, se simula el posicionamiento en el espacio con el software de MATLAB. Lo que lleva a concluir
que el trabajo presentado puede ser implementado para un proyecto ms avanzado en robtica, utilizando
la cinemtica para cerrar el lazo de control, haciendo que se tenga un brazo robtico didctico con capacidad
para recordar coordenadas y que se asemeje a un brazo industrial.

BIBLIOGRAFIA
http://www.pololu.com/product/1264

https://www.bourns.com/pdfs/ace.pdf

http://www.forosdeelectronica.com/f16/encoders-informacion-tecnica-25/

http://www.dynamoelectronics.com/index.php?product_id=1004&page=shop.product_details&category_id
=128&flypage=dynamo.tpl&option=com_virtuemart&Itemid=58

34