Ricardo Soto
Programa para los taller IMTchallenge
Diff: main.cpp
- Revision:
- 0:294d306fd915
- Child:
- 1:d86f732b4db0
--- /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