![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Programa para los taller IMTchallenge
Diff: main.cpp
- Revision:
- 0:294d306fd915
- Child:
- 1:d86f732b4db0
diff -r 000000000000 -r 294d306fd915 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 15 17:24:09 2015 +0000 @@ -0,0 +1,338 @@ +/* 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); + + + +//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.5); + + Garra=DynGarra[0]; + Muneca=DynMuneca[0]; + Codo=DynCodo[0]; + Hombro=DynCodo[0]; + Eje=DynEje[0]; + + while(1){ //Loop infinito + + printf("\n\nSleccion\n 'C' para calibarar,\n 'M' para mover los servos \n 'R' para correr rutina \n 'U' para ir a la ultima posicion guardad\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 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 27: + + 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++){ + + for(int i=0;i<=size-1;i++) + { + wait(0.5); + //Garra=DynGarra[i]; + SlowMotion(DynGarra[i], DynGarra[i-1],Garra); + + wait(0.5); + Muneca=DynMuneca[i]; + wait(0.5); + Codo=DynCodo[i]; + wait(0.5); + Hombro=DynHombro[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; + + if(NewPos<AlmostLast) + { + for(int i=0;i<4;i++) + { + temp-=0.01; + LowServo=temp; + wait_ms(10); + } + } + + if(NewPos>AlmostLast) + { + for(int i=0;i<3;i++) + { + temp+=0.01; + LowServo=temp; + wait_ms(10); + } + } +} \ No newline at end of file