Ricardo Soto
/
IMTarm
Programa para los taller IMTchallenge
main.cpp
- Committer:
- Soto
- Date:
- 2015-10-16
- Revision:
- 1:d86f732b4db0
- Parent:
- 0:294d306fd915
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); }