Piccolo / Mbed 2 deprecated ejemplo_serial_protocol_array

Piccolo

Dependencies:   mbed

Fork of 02_LAB_serial_protocol by ferney alberto beltran molina

main.cpp@15:ddcdd0f50b2b, 2017-10-21 (annotated)

Committer:
Vanessa620
Date:
Sat Oct 21 02:28:57 2017 +0000
Revision:
15:ddcdd0f50b2b
Parent:
14:f217d9874f8f
Child:
16:aeffa708f0f7
9:28

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