File content as of revision 1:d86f732b4db0:

/* 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);

//********Variables Globales******************

float RangeGarra=0.0007, RangeMuneca=0.0007, RangeCodo=0.0007;
float RangeHombro=0.0007, RangeEje=0.0007;
const float Changing= 0.025;
float DynPosGarra=0.5,DynPosMuneca=0.5,DynPosCodo=0.5;
float DynPosHombro=0.5,DynPosEje=0.5;
int Size,Casted,count;
char Iteraciones='\0';
int MoveOn=0;
//********Vectores de Movimiento*************

vector<float>DynGarra(0);
vector<float>DynMuneca(0);
vector<float>DynCodo(0);
vector<float>DynHombro(0);
vector<float>DynEje(0);

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

void Calibracion(Servo Calibrando,float& range);
void Mover(Servo Movement, float& NewPosition,float Position);
void Corriendo(int size,int Casted);
int Casting();
void Ultima_Posicion(void);
void SlowMotion(float NewPos, float AlmostLast,Servo LowServo);
void Home(void);


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

Serial pc(USBTX, USBRX); 
char Options= '\0'; 
//--------------------------------------------------------------------
//**************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");
    
    DynEje.push_back(0.5);
    DynHombro.push_back(0.5);
    DynCodo.push_back(0.5);
    DynMuneca.push_back(0.5);
    DynGarra.push_back(0.7);
    //*****************INTIT***********************
    
    Eje=0.5;
    Hombro=0.5;
    Codo=0.5;
    Muneca=0.5;
    Garra=0.7;
    //****************FOREVER**********************
    while(1){  //Loop infinito
    
    printf("\n\nSleccion\n 'C' para calibarar,\n 'M' para mover los servos \n 'L' para la ultima posicion\n 'U' para ir a la ultima posicion guardada\n");//'R' para correr rutina \n
    Casted=Casting();
        switch(Casted){
            
            case 12: 
            printf("\nServo Calibration Controls:\n\n");
            printf("1,2,3 - Position Servo (full left, middle, full right)\n");
            printf("4,5 - Decrease or Increase range\n");
            printf("Oprima 'Z' para salir de la etapa de calibracion\n");
                printf("\nCalibrando el motor de la garra\n\n");
                Calibracion(Garra,RangeGarra);
                printf("\n\nCalibrando el motor de la Muneca\n\n");
                Calibracion(Muneca,RangeMuneca);
                printf("\n\nCalibrando el motor del Codo\n\n");
                Calibracion(Codo,RangeCodo);
                printf("\n\nCalibrando el motor del Hombro\n\n");
                Calibracion(Hombro,RangeHombro);
                printf("\n\nCalibrando el motor del Eje\n\n");
                Calibracion(Hombro,RangeEje);
                //*************Parametros***********************
                Garra.calibrate(RangeGarra, 45.0);
                printf("\nValor de RangeGarra: %f",RangeGarra);
                Muneca.calibrate(RangeMuneca, 45.0);
                printf("\nValor de RangeMuneca: %f",RangeMuneca);
                Codo.calibrate(RangeCodo, 45.0);
                printf("\nValor de RangeCodo: %f",RangeCodo);
                Hombro.calibrate(RangeHombro, 45.0);
                printf("\nValor de RangeHombro: %f",RangeHombro);
                Eje.calibrate(RangeEje, 45.0);
    
                break;
                
            case 21:
                void Home(void);
                break;
            
            case 22:
                        printf("\nIncializando la secuencia de movimiento de ejes\n"\
                        "Presione '1' para aumentar la posicion\n"\
                        "Presion '3' para reducir la posicion\n"\
                        "Presione '4' para guardad valores en la rutina");
                        
                    do{     

                    printf("\n\nMoviendo eje de Eje\n"); 
                    Mover(Eje, DynPosEje,DynPosEje);
                    printf("\n\nNuevo valor de DynPosEje %f", DynPosEje);
                    DynEje.push_back(DynPosEje);                    
                                         
                    printf("\n\nMoviendo eje de Muneca\n");
                    Mover(Muneca,DynPosMuneca,DynPosMuneca);
                    printf("\n\nNuevo valor de DynPosMuneca %f",DynPosMuneca);
                    DynMuneca.push_back(DynPosMuneca);
                    
                    printf("\n\nMoviendo eje de Codo\n");
                    Mover(Codo,DynPosCodo,DynPosCodo);
                    printf("\n\nNuevo valor de DynPosCodo %f",DynPosCodo);
                    DynCodo.push_back(DynPosCodo);
                    
                    printf("\n\nMoviendo eje de Hombro\n");
                    Mover(Hombro,DynPosHombro,DynPosHombro);
                    printf("\n\nNuevo valor de DynPosHombro %f",DynPosHombro);
                    DynHombro.push_back(DynPosHombro);
                    
                    printf("\n\nMoviendo eje de Garra\n"); 
                    Mover(Garra, DynPosGarra,DynPosGarra);
                    printf("\n\nNuevo valor de DynPosGarra %f", DynPosGarra);
                    DynGarra.push_back(DynPosGarra);
                    
                    do{
                    printf("\n\n\tDesea agregar otra posicion?  (Y/N)\n");
                    printf("\n\n\tCantidad de posiciones es: %d\n\n", Size=DynGarra.size());
                    Options=Casting();
                        if((Options==23)|(Options==34)){
                            MoveOn=1;}
                        else{
                            printf("\n Operacion no valida\n Por favor especifique si quiere ingresar otra posicion\n");
                            MoveOn=0;}
                  }while(MoveOn==0);
                }while(Options!= 23);
                break;
            
            case 100:
            
            printf("\nIntroduzca cuantas veces quiere repetir la rutina"\
                    "\nPara cantidades mayores a 10 siga la logica:"\
                    "\nA=10,B=11,C=12...");
                    Casted=Casting();
                    Size=DynGarra.size();
            printf("\nCantidad de posiciones= %d e iteraciones %d",Size,Casted);
                Corriendo(Size,Casted);
                break;
                
            case 30:
                Ultima_Posicion();
                break;
            
            default:
                printf("\nEl comando es invalido, ingrese un comando valido\n");
                break;
        }//Fin del Switch case
    }//Fin del Loop infinito
    
    
}//********FIN DEL PROGRAMA PRINCIPAL*************
//************************************************
//************************************************
//************************************************

//**************Definicion de funciones para el correr el programa*************


//********************Funcion de Calibracion***********************************
void Calibracion(Servo Calibrando, float& range){
    
    range = 0.0008;
    float position = 0.5;
    int Char=0;
    
    do{
        Char=Casting();
                       
        switch(Char) {
            case 1: position = 0.0; break;
            case 2: position = 0.5; break;
            case 3: position = 1.0; break;
            case 4: range += 0.0001; break; 
            case 5: range -= 0.0001; break; 
        }//Close switch
        
        printf("position = %.1f, range = +/-%0.4f\n", position, range);
        Calibrando.calibrate(range, 45.0); 
        Calibrando = position;
    }while(Char != 35);//Close Do-While    
}
//******************Fin del funciion de calibracion***********

//******************Funcion de Movimiento**********************

void Mover(Servo Movement, float& NewPosition,float Position){
    
     NewPosition=Position;
     int Char=0;
    
    do{
        Char=Casting();
        
        switch(Char){
            
            case 1:
                 if(NewPosition>=0.975){
                printf("\n\nImposible aumentar mas la posicion\n");
            }else{
                NewPosition += Changing;
                printf("\n Valor de la posicion aumentada = %f", NewPosition);
            }
            break;
        
            case 3:
                if(NewPosition<=0.0){
                    printf("\n\nImposible reducir mas la posicion\n");
            }else{
                NewPosition -= Changing;
                printf("\n Valor de la posicion reducida = %f", NewPosition);
            }
            break;
        case 4://**********Escribir Secuencia**************
            Char=35;
            break;
        default: 
            printf("\nOperacion invalida, intente de nuevo\n");
            break;
        }//close swithc case.
        
            Movement=NewPosition;
    }while(Char!=35);        
}

//**********Funcion para correr rutina***********************
void Corriendo(int size, int iteraciones)
{
  for(int j=0;j<=iteraciones-1;j++){
    
    Garra=0.7;
    SlowMotion(DynHombro[size-1],0.5,Hombro);
    Muneca=0.5;
    Codo=0.5;
    Eje=0.5;
    
    for(int i=0;i<=size-1;i++)
    {
        Hombro=DynHombro[(i-1)];        
        wait(0.5);        
        //SlowMotion(DynEje[i],DynEje[i-1],Eje);
        Eje=DynEje[i];
        wait(0.5);
        //SlowMotion(DynMuneca[i],DynMuneca[i-1],Muneca);
        Muneca=DynMuneca[i];
        wait(0.5);
        Codo=DynCodo[i];
        //SlowMotion(DynCodo[i],DynCodo[i-1],Codo);
        wait(0.5);
        //SlowMotion(DynHombro[i],DynHombro[i-1],Hombro);
        Hombro=DynHombro[i];
        wait(0.5);
        Garra=DynGarra[i];
        wait(0.5);
    }//Fin del ciclo for
    printf("\nTerminada la iteracion %d", j+1);
    
  }   
  printf("\n\n\nTerminadas las iteraciones especificadas\n\n");
 }//Fin de la funcion    
 
 //Fin de la funcion**********************************

int Casting(){
    char Character;
    Pointer:
    Character=pc.getc();
    int Casted=0;
    Casted=(int)Character;
    if((Casted<=57)&&(Casted>=48)){
        Casted=Casted-48;
    }else if((Casted<=90)&&(Casted>=65)){
        Casted=Casted-55;
    }else if((Casted<=122)&&(Casted>=97)){
        Casted=Casted-87;
    }else{
        printf("\nOperacion no valida, vuelva a intentar\n");
        goto Pointer;
    }
    return Casted;
}

//*****Ir a la ultima posicion-*********

void Ultima_Posicion()
{
    int PositionSize=(DynGarra.size()-1);
    
    Garra=DynGarra[PositionSize];
    wait(0.5); 
    Muneca=DynMuneca[PositionSize];
    wait(0.5);
    Codo=DynMuneca[PositionSize];
    wait(0.5);
    Hombro=DynHombro[PositionSize];
    wait(0.5);
    Eje=DynEje[PositionSize];
    wait(0.5);
}

//******Slow move********

void SlowMotion(float NewPos, float AlmostLast,Servo LowServo)
{
  float temp=AlmostLast;

    do{  
          if(NewPos<AlmostLast)
            {
                   temp-=Changing;
                   LowServo=temp;
                   wait_ms(10);
            }  
            
             else if(NewPos>AlmostLast)
            {
                   temp+=Changing;
                   LowServo=temp;
                   wait_ms(10);
            }   
        }while(NewPos!=AlmostLast);

}

void Home(void)
{
    Garra=0.975;
    wait(0.2);
    
    Hombro=0.5;
    wait(0.2);
    
    Eje=0.5;
    wait(0.2);
    
    Codo=0.5;
    wait(0.2);
    
    Muneca=0.5;
    wait(0.2);
}