Piccolo / Mbed 2 deprecated ejemplo_serial_protocol_array

Piccolo

Dependencies:   mbed

Fork of 02_LAB_serial_protocol by ferney alberto beltran molina

main.cpp@14:f217d9874f8f, 2017-10-21 (annotated)

Committer:
Vanessa620
Date:
Sat Oct 21 00:16:15 2017 +0000
Revision:
14:f217d9874f8f
Parent:
13:94013a19713a
Child:
15:ddcdd0f50b2b
Modificacion 20 Oct 17

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Vanessa620 13:94013a19713a 1 //*****************************LIBRERIAS***************************************/
Vanessa620 13:94013a19713a 2
fabeltranm 0:55d11eeb0faf 3 #include "mbed.h"
Vanessa620 12:92abcf61971b 4 #define DEBUG 1
fabeltranm 0:55d11eeb0faf 5
Vanessa620 13:94013a19713a 6 /**************PUERTOS I/O DE SISTEMA EMBEBIDO NUCLEO-F411RE*******************/
Vanessa620 13:94013a19713a 7
fabeltranm 0:55d11eeb0faf 8 Serial command(USBTX, USBRX);
fabeltranm 2:200a9507b696 9 DigitalOut led(LED1);
Vanessa620 12:92abcf61971b 10 PwmOut myServoX(PB_3); //Servo X
Vanessa620 13:94013a19713a 11 PwmOut myServoY(PB_4); //Servo Y
Vanessa620 14:f217d9874f8f 12 PwmOut myServoZ(PC_7); //Servo Z
Vanessa620 12:92abcf61971b 13
fabeltranm 0:55d11eeb0faf 14 //*****************************************************************************
Vanessa620 13:94013a19713a 15 // COMANDO MOVER MOTOR
Vanessa620 13:94013a19713a 16 //
fabeltranm 0:55d11eeb0faf 17 // |POS 1|POS 2|POS 3|POS 4| POS 5|
fabeltranm 7:fab201aa45b7 18 // | < | #C | a | b | > |
Vanessa620 11:b4a3db5a0305 19 // | 3C |0 al A| D1 | D2 | D3 | D4 | 3E |
Vanessa620 13:94013a19713a 20 // #C -> indica el comando. En este caso es de 0 a 10 (en hexadesimal)
fabeltranm 6:cb6b868465c3 21 // a,b,c,d parametros del comando
fabeltranm 6:cb6b868465c3 22 // <,> -> inicio, y fin de comando
fabeltranm 0:55d11eeb0faf 23 // el inicio de comando no se almacena en el buffer
fabeltranm 0:55d11eeb0faf 24 //*****************************************************************************
fabeltranm 0:55d11eeb0faf 25
Vanessa620 13:94013a19713a 26 //***************************VARIABLES DEFINIDAS*******************************/
Vanessa620 13:94013a19713a 27
Vanessa620 12:92abcf61971b 28 #define BUFF_SIZE 6 // tamaño del comando enviado desde CoolTerm
Vanessa620 12:92abcf61971b 29 #define COMM_N 0 //
Vanessa620 12:92abcf61971b 30 #define INITPARAMETER 1 //
Vanessa620 12:92abcf61971b 31 #define MAXPOS 50 // Posicion maxima de la matriz de dibujo x,y en mm
Vanessa620 14:f217d9874f8f 32 #define POSDRAW 30 // Posicion del servomotor Z para dibujar
Vanessa620 13:94013a19713a 33 //#define SS_TIME 100 // no puedo degarla declarada cte. pues se debe mod por teclado
Vanessa620 12:92abcf61971b 34 #define PI 3.1415926 //
Vanessa620 13:94013a19713a 35 uint8_t RSTEP = 5; // Ini. variable de Resolucion para el dibujo
Vanessa620 13:94013a19713a 36 uint8_t DET=1;
Vanessa620 13:94013a19713a 37 uint8_t posx_old=0; // posición anterior del eje X
Vanessa620 13:94013a19713a 38 uint8_t posy_old=0; // posición anterior del eje Y
Vanessa620 13:94013a19713a 39 uint8_t SSTIME = 100;
Vanessa620 11:b4a3db5a0305 40
Vanessa620 13:94013a19713a 41 /****************************COMANDOS DE TECLADO*******************************/
Vanessa620 13:94013a19713a 42
Vanessa620 11:b4a3db5a0305 43 #define LED_NC 0 //ok
Vanessa620 11:b4a3db5a0305 44 #define DOT_NC 1 //ok
Vanessa620 11:b4a3db5a0305 45 #define LINE_NC 2 //ok
Vanessa620 12:92abcf61971b 46 #define RECTANGULO_NC 3 //ok
Vanessa620 13:94013a19713a 47 #define CIRCLE_NC 4 //ok
Vanessa620 11:b4a3db5a0305 48 #define HOME_NC 5 //ok
Vanessa620 13:94013a19713a 49 #define RESOLUCION_NC 6 //ok
Vanessa620 13:94013a19713a 50 #define TIEMPOPASOS_NC 7 // No se a que se refiere.
Vanessa620 12:92abcf61971b 51 #define STOP_NC 8 // Pendiente
Vanessa620 12:92abcf61971b 52 #define PAUSA_NC 9 // Pendiente
Vanessa620 13:94013a19713a 53 #define REANUDAR_NC 10 // Pendiente
Vanessa620 13:94013a19713a 54 //#define MOVER_NC 11 //Se refiere a el movimiento del motor paso a paso
Vanessa620 11:b4a3db5a0305 55
fabeltranm 7:fab201aa45b7 56
Vanessa620 13:94013a19713a 57 /**************FUNCIONES PARA MOVER LOS SERVOS X, Y, Z*************************/
Vanessa620 13:94013a19713a 58
Vanessa620 12:92abcf61971b 59 int coord2pulse(float coord) //
Vanessa620 12:92abcf61971b 60 {
manuelitoys 8:38ae341e2a4f 61 if(0 <= coord <= MAXPOS)
manuelitoys 8:38ae341e2a4f 62 return int(750+coord*1900/50);// u6
Vanessa620 14:f217d9874f8f 63 return 750;
Vanessa620 12:92abcf61971b 64 }
Vanessa620 12:92abcf61971b 65
Vanessa620 12:92abcf61971b 66 void vertex2d(float x, float y) // Funcion para enviarle la posicion a (x,y)
Vanessa620 12:92abcf61971b 67 {
Vanessa620 14:f217d9874f8f 68 wait_ms(SSTIME);
manuelitoys 9:5008f9501dbf 69 int pulseY = coord2pulse(y);
Vanessa620 14:f217d9874f8f 70 int pulseX = coord2pulse(x);
manuelitoys 9:5008f9501dbf 71
Vanessa620 14:f217d9874f8f 72 myServoY.pulsewidth_us(pulseY);
manuelitoys 9:5008f9501dbf 73 myServoX.pulsewidth_us(pulseX);
Vanessa620 14:f217d9874f8f 74
Vanessa620 12:92abcf61971b 75 }
Vanessa620 12:92abcf61971b 76
Vanessa620 12:92abcf61971b 77 void draw() // Funcion para enviarle la posicion de dibujo a z.
Vanessa620 12:92abcf61971b 78 {
Vanessa620 14:f217d9874f8f 79 wait_ms(SSTIME*10);
Vanessa620 14:f217d9874f8f 80 int pulseZ=coord2pulse(POSDRAW);
Vanessa620 14:f217d9874f8f 81 myServoZ.pulsewidth_us(pulseZ);
Vanessa620 14:f217d9874f8f 82 wait_ms(SSTIME);
Vanessa620 12:92abcf61971b 83 }
Vanessa620 12:92abcf61971b 84 void nodraw() // Funcion para enviarle la posicion de no dibujar a z.
Vanessa620 14:f217d9874f8f 85 {
Vanessa620 14:f217d9874f8f 86 wait_ms(SSTIME*10);
Vanessa620 14:f217d9874f8f 87 int pulseZ=coord2pulse(0);
Vanessa620 14:f217d9874f8f 88 myServoZ.pulsewidth_us(pulseZ);
Vanessa620 14:f217d9874f8f 89
Vanessa620 12:92abcf61971b 90 }
Vanessa620 13:94013a19713a 91
Vanessa620 13:94013a19713a 92 void initdraw(float x, float y)//ok
Vanessa620 13:94013a19713a 93 {
Vanessa620 14:f217d9874f8f 94 vertex2d(x,y);
Vanessa620 14:f217d9874f8f 95 wait_ms(SSTIME);
Vanessa620 13:94013a19713a 96 draw();
Vanessa620 13:94013a19713a 97 }
fabeltranm 7:fab201aa45b7 98
Vanessa620 13:94013a19713a 99 /*****************FUNCIONES PARA OBTENER E IMPRIMIR EL COMANDO*****************/
Vanessa620 13:94013a19713a 100
Vanessa620 13:94013a19713a 101 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0}; // Matriz del Comando enviado
fabeltranm 1:0bcd96e56022 102
Vanessa620 12:92abcf61971b 103 void print_num(uint8_t val) //
Vanessa620 12:92abcf61971b 104 {
Vanessa620 12:92abcf61971b 105 if (val <10)
fabeltranm 1:0bcd96e56022 106 command.putc(val+0x30);
fabeltranm 1:0bcd96e56022 107 else
Vanessa620 12:92abcf61971b 108 command.putc(val-9+0x40);
Vanessa620 12:92abcf61971b 109 }
Vanessa620 12:92abcf61971b 110
Vanessa620 13:94013a19713a 111 void print_bin2hex (uint8_t val) // Imprimir el comando enviado en Hexadecimal
Vanessa620 12:92abcf61971b 112 {
fabeltranm 1:0bcd96e56022 113 command.printf(" 0x");
fabeltranm 1:0bcd96e56022 114 print_num(val>>4);
Vanessa620 12:92abcf61971b 115 print_num(val&0x0f);
Vanessa620 12:92abcf61971b 116 }
fabeltranm 1:0bcd96e56022 117
Vanessa620 13:94013a19713a 118 void Read_command() // Leer el comando que se digito en CoolTerm
Vanessa620 12:92abcf61971b 119 {
fabeltranm 0:55d11eeb0faf 120 for (uint8_t i=0; i<BUFF_SIZE;i++)
fabeltranm 0:55d11eeb0faf 121 buffer_command[i]=command.getc();
Vanessa620 12:92abcf61971b 122 }
fabeltranm 0:55d11eeb0faf 123
Vanessa620 12:92abcf61971b 124 void echo_command() //
Vanessa620 12:92abcf61971b 125 {
fabeltranm 0:55d11eeb0faf 126 for (uint8_t i=0; i<BUFF_SIZE;i++)
Vanessa620 12:92abcf61971b 127 print_bin2hex(buffer_command[i]);
Vanessa620 12:92abcf61971b 128 }
fabeltranm 0:55d11eeb0faf 129
Vanessa620 13:94013a19713a 130 uint8_t check_command() // Verifica el ultimo valor del comando enviado '>'
Vanessa620 12:92abcf61971b 131 {
Vanessa620 12:92abcf61971b 132 if (buffer_command[BUFF_SIZE-1]== '>'){
fabeltranm 0:55d11eeb0faf 133 #if DEBUG
Vanessa620 12:92abcf61971b 134 command.printf("\nComando: ");
Vanessa620 12:92abcf61971b 135 print_bin2hex(buffer_command[COMM_N]);
Vanessa620 12:92abcf61971b 136 command.printf(" -> ");
Vanessa620 12:92abcf61971b 137 #endif
Vanessa620 12:92abcf61971b 138 return 1;
Vanessa620 12:92abcf61971b 139 }
Vanessa620 12:92abcf61971b 140 #if DEBUG
Vanessa620 13:94013a19713a 141 command.printf("\n !!!!!!!!ERROR EN EL COMANDO!!!!!!!!!!!!!! -> ");
Vanessa620 12:92abcf61971b 142 echo_command();
fabeltranm 0:55d11eeb0faf 143 #endif
Vanessa620 12:92abcf61971b 144 return 0;
Vanessa620 12:92abcf61971b 145 }
Vanessa620 12:92abcf61971b 146
Vanessa620 13:94013a19713a 147 /********************FUNCIONES PARA DIBUJAR EN EL PICOLO************************/
Vanessa620 13:94013a19713a 148
Vanessa620 13:94013a19713a 149 void Led(int tm) //Funcion para definir el tiempo de led en milisegundos
Vanessa620 12:92abcf61971b 150 {
Vanessa620 11:b4a3db5a0305 151 #if DEBUG
Vanessa620 13:94013a19713a 152 command.printf("\nTiempo led: %i seg\n", tm);
Vanessa620 11:b4a3db5a0305 153 #endif
Vanessa620 12:92abcf61971b 154 led=1;
Vanessa620 12:92abcf61971b 155 wait(tm);
Vanessa620 12:92abcf61971b 156 led=0;
Vanessa620 11:b4a3db5a0305 157 }
Vanessa620 12:92abcf61971b 158
Vanessa620 12:92abcf61971b 159 void punto(uint8_t x, uint8_t y) //Funcion para dibujar un punto
Vanessa620 12:92abcf61971b 160 {
Vanessa620 12:92abcf61971b 161 #if DEBUG
Vanessa620 13:94013a19713a 162 command.printf("\nCoordenadas x=%i, y=%i\n",x,y);
Vanessa620 12:92abcf61971b 163 #endif
Vanessa620 13:94013a19713a 164 initdraw(x,y);
Vanessa620 11:b4a3db5a0305 165 nodraw();
Vanessa620 13:94013a19713a 166 }
Vanessa620 11:b4a3db5a0305 167
Vanessa620 13:94013a19713a 168 void linea(float xi, float yi, float xf, float yf)
Vanessa620 13:94013a19713a 169 {
Vanessa620 12:92abcf61971b 170 #if DEBUG
Vanessa620 13:94013a19713a 171 command.printf("\nCoordenadas xi=%f, yi=%f, xf=%f, yf=%f, resolucion: %i \n", xi,yi,xf,yf,RSTEP);
Vanessa620 13:94013a19713a 172 #endif
Vanessa620 13:94013a19713a 173 float xp,yp;
Vanessa620 13:94013a19713a 174 float m=(yf-yi)/(xf-xi);
Vanessa620 13:94013a19713a 175 float b=yf-(m*xf);
Vanessa620 13:94013a19713a 176 #if DEBUG
Vanessa620 13:94013a19713a 177 command.printf("\n b =%f, m=%f \n", b,m);
Vanessa620 13:94013a19713a 178 #endif
Vanessa620 13:94013a19713a 179 float nstep =(m/RSTEP);
Vanessa620 13:94013a19713a 180 //nstep=RSTEP;
Vanessa620 13:94013a19713a 181 #if DEBUG
Vanessa620 13:94013a19713a 182 command.printf("\nstep = %f \n", nstep);
Vanessa620 12:92abcf61971b 183 #endif
Vanessa620 13:94013a19713a 184 if ((abs(xf-xi))>abs(yf-yi)){
Vanessa620 13:94013a19713a 185 if (xf>xi){
Vanessa620 14:f217d9874f8f 186 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 187 for (xp=xi; xp<=xf; xp+=RSTEP){
Vanessa620 13:94013a19713a 188 yp =m*xp+b;
Vanessa620 13:94013a19713a 189 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 190 #if DEBUG
Vanessa620 13:94013a19713a 191 command.printf(" CASO 1: ( dx>dy & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 192 #endif
Vanessa620 13:94013a19713a 193 } }
Vanessa620 13:94013a19713a 194 else{
Vanessa620 13:94013a19713a 195 float temp = xi;
Vanessa620 13:94013a19713a 196 xi = xf;
Vanessa620 13:94013a19713a 197 xf = temp;
Vanessa620 14:f217d9874f8f 198 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 199 for (xp=xi; xp<=xf; xp+=RSTEP){
Vanessa620 13:94013a19713a 200 yp =m*xp+b;
Vanessa620 13:94013a19713a 201 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 202 #if DEBUG
Vanessa620 13:94013a19713a 203 command.printf(" CASO 2: ( dx>dy & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 204 #endif
Vanessa620 13:94013a19713a 205 }}}
Vanessa620 13:94013a19713a 206 else {
Vanessa620 13:94013a19713a 207 if (yf>yi){
Vanessa620 14:f217d9874f8f 208 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 209 for (yp=yi; yp<=yf; yp+=RSTEP){
Vanessa620 13:94013a19713a 210 xp=(yp-b)/m;
Vanessa620 13:94013a19713a 211 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 212 #if DEBUG
Vanessa620 13:94013a19713a 213 command.printf(" CASO 3: ( dy>dx & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 214 #endif
Vanessa620 13:94013a19713a 215 }}
Vanessa620 13:94013a19713a 216 else{
Vanessa620 13:94013a19713a 217 float tempo = yi;
Vanessa620 13:94013a19713a 218 yi = yf;
Vanessa620 13:94013a19713a 219 yf = tempo;
Vanessa620 14:f217d9874f8f 220 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 221 for (yp=yi; yp<=yf; yp+=RSTEP){
Vanessa620 13:94013a19713a 222 xp=(yp-b)/m;
Vanessa620 13:94013a19713a 223 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 224 #if DEBUG
Vanessa620 13:94013a19713a 225 command.printf(" CASO 4: ( dy>dx & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 226 #endif
Vanessa620 13:94013a19713a 227 }
Vanessa620 13:94013a19713a 228 }
Vanessa620 11:b4a3db5a0305 229 }
Vanessa620 13:94013a19713a 230 nodraw();
Vanessa620 12:92abcf61971b 231 }
Vanessa620 12:92abcf61971b 232
Vanessa620 12:92abcf61971b 233 void Rectangulo(uint8_t x, uint8_t y, uint8_t a, uint8_t h)
Vanessa620 12:92abcf61971b 234 {
Vanessa620 12:92abcf61971b 235 #if DEBUG
Vanessa620 13:94013a19713a 236 command.printf("\nCoordenadas x=%i, y=%i, ancho=%i, alto=%i, resolucion=%i\n", x,y,a,h,RSTEP);
Vanessa620 12:92abcf61971b 237 #endif
Vanessa620 13:94013a19713a 238 initdraw(x,y);
Vanessa620 13:94013a19713a 239
Vanessa620 14:f217d9874f8f 240 for(uint8_t xi=x; xi<=(x+a); xi+=RSTEP){
Vanessa620 13:94013a19713a 241 vertex2d(xi,y);
Vanessa620 12:92abcf61971b 242 #if DEBUG
Vanessa620 13:94013a19713a 243 command.printf("Coordenadas x=%i,y=%i\n", xi,y);
Vanessa620 13:94013a19713a 244 #endif
Vanessa620 13:94013a19713a 245 }
Vanessa620 13:94013a19713a 246 for (uint8_t yi=y; yi<=(y+h); yi+=RSTEP){
Vanessa620 13:94013a19713a 247 vertex2d(x+a,yi);
Vanessa620 12:92abcf61971b 248 #if DEBUG
Vanessa620 13:94013a19713a 249 command.printf("Coordenadas x=%i,y=%i\n", x+a,yi);
Vanessa620 13:94013a19713a 250 #endif
Vanessa620 12:92abcf61971b 251 }
Vanessa620 14:f217d9874f8f 252 for( uint8_t xf=(x+a); xf>=x; xf-=RSTEP){
Vanessa620 14:f217d9874f8f 253 vertex2d(xf,y+h);
Vanessa620 12:92abcf61971b 254 #if DEBUG
Vanessa620 14:f217d9874f8f 255 command.printf("Coordenadas x=%i,y=%i\n", xf,y+h);
Vanessa620 13:94013a19713a 256 #endif
Vanessa620 13:94013a19713a 257 }
Vanessa620 14:f217d9874f8f 258 for ( uint8_t yf=(y+h); yf>=y; yf-=RSTEP){
Vanessa620 14:f217d9874f8f 259 vertex2d(x,yf);
Vanessa620 13:94013a19713a 260 #if DEBUG
Vanessa620 14:f217d9874f8f 261 command.printf("Coordenadas x=%i,y=%i\n", x,yf);
Vanessa620 13:94013a19713a 262 #endif
Vanessa620 13:94013a19713a 263 }
Vanessa620 13:94013a19713a 264 nodraw();
Vanessa620 10:3eab080dbc12 265 }
Vanessa620 11:b4a3db5a0305 266
Vanessa620 10:3eab080dbc12 267 void circle(uint8_t cx, uint8_t cy, uint8_t radio)
Vanessa620 10:3eab080dbc12 268 {
Vanessa620 10:3eab080dbc12 269 int y;
Vanessa620 10:3eab080dbc12 270 int x;
Vanessa620 13:94013a19713a 271 vertex2d(cx,cy);
Vanessa620 13:94013a19713a 272 #if DEBUG
Vanessa620 13:94013a19713a 273 command.printf("\nCoordenadas xc =%i, yc =%i, Radio=%i \n",cx,cy,radio);
Vanessa620 13:94013a19713a 274 #endif
Vanessa620 11:b4a3db5a0305 275
Vanessa620 13:94013a19713a 276 for(double i=0; i<=PI/2 ;i+=((PI/2)/RSTEP))
Vanessa620 13:94013a19713a 277 {
Vanessa620 13:94013a19713a 278 x=radio*cos(i);
Vanessa620 13:94013a19713a 279 y=radio*sin(i);
Vanessa620 13:94013a19713a 280 initdraw(x+cx,y+cy);
Vanessa620 13:94013a19713a 281 #if DEBUG
Vanessa620 13:94013a19713a 282 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
Vanessa620 13:94013a19713a 283 #endif
Vanessa620 13:94013a19713a 284 }
Vanessa620 13:94013a19713a 285 #if DEBUG
Vanessa620 13:94013a19713a 286 command.printf("\n");
Vanessa620 13:94013a19713a 287 #endif
Vanessa620 13:94013a19713a 288 for(double i=PI/2; i<=PI ;i+=((PI/2)/RSTEP))
Vanessa620 12:92abcf61971b 289 {
Vanessa620 10:3eab080dbc12 290 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 291 y=radio*sin(i);
Vanessa620 10:3eab080dbc12 292 vertex2d(x+cx,y+cy);
Vanessa620 11:b4a3db5a0305 293 #if DEBUG
Vanessa620 13:94013a19713a 294 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio, ((PI/2)/RSTEP));
Vanessa620 12:92abcf61971b 295 #endif
Vanessa620 13:94013a19713a 296 }
Vanessa620 13:94013a19713a 297 #if DEBUG
Vanessa620 13:94013a19713a 298 command.printf("\n");
Vanessa620 13:94013a19713a 299 #endif
Vanessa620 13:94013a19713a 300 for(double i=PI; i<=((3*PI)/2) ;i+=((PI/2)/RSTEP))
Vanessa620 12:92abcf61971b 301 {
Vanessa620 11:b4a3db5a0305 302 x=radio*cos(i);
Vanessa620 11:b4a3db5a0305 303 y=radio*sin(i);
Vanessa620 13:94013a19713a 304 initdraw(x+cx,y+cy);
Vanessa620 11:b4a3db5a0305 305 #if DEBUG
Vanessa620 13:94013a19713a 306 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
Vanessa620 11:b4a3db5a0305 307 #endif
Vanessa620 11:b4a3db5a0305 308 }
Vanessa620 11:b4a3db5a0305 309 #if DEBUG
Vanessa620 13:94013a19713a 310 command.printf("\n");
Vanessa620 11:b4a3db5a0305 311 #endif
Vanessa620 13:94013a19713a 312 for(double i=((3*PI)/2); i<=(2*PI) ;i+=((PI/2)/RSTEP))
Vanessa620 12:92abcf61971b 313 {
Vanessa620 10:3eab080dbc12 314 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 315 y=radio*sin(i);
Vanessa620 13:94013a19713a 316 initdraw(x+cx,y+cy);
Vanessa620 11:b4a3db5a0305 317 #if DEBUG
Vanessa620 13:94013a19713a 318 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
Vanessa620 11:b4a3db5a0305 319 #endif
Vanessa620 13:94013a19713a 320 }
Vanessa620 13:94013a19713a 321 nodraw();
Vanessa620 10:3eab080dbc12 322 }
Vanessa620 13:94013a19713a 323
Vanessa620 13:94013a19713a 324 void home()
Vanessa620 13:94013a19713a 325 {
Vanessa620 13:94013a19713a 326 nodraw();
Vanessa620 13:94013a19713a 327 vertex2d(0,0);
Vanessa620 13:94013a19713a 328 #if DEBUG
Vanessa620 13:94013a19713a 329 command.printf("\nCoordenada HOME x=0, y =0");
Vanessa620 13:94013a19713a 330 #endif
Vanessa620 13:94013a19713a 331 }
Vanessa620 13:94013a19713a 332
Vanessa620 13:94013a19713a 333 void resolucion(int res) // Funcion para definir la resolucion del dibujo
Vanessa620 13:94013a19713a 334 {
Vanessa620 13:94013a19713a 335 RSTEP = res;
Vanessa620 13:94013a19713a 336 #if DEBUG
Vanessa620 13:94013a19713a 337 command.printf("\nResolucion definida en=%i \n", RSTEP);
Vanessa620 13:94013a19713a 338 #endif
Vanessa620 13:94013a19713a 339 }
Vanessa620 12:92abcf61971b 340
Vanessa620 13:94013a19713a 341 void TiempoPasos(int SST) //Funcion para definir el tiempo de led
Vanessa620 13:94013a19713a 342 {
Vanessa620 13:94013a19713a 343 SSTIME=SST;
Vanessa620 13:94013a19713a 344 #if DEBUG
Vanessa620 13:94013a19713a 345 command.printf("\nTiempo en pasos definida en:%i\n",SSTIME);
Vanessa620 13:94013a19713a 346 #endif
Vanessa620 13:94013a19713a 347 }
Vanessa620 14:f217d9874f8f 348 /*void sstime(uint8_t x, uint8_t y) //
Vanessa620 13:94013a19713a 349 {
Vanessa620 13:94013a19713a 350 double dx=abs(x-posx_old);
Vanessa620 13:94013a19713a 351 double dy=abs(y-posy_old);
Vanessa620 13:94013a19713a 352 double dist= sqrt(dx*dx+dy*dy);
Vanessa620 13:94013a19713a 353 wait_ms((int)(SSTIME*dist));
Vanessa620 13:94013a19713a 354 posx_old =x;
Vanessa620 13:94013a19713a 355 posy_old=y;
Vanessa620 14:f217d9874f8f 356
Vanessa620 14:f217d9874f8f 357 }*/
Vanessa620 13:94013a19713a 358 void Stop(int ST) //Funcion para detener el programa
Vanessa620 13:94013a19713a 359 {
Vanessa620 13:94013a19713a 360 if(ST == 0){
Vanessa620 13:94013a19713a 361 home();
Vanessa620 13:94013a19713a 362 #if DEBUG
Vanessa620 13:94013a19713a 363 command.printf("\n...Parada total...\n", ST);
Vanessa620 13:94013a19713a 364 #endif
Vanessa620 13:94013a19713a 365 exit(ST);
Vanessa620 13:94013a19713a 366 }
Vanessa620 13:94013a19713a 367 }
Vanessa620 13:94013a19713a 368
Vanessa620 13:94013a19713a 369 void Pausa(int det) //Funcion para definir el tiempo de led
Vanessa620 12:92abcf61971b 370 {
Vanessa620 13:94013a19713a 371
Vanessa620 13:94013a19713a 372 system("PAUSE()");
Vanessa620 13:94013a19713a 373
Vanessa620 13:94013a19713a 374 /* if(det == 0){
Vanessa620 13:94013a19713a 375 DET=det;
Vanessa620 12:92abcf61971b 376 #if DEBUG
Vanessa620 13:94013a19713a 377 command.printf("\n...Pausado...\n", DET);
Vanessa620 12:92abcf61971b 378 #endif
Vanessa620 13:94013a19713a 379 }*/
Vanessa620 12:92abcf61971b 380 }
Vanessa620 12:92abcf61971b 381
Vanessa620 13:94013a19713a 382 void Reanudar(int det) //Funcion para definir el tiempo de led
Vanessa620 13:94013a19713a 383 {
Vanessa620 13:94013a19713a 384 system("CLS()");
Vanessa620 13:94013a19713a 385 /* if(det == 1){
Vanessa620 13:94013a19713a 386 DET=det;
Vanessa620 13:94013a19713a 387 #if DEBUG
Vanessa620 13:94013a19713a 388 command.printf("\nReanudando imagen...%i/n", DET);
Vanessa620 13:94013a19713a 389 #endif
Vanessa620 13:94013a19713a 390 } */
Vanessa620 13:94013a19713a 391 }
Vanessa620 13:94013a19713a 392
Vanessa620 13:94013a19713a 393 /*void Mover(int tm) //Funcion para definir el tiempo de led
Vanessa620 12:92abcf61971b 394 {
Vanessa620 12:92abcf61971b 395 #if DEBUG
Vanessa620 13:94013a19713a 396 command.printf("\nMover a: %i/n", tm);
Vanessa620 12:92abcf61971b 397 #endif
Vanessa620 12:92abcf61971b 398 }
Vanessa620 13:94013a19713a 399 */
Vanessa620 13:94013a19713a 400
Vanessa620 13:94013a19713a 401 /*********************** CASOS DE USO PARA SELECCION DE COMANDOS***************/
Vanessa620 10:3eab080dbc12 402
fabeltranm 7:fab201aa45b7 403 void command_exe()
Vanessa620 12:92abcf61971b 404 {
Vanessa620 12:92abcf61971b 405 switch (buffer_command[COMM_N]){
Vanessa620 12:92abcf61971b 406
Vanessa620 12:92abcf61971b 407 case (LED_NC):
Vanessa620 12:92abcf61971b 408 #if DEBUG
Vanessa620 13:94013a19713a 409 command.printf(" LED ON/OFF\n");
Vanessa620 12:92abcf61971b 410 #endif
Vanessa620 12:92abcf61971b 411 Led(buffer_command[INITPARAMETER]);
Vanessa620 12:92abcf61971b 412 break;
Vanessa620 12:92abcf61971b 413
Vanessa620 12:92abcf61971b 414 case (DOT_NC):
Vanessa620 12:92abcf61971b 415 #if DEBUG
Vanessa620 13:94013a19713a 416 command.printf(" PUNTO\n");
Vanessa620 12:92abcf61971b 417 #endif
Vanessa620 12:92abcf61971b 418 punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]);
Vanessa620 12:92abcf61971b 419 break;
fabeltranm 7:fab201aa45b7 420
Vanessa620 12:92abcf61971b 421 case (LINE_NC):
Vanessa620 12:92abcf61971b 422 #if DEBUG
Vanessa620 13:94013a19713a 423 command.printf(" LINEA\n");
Vanessa620 12:92abcf61971b 424 #endif
Vanessa620 12:92abcf61971b 425 linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
Vanessa620 12:92abcf61971b 426 break;
fabeltranm 7:fab201aa45b7 427
Vanessa620 12:92abcf61971b 428 case (RECTANGULO_NC):
Vanessa620 12:92abcf61971b 429 #if DEBUG
Vanessa620 13:94013a19713a 430 command.printf(" RECTANGULO\n");
Vanessa620 12:92abcf61971b 431 #endif
Vanessa620 12:92abcf61971b 432 Rectangulo(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
Vanessa620 12:92abcf61971b 433 break;
Vanessa620 12:92abcf61971b 434
Vanessa620 13:94013a19713a 435 case (CIRCLE_NC):
Vanessa620 12:92abcf61971b 436 #if DEBUG
Vanessa620 13:94013a19713a 437 command.printf(" CIRCULO\n");
Vanessa620 12:92abcf61971b 438 #endif
Vanessa620 12:92abcf61971b 439 circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
Vanessa620 12:92abcf61971b 440 break;
fabeltranm 7:fab201aa45b7 441
Vanessa620 12:92abcf61971b 442 case (HOME_NC):
Vanessa620 12:92abcf61971b 443 #if DEBUG
Vanessa620 13:94013a19713a 444 command.printf(" HOME\n");
Vanessa620 12:92abcf61971b 445 #endif
Vanessa620 12:92abcf61971b 446 home();
Vanessa620 12:92abcf61971b 447 break;
Vanessa620 12:92abcf61971b 448
Vanessa620 12:92abcf61971b 449 case (RESOLUCION_NC):
Vanessa620 12:92abcf61971b 450 #if DEBUG
Vanessa620 13:94013a19713a 451 command.printf(" RESOLUCION\n");
Vanessa620 12:92abcf61971b 452 #endif
Vanessa620 12:92abcf61971b 453 resolucion(buffer_command[INITPARAMETER]);
Vanessa620 12:92abcf61971b 454 break;
fabeltranm 7:fab201aa45b7 455
Vanessa620 13:94013a19713a 456 case (TIEMPOPASOS_NC):
Vanessa620 12:92abcf61971b 457 #if DEBUG
Vanessa620 13:94013a19713a 458 command.printf(" TIEMPO EN PASOS\n");
Vanessa620 12:92abcf61971b 459 #endif
Vanessa620 12:92abcf61971b 460 TiempoPasos(buffer_command[INITPARAMETER]);
Vanessa620 12:92abcf61971b 461 break;
Vanessa620 13:94013a19713a 462
Vanessa620 12:92abcf61971b 463 case (STOP_NC):
Vanessa620 12:92abcf61971b 464 #if DEBUG
Vanessa620 13:94013a19713a 465 command.printf(" STOP\n");
Vanessa620 12:92abcf61971b 466 #endif
Vanessa620 12:92abcf61971b 467 Stop(buffer_command[INITPARAMETER]);
Vanessa620 13:94013a19713a 468
Vanessa620 12:92abcf61971b 469 break;
Vanessa620 12:92abcf61971b 470
Vanessa620 12:92abcf61971b 471 case (PAUSA_NC):
Vanessa620 12:92abcf61971b 472 #if DEBUG
Vanessa620 13:94013a19713a 473 command.printf(" PAUSA\n");
Vanessa620 12:92abcf61971b 474 #endif
Vanessa620 12:92abcf61971b 475 Pausa(buffer_command[INITPARAMETER]);
Vanessa620 12:92abcf61971b 476 break;
Vanessa620 12:92abcf61971b 477
Vanessa620 12:92abcf61971b 478 case (REANUDAR_NC):
Vanessa620 12:92abcf61971b 479 #if DEBUG
Vanessa620 13:94013a19713a 480 command.printf(" REANUDAR\n");
Vanessa620 12:92abcf61971b 481 #endif
Vanessa620 12:92abcf61971b 482 Reanudar(buffer_command[INITPARAMETER]);
Vanessa620 12:92abcf61971b 483 break;
Vanessa620 12:92abcf61971b 484
Vanessa620 13:94013a19713a 485 /* case (MOVER_NC):
Vanessa620 12:92abcf61971b 486 #if DEBUG
Vanessa620 13:94013a19713a 487 command.printf(" MOVER\n");
Vanessa620 12:92abcf61971b 488 #endif
Vanessa620 12:92abcf61971b 489 Mover(buffer_command[INITPARAMETER]);
Vanessa620 12:92abcf61971b 490 break;
Vanessa620 13:94013a19713a 491 */
Vanessa620 12:92abcf61971b 492 default:
Vanessa620 12:92abcf61971b 493 #if DEBUG
Vanessa620 12:92abcf61971b 494 command.printf("Comando no encontrado\n");
Vanessa620 12:92abcf61971b 495 #endif
Vanessa620 12:92abcf61971b 496 }
fabeltranm 7:fab201aa45b7 497 }
fabeltranm 7:fab201aa45b7 498
Vanessa620 12:92abcf61971b 499 int main()
Vanessa620 12:92abcf61971b 500 {
Vanessa620 12:92abcf61971b 501 myServoX.period_ms(20);
Vanessa620 12:92abcf61971b 502 myServoY.period_ms(20);
Vanessa620 12:92abcf61971b 503 myServoZ.period_ms(20);
Vanessa620 12:92abcf61971b 504
fabeltranm 0:55d11eeb0faf 505 #if DEBUG
fabeltranm 0:55d11eeb0faf 506 command.printf("inicio con debug\n");
fabeltranm 0:55d11eeb0faf 507 #else
fabeltranm 0:55d11eeb0faf 508 command.printf("inicio sin debug\n");
fabeltranm 0:55d11eeb0faf 509 #endif
fabeltranm 0:55d11eeb0faf 510 uint8_t val;
Vanessa620 13:94013a19713a 511 while(DET){
fabeltranm 0:55d11eeb0faf 512 val=command.getc();
Vanessa620 13:94013a19713a 513
fabeltranm 0:55d11eeb0faf 514 if (val== '<'){
Vanessa620 13:94013a19713a 515 Read_command();
fabeltranm 4:bcc2d1bebb95 516 if (check_command()){
fabeltranm 7:fab201aa45b7 517 command_exe();
fabeltranm 6:cb6b868465c3 518 #if DEBUG
Vanessa620 12:92abcf61971b 519 echo_command();
Vanessa620 13:94013a19713a 520 #endif
Vanessa620 12:92abcf61971b 521 }
Vanessa620 12:92abcf61971b 522 }
Vanessa620 12:92abcf61971b 523 }
fabeltranm 0:55d11eeb0faf 524 }
fabeltranm 0:55d11eeb0faf 525