File content as of revision 0:21188b66200a:

/* Programa para el control de 
un brazon robotico con 5 servos

Carrera Ingenieria Mecatronica
IMT
*/

#include "mbed.h"
#include "Servo.h"
#include <vector>

//*****************Servos y sus Puertos*******

Servo Garra(PTC9); 
Servo Muneca(PTC8);
Servo Codo(PTA5);
Servo Hombro(PTA4);
Servo Eje(PTA12);

//*********Definicion de Funciones************

 void ServosMovingTogether(float, float, float, float, float);
 
//*********Macros Defines*********************
#define MidStepDelay 1  //modificar este valor para el tiempo entre el movimiento de un motor y otro
#define GenralLoopDelay 1 //Modificar este valor para el tiempo entre una secuencia de movimiento de 5 motores y la secuencia siguiente

//Comando para iniciar un comunicacion entre la FRDM-KL-Z y la computador

Serial pc(USBTX, USBRX); 
//--------------------------------------------------------------------
//**************Inicio del programa principal*************************
//--------------------------------------------------------------------
int main(){
    
    pc.baud(9600); //Velocidad de la comunicacion entre la tarjeta y la computadora 
    
    printf("\nBienvenido\n\n Iniciando la rutina del IMTarm");
    while(1){  //Loop infinito
    
        ServosMovingTogether(0.5,0.5,0.5,0.5,0.5);
        wait(GenralLoopDelay);
        

    }//Fin del Loop infinito
    
 //+++++++++++++++++++++++++++++++++++++++++++++++
 //-----------------------------------------------   
}//********FIN DEL PROGRAMA PRINCIPAL*************
 //-----------------------------------------------
 //+++++++++++++++++++++++++++++++++++++++++++++++
 
 
 //-----------------------------------------------
 //Funcion para mover los 5 servos en secuencia
 //-----------------------------------------------
 void ServosMovingTogether(float EjePos, float HombroPos, float CodoPos, float MunecaPos, float GarraPos)
{
    Eje=EjePos;
    wait (MidStepDelay);
    
    Hombro=HombroPos;
    wait (MidStepDelay);
    
    Codo=CodoPos;
    wait (MidStepDelay);
        
    Muneca=MunecaPos;
    wait (MidStepDelay);
        
    
    Garra=GarraPos;
    wait (MidStepDelay);
}