File content as of revision 1:d47f3431441b:

/* 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 GENERALLOOPDELAY 1 //Modificar este valor para el tiempo entre una secuencia de movimiento de 5 motores y la secuencia siguiente
#define LOOPSNUMBER 1      //Modificar este valor segun a la cantidad de veces que quiere repetir la rutina

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

Serial pc(USBTX, USBRX); 
//--------------------------------------------------------------------
//**************Inicio del programa principal*************************
//--------------------------------------------------------------------
int main(){
    
    char StartBit= '\0';
    int temp= LOOPSNUMBER; 
    
    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
    
    printf("\nPresione cualquer tecla para empezar\n");
    StartBit=pc.getc();
    
    if(StartBit!=0)
    {
        StartBit='\0';
        do
        {
        ServosMovingTogether(0.5,0.5,0.5,0.5,0.5);
        //---------------------|   |   |   |   |   Garra
        //---------------------|   |   |   |       Muneca
        //---------------------|   |   |           Codo
        //---------------------|   |               Hombro
        //---------------------|                   Eje
        
        wait(GENERALLOOPDELAY);
        //*********************************************************
        //******Modificar a partir de este punto*******************
        //*********************************************************
        
        ServosMovingTogether(0.125,0.6,0.05,0.975,0.975);
        wait(GENERALLOOPDELAY);
        
        
        ServosMovingTogether(0.125,0.55,0.0,0.975,0.775);
        wait(GENERALLOOPDELAY);
        
        
        ServosMovingTogether(0.125,0.35,0.40,0.975,0.775);
        wait(GENERALLOOPDELAY);
        
        
        ServosMovingTogether(0.825,0.35,0.40,0.975,0.975);
        wait(GENERALLOOPDELAY);
        //***********Dejar de Modificar******************
        }while(--temp);
    }
    }//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);
    
    Muneca=MunecaPos;
    wait (MIDSTEPDELAY);
    
    Codo=CodoPos;
    wait (MIDSTEPDELAY);
    
    Hombro=HombroPos;
    wait (MIDSTEPDELAY);
    
    Garra=GarraPos;
    wait (MIDSTEPDELAY);
}