Programa para los taller IMTchallenge

Dependencies:   Servo mbed

Committer:
Soto
Date:
Fri Oct 16 04:39:29 2015 +0000
Revision:
1:d86f732b4db0
Parent:
0:294d306fd915
version 1.2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Soto 0:294d306fd915 1 /* Programa para el control de
Soto 0:294d306fd915 2 un brazon robotico con 5 servos
Soto 0:294d306fd915 3
Soto 0:294d306fd915 4 Carrera Ingenieria Mecatronica
Soto 0:294d306fd915 5 IMT
Soto 0:294d306fd915 6 */
Soto 0:294d306fd915 7
Soto 0:294d306fd915 8 #include "mbed.h"
Soto 0:294d306fd915 9 #include "Servo.h"
Soto 0:294d306fd915 10 #include <vector>
Soto 0:294d306fd915 11
Soto 0:294d306fd915 12 //*****************Servos y sus Puertos*******
Soto 0:294d306fd915 13
Soto 0:294d306fd915 14 Servo Garra(PTC9);
Soto 0:294d306fd915 15 Servo Muneca(PTC8);
Soto 0:294d306fd915 16 Servo Codo(PTA5);
Soto 0:294d306fd915 17 Servo Hombro(PTA4);
Soto 0:294d306fd915 18 Servo Eje(PTA12);
Soto 0:294d306fd915 19
Soto 0:294d306fd915 20 //********Variables Globales******************
Soto 0:294d306fd915 21
Soto 0:294d306fd915 22 float RangeGarra=0.0007, RangeMuneca=0.0007, RangeCodo=0.0007;
Soto 0:294d306fd915 23 float RangeHombro=0.0007, RangeEje=0.0007;
Soto 0:294d306fd915 24 const float Changing= 0.025;
Soto 0:294d306fd915 25 float DynPosGarra=0.5,DynPosMuneca=0.5,DynPosCodo=0.5;
Soto 0:294d306fd915 26 float DynPosHombro=0.5,DynPosEje=0.5;
Soto 0:294d306fd915 27 int Size,Casted,count;
Soto 0:294d306fd915 28 char Iteraciones='\0';
Soto 0:294d306fd915 29 int MoveOn=0;
Soto 0:294d306fd915 30 //********Vectores de Movimiento*************
Soto 0:294d306fd915 31
Soto 0:294d306fd915 32 vector<float>DynGarra(0);
Soto 0:294d306fd915 33 vector<float>DynMuneca(0);
Soto 0:294d306fd915 34 vector<float>DynCodo(0);
Soto 0:294d306fd915 35 vector<float>DynHombro(0);
Soto 0:294d306fd915 36 vector<float>DynEje(0);
Soto 0:294d306fd915 37
Soto 0:294d306fd915 38 //*********Definicion de Funciones************
Soto 0:294d306fd915 39
Soto 0:294d306fd915 40 void Calibracion(Servo Calibrando,float& range);
Soto 0:294d306fd915 41 void Mover(Servo Movement, float& NewPosition,float Position);
Soto 0:294d306fd915 42 void Corriendo(int size,int Casted);
Soto 0:294d306fd915 43 int Casting();
Soto 0:294d306fd915 44 void Ultima_Posicion(void);
Soto 0:294d306fd915 45 void SlowMotion(float NewPos, float AlmostLast,Servo LowServo);
Soto 1:d86f732b4db0 46 void Home(void);
Soto 0:294d306fd915 47
Soto 0:294d306fd915 48
Soto 0:294d306fd915 49 //Comando para iniciar un comunicacion entre la FRDM-KL-Z y la computador
Soto 0:294d306fd915 50
Soto 0:294d306fd915 51 Serial pc(USBTX, USBRX);
Soto 0:294d306fd915 52 char Options= '\0';
Soto 0:294d306fd915 53 //--------------------------------------------------------------------
Soto 0:294d306fd915 54 //**************Inicio del programa principal*************************
Soto 0:294d306fd915 55 //--------------------------------------------------------------------
Soto 0:294d306fd915 56 int main(){
Soto 0:294d306fd915 57
Soto 0:294d306fd915 58 pc.baud(9600); //Velocidad de la comunicacion entre la tarjeta y la computadora
Soto 0:294d306fd915 59
Soto 0:294d306fd915 60 printf("\nBienvenido\n\n Iniciando la rutina del IMTarm");
Soto 0:294d306fd915 61
Soto 0:294d306fd915 62 DynEje.push_back(0.5);
Soto 0:294d306fd915 63 DynHombro.push_back(0.5);
Soto 0:294d306fd915 64 DynCodo.push_back(0.5);
Soto 0:294d306fd915 65 DynMuneca.push_back(0.5);
Soto 1:d86f732b4db0 66 DynGarra.push_back(0.7);
Soto 1:d86f732b4db0 67 //*****************INTIT***********************
Soto 0:294d306fd915 68
Soto 1:d86f732b4db0 69 Eje=0.5;
Soto 1:d86f732b4db0 70 Hombro=0.5;
Soto 1:d86f732b4db0 71 Codo=0.5;
Soto 1:d86f732b4db0 72 Muneca=0.5;
Soto 1:d86f732b4db0 73 Garra=0.7;
Soto 1:d86f732b4db0 74 //****************FOREVER**********************
Soto 0:294d306fd915 75 while(1){ //Loop infinito
Soto 0:294d306fd915 76
Soto 1:d86f732b4db0 77 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
Soto 0:294d306fd915 78 Casted=Casting();
Soto 0:294d306fd915 79 switch(Casted){
Soto 0:294d306fd915 80
Soto 0:294d306fd915 81 case 12:
Soto 0:294d306fd915 82 printf("\nServo Calibration Controls:\n\n");
Soto 0:294d306fd915 83 printf("1,2,3 - Position Servo (full left, middle, full right)\n");
Soto 0:294d306fd915 84 printf("4,5 - Decrease or Increase range\n");
Soto 0:294d306fd915 85 printf("Oprima 'Z' para salir de la etapa de calibracion\n");
Soto 0:294d306fd915 86 printf("\nCalibrando el motor de la garra\n\n");
Soto 0:294d306fd915 87 Calibracion(Garra,RangeGarra);
Soto 0:294d306fd915 88 printf("\n\nCalibrando el motor de la Muneca\n\n");
Soto 0:294d306fd915 89 Calibracion(Muneca,RangeMuneca);
Soto 0:294d306fd915 90 printf("\n\nCalibrando el motor del Codo\n\n");
Soto 0:294d306fd915 91 Calibracion(Codo,RangeCodo);
Soto 0:294d306fd915 92 printf("\n\nCalibrando el motor del Hombro\n\n");
Soto 0:294d306fd915 93 Calibracion(Hombro,RangeHombro);
Soto 0:294d306fd915 94 printf("\n\nCalibrando el motor del Eje\n\n");
Soto 0:294d306fd915 95 Calibracion(Hombro,RangeEje);
Soto 0:294d306fd915 96 //*************Parametros***********************
Soto 0:294d306fd915 97 Garra.calibrate(RangeGarra, 45.0);
Soto 0:294d306fd915 98 printf("\nValor de RangeGarra: %f",RangeGarra);
Soto 0:294d306fd915 99 Muneca.calibrate(RangeMuneca, 45.0);
Soto 0:294d306fd915 100 printf("\nValor de RangeMuneca: %f",RangeMuneca);
Soto 0:294d306fd915 101 Codo.calibrate(RangeCodo, 45.0);
Soto 0:294d306fd915 102 printf("\nValor de RangeCodo: %f",RangeCodo);
Soto 0:294d306fd915 103 Hombro.calibrate(RangeHombro, 45.0);
Soto 0:294d306fd915 104 printf("\nValor de RangeHombro: %f",RangeHombro);
Soto 0:294d306fd915 105 Eje.calibrate(RangeEje, 45.0);
Soto 0:294d306fd915 106
Soto 0:294d306fd915 107 break;
Soto 1:d86f732b4db0 108
Soto 1:d86f732b4db0 109 case 21:
Soto 1:d86f732b4db0 110 void Home(void);
Soto 1:d86f732b4db0 111 break;
Soto 0:294d306fd915 112
Soto 0:294d306fd915 113 case 22:
Soto 0:294d306fd915 114 printf("\nIncializando la secuencia de movimiento de ejes\n"\
Soto 0:294d306fd915 115 "Presione '1' para aumentar la posicion\n"\
Soto 0:294d306fd915 116 "Presion '3' para reducir la posicion\n"\
Soto 0:294d306fd915 117 "Presione '4' para guardad valores en la rutina");
Soto 0:294d306fd915 118
Soto 0:294d306fd915 119 do{
Soto 0:294d306fd915 120
Soto 0:294d306fd915 121 printf("\n\nMoviendo eje de Eje\n");
Soto 0:294d306fd915 122 Mover(Eje, DynPosEje,DynPosEje);
Soto 0:294d306fd915 123 printf("\n\nNuevo valor de DynPosEje %f", DynPosEje);
Soto 0:294d306fd915 124 DynEje.push_back(DynPosEje);
Soto 0:294d306fd915 125
Soto 0:294d306fd915 126 printf("\n\nMoviendo eje de Muneca\n");
Soto 0:294d306fd915 127 Mover(Muneca,DynPosMuneca,DynPosMuneca);
Soto 0:294d306fd915 128 printf("\n\nNuevo valor de DynPosMuneca %f",DynPosMuneca);
Soto 0:294d306fd915 129 DynMuneca.push_back(DynPosMuneca);
Soto 0:294d306fd915 130
Soto 0:294d306fd915 131 printf("\n\nMoviendo eje de Codo\n");
Soto 0:294d306fd915 132 Mover(Codo,DynPosCodo,DynPosCodo);
Soto 0:294d306fd915 133 printf("\n\nNuevo valor de DynPosCodo %f",DynPosCodo);
Soto 0:294d306fd915 134 DynCodo.push_back(DynPosCodo);
Soto 0:294d306fd915 135
Soto 0:294d306fd915 136 printf("\n\nMoviendo eje de Hombro\n");
Soto 0:294d306fd915 137 Mover(Hombro,DynPosHombro,DynPosHombro);
Soto 0:294d306fd915 138 printf("\n\nNuevo valor de DynPosHombro %f",DynPosHombro);
Soto 0:294d306fd915 139 DynHombro.push_back(DynPosHombro);
Soto 0:294d306fd915 140
Soto 0:294d306fd915 141 printf("\n\nMoviendo eje de Garra\n");
Soto 0:294d306fd915 142 Mover(Garra, DynPosGarra,DynPosGarra);
Soto 0:294d306fd915 143 printf("\n\nNuevo valor de DynPosGarra %f", DynPosGarra);
Soto 0:294d306fd915 144 DynGarra.push_back(DynPosGarra);
Soto 0:294d306fd915 145
Soto 0:294d306fd915 146 do{
Soto 0:294d306fd915 147 printf("\n\n\tDesea agregar otra posicion? (Y/N)\n");
Soto 0:294d306fd915 148 printf("\n\n\tCantidad de posiciones es: %d\n\n", Size=DynGarra.size());
Soto 0:294d306fd915 149 Options=Casting();
Soto 0:294d306fd915 150 if((Options==23)|(Options==34)){
Soto 0:294d306fd915 151 MoveOn=1;}
Soto 0:294d306fd915 152 else{
Soto 0:294d306fd915 153 printf("\n Operacion no valida\n Por favor especifique si quiere ingresar otra posicion\n");
Soto 0:294d306fd915 154 MoveOn=0;}
Soto 0:294d306fd915 155 }while(MoveOn==0);
Soto 0:294d306fd915 156 }while(Options!= 23);
Soto 0:294d306fd915 157 break;
Soto 0:294d306fd915 158
Soto 1:d86f732b4db0 159 case 100:
Soto 0:294d306fd915 160
Soto 0:294d306fd915 161 printf("\nIntroduzca cuantas veces quiere repetir la rutina"\
Soto 0:294d306fd915 162 "\nPara cantidades mayores a 10 siga la logica:"\
Soto 0:294d306fd915 163 "\nA=10,B=11,C=12...");
Soto 0:294d306fd915 164 Casted=Casting();
Soto 0:294d306fd915 165 Size=DynGarra.size();
Soto 0:294d306fd915 166 printf("\nCantidad de posiciones= %d e iteraciones %d",Size,Casted);
Soto 0:294d306fd915 167 Corriendo(Size,Casted);
Soto 0:294d306fd915 168 break;
Soto 0:294d306fd915 169
Soto 0:294d306fd915 170 case 30:
Soto 0:294d306fd915 171 Ultima_Posicion();
Soto 0:294d306fd915 172 break;
Soto 0:294d306fd915 173
Soto 0:294d306fd915 174 default:
Soto 0:294d306fd915 175 printf("\nEl comando es invalido, ingrese un comando valido\n");
Soto 0:294d306fd915 176 break;
Soto 0:294d306fd915 177 }//Fin del Switch case
Soto 0:294d306fd915 178 }//Fin del Loop infinito
Soto 0:294d306fd915 179
Soto 0:294d306fd915 180
Soto 0:294d306fd915 181 }//********FIN DEL PROGRAMA PRINCIPAL*************
Soto 0:294d306fd915 182 //************************************************
Soto 0:294d306fd915 183 //************************************************
Soto 0:294d306fd915 184 //************************************************
Soto 0:294d306fd915 185
Soto 0:294d306fd915 186 //**************Definicion de funciones para el correr el programa*************
Soto 0:294d306fd915 187
Soto 0:294d306fd915 188
Soto 0:294d306fd915 189 //********************Funcion de Calibracion***********************************
Soto 0:294d306fd915 190 void Calibracion(Servo Calibrando, float& range){
Soto 0:294d306fd915 191
Soto 0:294d306fd915 192 range = 0.0008;
Soto 0:294d306fd915 193 float position = 0.5;
Soto 0:294d306fd915 194 int Char=0;
Soto 0:294d306fd915 195
Soto 0:294d306fd915 196 do{
Soto 0:294d306fd915 197 Char=Casting();
Soto 0:294d306fd915 198
Soto 0:294d306fd915 199 switch(Char) {
Soto 0:294d306fd915 200 case 1: position = 0.0; break;
Soto 0:294d306fd915 201 case 2: position = 0.5; break;
Soto 0:294d306fd915 202 case 3: position = 1.0; break;
Soto 0:294d306fd915 203 case 4: range += 0.0001; break;
Soto 0:294d306fd915 204 case 5: range -= 0.0001; break;
Soto 0:294d306fd915 205 }//Close switch
Soto 0:294d306fd915 206
Soto 0:294d306fd915 207 printf("position = %.1f, range = +/-%0.4f\n", position, range);
Soto 0:294d306fd915 208 Calibrando.calibrate(range, 45.0);
Soto 0:294d306fd915 209 Calibrando = position;
Soto 0:294d306fd915 210 }while(Char != 35);//Close Do-While
Soto 0:294d306fd915 211 }
Soto 0:294d306fd915 212 //******************Fin del funciion de calibracion***********
Soto 0:294d306fd915 213
Soto 0:294d306fd915 214 //******************Funcion de Movimiento**********************
Soto 0:294d306fd915 215
Soto 0:294d306fd915 216 void Mover(Servo Movement, float& NewPosition,float Position){
Soto 0:294d306fd915 217
Soto 0:294d306fd915 218 NewPosition=Position;
Soto 0:294d306fd915 219 int Char=0;
Soto 0:294d306fd915 220
Soto 0:294d306fd915 221 do{
Soto 0:294d306fd915 222 Char=Casting();
Soto 0:294d306fd915 223
Soto 0:294d306fd915 224 switch(Char){
Soto 0:294d306fd915 225
Soto 0:294d306fd915 226 case 1:
Soto 0:294d306fd915 227 if(NewPosition>=0.975){
Soto 0:294d306fd915 228 printf("\n\nImposible aumentar mas la posicion\n");
Soto 0:294d306fd915 229 }else{
Soto 0:294d306fd915 230 NewPosition += Changing;
Soto 0:294d306fd915 231 printf("\n Valor de la posicion aumentada = %f", NewPosition);
Soto 0:294d306fd915 232 }
Soto 0:294d306fd915 233 break;
Soto 0:294d306fd915 234
Soto 0:294d306fd915 235 case 3:
Soto 0:294d306fd915 236 if(NewPosition<=0.0){
Soto 0:294d306fd915 237 printf("\n\nImposible reducir mas la posicion\n");
Soto 0:294d306fd915 238 }else{
Soto 0:294d306fd915 239 NewPosition -= Changing;
Soto 0:294d306fd915 240 printf("\n Valor de la posicion reducida = %f", NewPosition);
Soto 0:294d306fd915 241 }
Soto 0:294d306fd915 242 break;
Soto 0:294d306fd915 243 case 4://**********Escribir Secuencia**************
Soto 0:294d306fd915 244 Char=35;
Soto 0:294d306fd915 245 break;
Soto 0:294d306fd915 246 default:
Soto 0:294d306fd915 247 printf("\nOperacion invalida, intente de nuevo\n");
Soto 0:294d306fd915 248 break;
Soto 0:294d306fd915 249 }//close swithc case.
Soto 0:294d306fd915 250
Soto 0:294d306fd915 251 Movement=NewPosition;
Soto 0:294d306fd915 252 }while(Char!=35);
Soto 0:294d306fd915 253 }
Soto 0:294d306fd915 254
Soto 0:294d306fd915 255 //**********Funcion para correr rutina***********************
Soto 0:294d306fd915 256 void Corriendo(int size, int iteraciones)
Soto 0:294d306fd915 257 {
Soto 0:294d306fd915 258 for(int j=0;j<=iteraciones-1;j++){
Soto 0:294d306fd915 259
Soto 1:d86f732b4db0 260 Garra=0.7;
Soto 1:d86f732b4db0 261 SlowMotion(DynHombro[size-1],0.5,Hombro);
Soto 1:d86f732b4db0 262 Muneca=0.5;
Soto 1:d86f732b4db0 263 Codo=0.5;
Soto 1:d86f732b4db0 264 Eje=0.5;
Soto 1:d86f732b4db0 265
Soto 0:294d306fd915 266 for(int i=0;i<=size-1;i++)
Soto 0:294d306fd915 267 {
Soto 1:d86f732b4db0 268 Hombro=DynHombro[(i-1)];
Soto 1:d86f732b4db0 269 wait(0.5);
Soto 1:d86f732b4db0 270 //SlowMotion(DynEje[i],DynEje[i-1],Eje);
Soto 1:d86f732b4db0 271 Eje=DynEje[i];
Soto 0:294d306fd915 272 wait(0.5);
Soto 1:d86f732b4db0 273 //SlowMotion(DynMuneca[i],DynMuneca[i-1],Muneca);
Soto 0:294d306fd915 274 Muneca=DynMuneca[i];
Soto 0:294d306fd915 275 wait(0.5);
Soto 0:294d306fd915 276 Codo=DynCodo[i];
Soto 1:d86f732b4db0 277 //SlowMotion(DynCodo[i],DynCodo[i-1],Codo);
Soto 0:294d306fd915 278 wait(0.5);
Soto 1:d86f732b4db0 279 //SlowMotion(DynHombro[i],DynHombro[i-1],Hombro);
Soto 0:294d306fd915 280 Hombro=DynHombro[i];
Soto 1:d86f732b4db0 281 wait(0.5);
Soto 1:d86f732b4db0 282 Garra=DynGarra[i];
Soto 1:d86f732b4db0 283 wait(0.5);
Soto 0:294d306fd915 284 }//Fin del ciclo for
Soto 0:294d306fd915 285 printf("\nTerminada la iteracion %d", j+1);
Soto 1:d86f732b4db0 286
Soto 1:d86f732b4db0 287 }
Soto 0:294d306fd915 288 printf("\n\n\nTerminadas las iteraciones especificadas\n\n");
Soto 0:294d306fd915 289 }//Fin de la funcion
Soto 0:294d306fd915 290
Soto 0:294d306fd915 291 //Fin de la funcion**********************************
Soto 0:294d306fd915 292
Soto 0:294d306fd915 293 int Casting(){
Soto 0:294d306fd915 294 char Character;
Soto 0:294d306fd915 295 Pointer:
Soto 0:294d306fd915 296 Character=pc.getc();
Soto 0:294d306fd915 297 int Casted=0;
Soto 0:294d306fd915 298 Casted=(int)Character;
Soto 0:294d306fd915 299 if((Casted<=57)&&(Casted>=48)){
Soto 0:294d306fd915 300 Casted=Casted-48;
Soto 0:294d306fd915 301 }else if((Casted<=90)&&(Casted>=65)){
Soto 0:294d306fd915 302 Casted=Casted-55;
Soto 0:294d306fd915 303 }else if((Casted<=122)&&(Casted>=97)){
Soto 0:294d306fd915 304 Casted=Casted-87;
Soto 0:294d306fd915 305 }else{
Soto 0:294d306fd915 306 printf("\nOperacion no valida, vuelva a intentar\n");
Soto 0:294d306fd915 307 goto Pointer;
Soto 0:294d306fd915 308 }
Soto 0:294d306fd915 309 return Casted;
Soto 0:294d306fd915 310 }
Soto 0:294d306fd915 311
Soto 0:294d306fd915 312 //*****Ir a la ultima posicion-*********
Soto 0:294d306fd915 313
Soto 0:294d306fd915 314 void Ultima_Posicion()
Soto 0:294d306fd915 315 {
Soto 0:294d306fd915 316 int PositionSize=(DynGarra.size()-1);
Soto 0:294d306fd915 317
Soto 0:294d306fd915 318 Garra=DynGarra[PositionSize];
Soto 0:294d306fd915 319 wait(0.5);
Soto 0:294d306fd915 320 Muneca=DynMuneca[PositionSize];
Soto 0:294d306fd915 321 wait(0.5);
Soto 0:294d306fd915 322 Codo=DynMuneca[PositionSize];
Soto 0:294d306fd915 323 wait(0.5);
Soto 0:294d306fd915 324 Hombro=DynHombro[PositionSize];
Soto 0:294d306fd915 325 wait(0.5);
Soto 0:294d306fd915 326 Eje=DynEje[PositionSize];
Soto 0:294d306fd915 327 wait(0.5);
Soto 0:294d306fd915 328 }
Soto 0:294d306fd915 329
Soto 0:294d306fd915 330 //******Slow move********
Soto 0:294d306fd915 331
Soto 0:294d306fd915 332 void SlowMotion(float NewPos, float AlmostLast,Servo LowServo)
Soto 0:294d306fd915 333 {
Soto 0:294d306fd915 334 float temp=AlmostLast;
Soto 1:d86f732b4db0 335
Soto 1:d86f732b4db0 336 do{
Soto 1:d86f732b4db0 337 if(NewPos<AlmostLast)
Soto 1:d86f732b4db0 338 {
Soto 1:d86f732b4db0 339 temp-=Changing;
Soto 1:d86f732b4db0 340 LowServo=temp;
Soto 1:d86f732b4db0 341 wait_ms(10);
Soto 1:d86f732b4db0 342 }
Soto 1:d86f732b4db0 343
Soto 1:d86f732b4db0 344 else if(NewPos>AlmostLast)
Soto 1:d86f732b4db0 345 {
Soto 1:d86f732b4db0 346 temp+=Changing;
Soto 1:d86f732b4db0 347 LowServo=temp;
Soto 1:d86f732b4db0 348 wait_ms(10);
Soto 1:d86f732b4db0 349 }
Soto 1:d86f732b4db0 350 }while(NewPos!=AlmostLast);
Soto 1:d86f732b4db0 351
Soto 1:d86f732b4db0 352 }
Soto 1:d86f732b4db0 353
Soto 1:d86f732b4db0 354 void Home(void)
Soto 1:d86f732b4db0 355 {
Soto 1:d86f732b4db0 356 Garra=0.975;
Soto 1:d86f732b4db0 357 wait(0.2);
Soto 0:294d306fd915 358
Soto 1:d86f732b4db0 359 Hombro=0.5;
Soto 1:d86f732b4db0 360 wait(0.2);
Soto 1:d86f732b4db0 361
Soto 1:d86f732b4db0 362 Eje=0.5;
Soto 1:d86f732b4db0 363 wait(0.2);
Soto 1:d86f732b4db0 364
Soto 1:d86f732b4db0 365 Codo=0.5;
Soto 1:d86f732b4db0 366 wait(0.2);
Soto 1:d86f732b4db0 367
Soto 1:d86f732b4db0 368 Muneca=0.5;
Soto 1:d86f732b4db0 369 wait(0.2);
Soto 0:294d306fd915 370 }