Ricardo Soto
/
IMTarm
Programa para los taller IMTchallenge
main.cpp@1:d86f732b4db0, 2015-10-16 (annotated)
- 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?
User | Revision | Line number | New 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 | } |