Piccolo / Mbed 2 deprecated ejemplo_serial_protocol_array

Piccolo

Dependencies:   mbed

Fork of 02_LAB_serial_protocol by ferney alberto beltran molina

main.cpp@13:94013a19713a, 2017-10-14 (annotated)

Committer:
Vanessa620
Date:
Sat Oct 14 13:02:04 2017 +0000
Revision:
13:94013a19713a
Parent:
12:92abcf61971b
Child:
14:f217d9874f8f
Pending SSTIME, STOP, PAUSA, REANUDAR funtions.

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