![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Programa para los taller IMTchallenge
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?
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 | 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 | } |