handServoReciever.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 Serial xbee1(PA_11, PA_12); //Inicializamos el xbee
00004 
00005 Serial pc(USBTX, USBRX);
00006 Servo chico(D9);
00007 Servo gordo(D3);
00008 Servo indice(D5);
00009 Servo corte(D6);
00010 Servo anillo(D10);
00011 
00012 int8_t chicoPosicion, gordoPosicion, anilloPosicion, indicePosicion,
00013         cortePosicion;
00014 char finalDeTrama;
00015 void calibrarServos();
00016 void calibrarServos() {
00017     chico.calibrate(0.00095, 90);
00018     chico.position(-10);
00019     gordo.calibrate(0.00095, 90);
00020     gordo.position(-10);
00021     indice.calibrate(0.00095, 90);
00022     indice.position(-20);
00023     corte.calibrate(0.00095, 90);
00024     corte.position(-10);
00025     anillo.calibrate(0.00095, 90);
00026     anillo.position(-10);
00027 }
00028 
00029 int main() {
00030     calibrarServos(); // Con esta funcion calibramos los servos y los ponemos en su posicion
00031     xbee1.baud(38400);
00032     while (1) {
00033 
00034         //Traducimos el valor analogico de entrada del sensor a grados
00035         //con el reescalado y asi obtener la posicion
00036         if (xbee1.readable()) {
00037             if (xbee1.getc() == 0x7E) {
00038                 //Recogemos las variables en el orden que
00039                 //necesitamos para que se muevan los dedos
00040                 chicoPosicion = xbee1.getc();
00041                 gordoPosicion = xbee1.getc();
00042                 anilloPosicion = xbee1.getc();
00043                 indicePosicion = xbee1.getc();
00044                 cortePosicion = xbee1.getc();
00045                 finalDeTrama = xbee1.getc();
00046                 pc.printf("Esto es : %d \r\n", cortePosicion);
00047                 if (finalDeTrama == 0x0D) {
00048                     pc.printf("Trama correcta \r\n");
00049 
00050                     chico.position(chicoPosicion);
00051                     gordo.position(gordoPosicion);
00052                     anillo.position(anilloPosicion);
00053                     indice.position(indicePosicion);
00054                     corte.position(cortePosicion);
00055 
00056                 } else {
00057                     pc.printf("Trama INCORRECTA \r\n");
00058                 }
00059             }
00060         }
00061 
00062     }
00063 
00064     //Movemos a la posicion los dedos dandole la informacion necesaria a los servos.
00065     wait_ms(30);
00066 
00067 }
00068