Piccolo / Mbed 2 deprecated ejemplo_serial_protocol_array

Piccolo

Dependencies:   mbed

Fork of 02_LAB_serial_protocol by ferney alberto beltran molina

main.cpp@17:53c6058d2021, 2017-10-24 (annotated)

Committer:
Vanessa620
Date:
Tue Oct 24 00:06:48 2017 +0000
Revision:
17:53c6058d2021
Parent:
16:aeffa708f0f7
Child:
18:acc0ff6b308d
kj; ;

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