Ricardo Soto / Mbed 2 deprecated IMTarm

Programa para los taller IMTchallenge

Dependencies:   Servo mbed

main.cpp@0:294d306fd915, 2015-10-15 (annotated)

Committer:
Soto
Date:
Thu Oct 15 17:24:09 2015 +0000
Revision:
0:294d306fd915
Child:
1:d86f732b4db0
Programa para controlar un brazo mecanico de 5 grados de libertad

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 0:294d306fd915 46
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 0:294d306fd915 66 DynGarra.push_back(0.5);
Soto 0:294d306fd915 67
Soto 0:294d306fd915 68 Garra=DynGarra[0];
Soto 0:294d306fd915 69 Muneca=DynMuneca[0];
Soto 0:294d306fd915 70 Codo=DynCodo[0];
Soto 0:294d306fd915 71 Hombro=DynCodo[0];
Soto 0:294d306fd915 72 Eje=DynEje[0];
Soto 0:294d306fd915 73
Soto 0:294d306fd915 74 while(1){ //Loop infinito
Soto 0:294d306fd915 75
Soto 0:294d306fd915 76 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");
Soto 0:294d306fd915 77 Casted=Casting();
Soto 0:294d306fd915 78 switch(Casted){
Soto 0:294d306fd915 79
Soto 0:294d306fd915 80 case 12:
Soto 0:294d306fd915 81 printf("\nServo Calibration Controls:\n\n");
Soto 0:294d306fd915 82 printf("1,2,3 - Position Servo (full left, middle, full right)\n");
Soto 0:294d306fd915 83 printf("4,5 - Decrease or Increase range\n");
Soto 0:294d306fd915 84 printf("Oprima 'Z' para salir de la etapa de calibracion\n");
Soto 0:294d306fd915 85 printf("\nCalibrando el motor de la garra\n\n");
Soto 0:294d306fd915 86 Calibracion(Garra,RangeGarra);
Soto 0:294d306fd915 87 printf("\n\nCalibrando el motor de la Muneca\n\n");
Soto 0:294d306fd915 88 Calibracion(Muneca,RangeMuneca);
Soto 0:294d306fd915 89 printf("\n\nCalibrando el motor del Codo\n\n");
Soto 0:294d306fd915 90 Calibracion(Codo,RangeCodo);
Soto 0:294d306fd915 91 printf("\n\nCalibrando el motor del Hombro\n\n");
Soto 0:294d306fd915 92 Calibracion(Hombro,RangeHombro);
Soto 0:294d306fd915 93 printf("\n\nCalibrando el motor del Eje\n\n");
Soto 0:294d306fd915 94 Calibracion(Hombro,RangeEje);
Soto 0:294d306fd915 95 //*************Parametros***********************
Soto 0:294d306fd915 96 Garra.calibrate(RangeGarra, 45.0);
Soto 0:294d306fd915 97 printf("\nValor de RangeGarra: %f",RangeGarra);
Soto 0:294d306fd915 98 Muneca.calibrate(RangeMuneca, 45.0);
Soto 0:294d306fd915 99 printf("\nValor de RangeMuneca: %f",RangeMuneca);
Soto 0:294d306fd915 100 Codo.calibrate(RangeCodo, 45.0);
Soto 0:294d306fd915 101 printf("\nValor de RangeCodo: %f",RangeCodo);
Soto 0:294d306fd915 102 Hombro.calibrate(RangeHombro, 45.0);
Soto 0:294d306fd915 103 printf("\nValor de RangeHombro: %f",RangeHombro);
Soto 0:294d306fd915 104 Eje.calibrate(RangeEje, 45.0);
Soto 0:294d306fd915 105
Soto 0:294d306fd915 106 break;
Soto 0:294d306fd915 107
Soto 0:294d306fd915 108 case 22:
Soto 0:294d306fd915 109 printf("\nIncializando la secuencia de movimiento de ejes\n"\
Soto 0:294d306fd915 110 "Presione '1' para aumentar la posicion\n"\
Soto 0:294d306fd915 111 "Presion '3' para reducir la posicion\n"\
Soto 0:294d306fd915 112 "Presione '4' para guardad valores en la rutina");
Soto 0:294d306fd915 113
Soto 0:294d306fd915 114 do{
Soto 0:294d306fd915 115
Soto 0:294d306fd915 116 printf("\n\nMoviendo eje de Eje\n");
Soto 0:294d306fd915 117 Mover(Eje, DynPosEje,DynPosEje);
Soto 0:294d306fd915 118 printf("\n\nNuevo valor de DynPosEje %f", DynPosEje);
Soto 0:294d306fd915 119 DynEje.push_back(DynPosEje);
Soto 0:294d306fd915 120
Soto 0:294d306fd915 121 printf("\n\nMoviendo eje de Muneca\n");
Soto 0:294d306fd915 122 Mover(Muneca,DynPosMuneca,DynPosMuneca);
Soto 0:294d306fd915 123 printf("\n\nNuevo valor de DynPosMuneca %f",DynPosMuneca);
Soto 0:294d306fd915 124 DynMuneca.push_back(DynPosMuneca);
Soto 0:294d306fd915 125
Soto 0:294d306fd915 126 printf("\n\nMoviendo eje de Codo\n");
Soto 0:294d306fd915 127 Mover(Codo,DynPosCodo,DynPosCodo);
Soto 0:294d306fd915 128 printf("\n\nNuevo valor de DynPosCodo %f",DynPosCodo);
Soto 0:294d306fd915 129 DynCodo.push_back(DynPosCodo);
Soto 0:294d306fd915 130
Soto 0:294d306fd915 131 printf("\n\nMoviendo eje de Hombro\n");
Soto 0:294d306fd915 132 Mover(Hombro,DynPosHombro,DynPosHombro);
Soto 0:294d306fd915 133 printf("\n\nNuevo valor de DynPosHombro %f",DynPosHombro);
Soto 0:294d306fd915 134 DynHombro.push_back(DynPosHombro);
Soto 0:294d306fd915 135
Soto 0:294d306fd915 136 printf("\n\nMoviendo eje de Garra\n");
Soto 0:294d306fd915 137 Mover(Garra, DynPosGarra,DynPosGarra);
Soto 0:294d306fd915 138 printf("\n\nNuevo valor de DynPosGarra %f", DynPosGarra);
Soto 0:294d306fd915 139 DynGarra.push_back(DynPosGarra);
Soto 0:294d306fd915 140
Soto 0:294d306fd915 141 do{
Soto 0:294d306fd915 142 printf("\n\n\tDesea agregar otra posicion? (Y/N)\n");
Soto 0:294d306fd915 143 printf("\n\n\tCantidad de posiciones es: %d\n\n", Size=DynGarra.size());
Soto 0:294d306fd915 144 Options=Casting();
Soto 0:294d306fd915 145 if((Options==23)|(Options==34)){
Soto 0:294d306fd915 146 MoveOn=1;}
Soto 0:294d306fd915 147 else{
Soto 0:294d306fd915 148 printf("\n Operacion no valida\n Por favor especifique si quiere ingresar otra posicion\n");
Soto 0:294d306fd915 149 MoveOn=0;}
Soto 0:294d306fd915 150 }while(MoveOn==0);
Soto 0:294d306fd915 151 }while(Options!= 23);
Soto 0:294d306fd915 152 break;
Soto 0:294d306fd915 153
Soto 0:294d306fd915 154 case 27:
Soto 0:294d306fd915 155
Soto 0:294d306fd915 156 printf("\nIntroduzca cuantas veces quiere repetir la rutina"\
Soto 0:294d306fd915 157 "\nPara cantidades mayores a 10 siga la logica:"\
Soto 0:294d306fd915 158 "\nA=10,B=11,C=12...");
Soto 0:294d306fd915 159 Casted=Casting();
Soto 0:294d306fd915 160 Size=DynGarra.size();
Soto 0:294d306fd915 161 printf("\nCantidad de posiciones= %d e iteraciones %d",Size,Casted);
Soto 0:294d306fd915 162 Corriendo(Size,Casted);
Soto 0:294d306fd915 163 break;
Soto 0:294d306fd915 164
Soto 0:294d306fd915 165 case 30:
Soto 0:294d306fd915 166 Ultima_Posicion();
Soto 0:294d306fd915 167 break;
Soto 0:294d306fd915 168
Soto 0:294d306fd915 169 default:
Soto 0:294d306fd915 170 printf("\nEl comando es invalido, ingrese un comando valido\n");
Soto 0:294d306fd915 171 break;
Soto 0:294d306fd915 172 }//Fin del Switch case
Soto 0:294d306fd915 173 }//Fin del Loop infinito
Soto 0:294d306fd915 174
Soto 0:294d306fd915 175
Soto 0:294d306fd915 176 }//********FIN DEL PROGRAMA PRINCIPAL*************
Soto 0:294d306fd915 177 //************************************************
Soto 0:294d306fd915 178 //************************************************
Soto 0:294d306fd915 179 //************************************************
Soto 0:294d306fd915 180
Soto 0:294d306fd915 181 //**************Definicion de funciones para el correr el programa*************
Soto 0:294d306fd915 182
Soto 0:294d306fd915 183
Soto 0:294d306fd915 184 //********************Funcion de Calibracion***********************************
Soto 0:294d306fd915 185 void Calibracion(Servo Calibrando, float& range){
Soto 0:294d306fd915 186
Soto 0:294d306fd915 187 range = 0.0008;
Soto 0:294d306fd915 188 float position = 0.5;
Soto 0:294d306fd915 189 int Char=0;
Soto 0:294d306fd915 190
Soto 0:294d306fd915 191 do{
Soto 0:294d306fd915 192 Char=Casting();
Soto 0:294d306fd915 193
Soto 0:294d306fd915 194 switch(Char) {
Soto 0:294d306fd915 195 case 1: position = 0.0; break;
Soto 0:294d306fd915 196 case 2: position = 0.5; break;
Soto 0:294d306fd915 197 case 3: position = 1.0; break;
Soto 0:294d306fd915 198 case 4: range += 0.0001; break;
Soto 0:294d306fd915 199 case 5: range -= 0.0001; break;
Soto 0:294d306fd915 200 }//Close switch
Soto 0:294d306fd915 201
Soto 0:294d306fd915 202 printf("position = %.1f, range = +/-%0.4f\n", position, range);
Soto 0:294d306fd915 203 Calibrando.calibrate(range, 45.0);
Soto 0:294d306fd915 204 Calibrando = position;
Soto 0:294d306fd915 205 }while(Char != 35);//Close Do-While
Soto 0:294d306fd915 206 }
Soto 0:294d306fd915 207 //******************Fin del funciion de calibracion***********
Soto 0:294d306fd915 208
Soto 0:294d306fd915 209 //******************Funcion de Movimiento**********************
Soto 0:294d306fd915 210
Soto 0:294d306fd915 211 void Mover(Servo Movement, float& NewPosition,float Position){
Soto 0:294d306fd915 212
Soto 0:294d306fd915 213 NewPosition=Position;
Soto 0:294d306fd915 214 int Char=0;
Soto 0:294d306fd915 215
Soto 0:294d306fd915 216 do{
Soto 0:294d306fd915 217 Char=Casting();
Soto 0:294d306fd915 218
Soto 0:294d306fd915 219 switch(Char){
Soto 0:294d306fd915 220
Soto 0:294d306fd915 221 case 1:
Soto 0:294d306fd915 222 if(NewPosition>=0.975){
Soto 0:294d306fd915 223 printf("\n\nImposible aumentar mas la posicion\n");
Soto 0:294d306fd915 224 }else{
Soto 0:294d306fd915 225 NewPosition += Changing;
Soto 0:294d306fd915 226 printf("\n Valor de la posicion aumentada = %f", NewPosition);
Soto 0:294d306fd915 227 }
Soto 0:294d306fd915 228 break;
Soto 0:294d306fd915 229
Soto 0:294d306fd915 230 case 3:
Soto 0:294d306fd915 231 if(NewPosition<=0.0){
Soto 0:294d306fd915 232 printf("\n\nImposible reducir mas la posicion\n");
Soto 0:294d306fd915 233 }else{
Soto 0:294d306fd915 234 NewPosition -= Changing;
Soto 0:294d306fd915 235 printf("\n Valor de la posicion reducida = %f", NewPosition);
Soto 0:294d306fd915 236 }
Soto 0:294d306fd915 237 break;
Soto 0:294d306fd915 238 case 4://**********Escribir Secuencia**************
Soto 0:294d306fd915 239 Char=35;
Soto 0:294d306fd915 240 break;
Soto 0:294d306fd915 241 default:
Soto 0:294d306fd915 242 printf("\nOperacion invalida, intente de nuevo\n");
Soto 0:294d306fd915 243 break;
Soto 0:294d306fd915 244 }//close swithc case.
Soto 0:294d306fd915 245
Soto 0:294d306fd915 246 Movement=NewPosition;
Soto 0:294d306fd915 247 }while(Char!=35);
Soto 0:294d306fd915 248 }
Soto 0:294d306fd915 249
Soto 0:294d306fd915 250 //**********Funcion para correr rutina***********************
Soto 0:294d306fd915 251 void Corriendo(int size, int iteraciones)
Soto 0:294d306fd915 252 {
Soto 0:294d306fd915 253 for(int j=0;j<=iteraciones-1;j++){
Soto 0:294d306fd915 254
Soto 0:294d306fd915 255 for(int i=0;i<=size-1;i++)
Soto 0:294d306fd915 256 {
Soto 0:294d306fd915 257 wait(0.5);
Soto 0:294d306fd915 258 //Garra=DynGarra[i];
Soto 0:294d306fd915 259 SlowMotion(DynGarra[i], DynGarra[i-1],Garra);
Soto 0:294d306fd915 260
Soto 0:294d306fd915 261 wait(0.5);
Soto 0:294d306fd915 262 Muneca=DynMuneca[i];
Soto 0:294d306fd915 263 wait(0.5);
Soto 0:294d306fd915 264 Codo=DynCodo[i];
Soto 0:294d306fd915 265 wait(0.5);
Soto 0:294d306fd915 266 Hombro=DynHombro[i];
Soto 0:294d306fd915 267 wait(0.5);
Soto 0:294d306fd915 268 }//Fin del ciclo for
Soto 0:294d306fd915 269 printf("\nTerminada la iteracion %d", j+1);
Soto 0:294d306fd915 270 }
Soto 0:294d306fd915 271 printf("\n\n\nTerminadas las iteraciones especificadas\n\n");
Soto 0:294d306fd915 272 }//Fin de la funcion
Soto 0:294d306fd915 273
Soto 0:294d306fd915 274 //Fin de la funcion**********************************
Soto 0:294d306fd915 275
Soto 0:294d306fd915 276 int Casting(){
Soto 0:294d306fd915 277 char Character;
Soto 0:294d306fd915 278 Pointer:
Soto 0:294d306fd915 279 Character=pc.getc();
Soto 0:294d306fd915 280 int Casted=0;
Soto 0:294d306fd915 281 Casted=(int)Character;
Soto 0:294d306fd915 282 if((Casted<=57)&&(Casted>=48)){
Soto 0:294d306fd915 283 Casted=Casted-48;
Soto 0:294d306fd915 284 }else if((Casted<=90)&&(Casted>=65)){
Soto 0:294d306fd915 285 Casted=Casted-55;
Soto 0:294d306fd915 286 }else if((Casted<=122)&&(Casted>=97)){
Soto 0:294d306fd915 287 Casted=Casted-87;
Soto 0:294d306fd915 288 }else{
Soto 0:294d306fd915 289 printf("\nOperacion no valida, vuelva a intentar\n");
Soto 0:294d306fd915 290 goto Pointer;
Soto 0:294d306fd915 291 }
Soto 0:294d306fd915 292 return Casted;
Soto 0:294d306fd915 293 }
Soto 0:294d306fd915 294
Soto 0:294d306fd915 295 //*****Ir a la ultima posicion-*********
Soto 0:294d306fd915 296
Soto 0:294d306fd915 297 void Ultima_Posicion()
Soto 0:294d306fd915 298 {
Soto 0:294d306fd915 299 int PositionSize=(DynGarra.size()-1);
Soto 0:294d306fd915 300
Soto 0:294d306fd915 301 Garra=DynGarra[PositionSize];
Soto 0:294d306fd915 302 wait(0.5);
Soto 0:294d306fd915 303 Muneca=DynMuneca[PositionSize];
Soto 0:294d306fd915 304 wait(0.5);
Soto 0:294d306fd915 305 Codo=DynMuneca[PositionSize];
Soto 0:294d306fd915 306 wait(0.5);
Soto 0:294d306fd915 307 Hombro=DynHombro[PositionSize];
Soto 0:294d306fd915 308 wait(0.5);
Soto 0:294d306fd915 309 Eje=DynEje[PositionSize];
Soto 0:294d306fd915 310 wait(0.5);
Soto 0:294d306fd915 311 }
Soto 0:294d306fd915 312
Soto 0:294d306fd915 313 //******Slow move********
Soto 0:294d306fd915 314
Soto 0:294d306fd915 315 void SlowMotion(float NewPos, float AlmostLast,Servo LowServo)
Soto 0:294d306fd915 316 {
Soto 0:294d306fd915 317 float temp=AlmostLast;
Soto 0:294d306fd915 318
Soto 0:294d306fd915 319 if(NewPos<AlmostLast)
Soto 0:294d306fd915 320 {
Soto 0:294d306fd915 321 for(int i=0;i<4;i++)
Soto 0:294d306fd915 322 {
Soto 0:294d306fd915 323 temp-=0.01;
Soto 0:294d306fd915 324 LowServo=temp;
Soto 0:294d306fd915 325 wait_ms(10);
Soto 0:294d306fd915 326 }
Soto 0:294d306fd915 327 }
Soto 0:294d306fd915 328
Soto 0:294d306fd915 329 if(NewPos>AlmostLast)
Soto 0:294d306fd915 330 {
Soto 0:294d306fd915 331 for(int i=0;i<3;i++)
Soto 0:294d306fd915 332 {
Soto 0:294d306fd915 333 temp+=0.01;
Soto 0:294d306fd915 334 LowServo=temp;
Soto 0:294d306fd915 335 wait_ms(10);
Soto 0:294d306fd915 336 }
Soto 0:294d306fd915 337 }
Soto 0:294d306fd915 338 }