Piccolo / Mbed 2 deprecated ejemplo_serial_protocol_array

Piccolo

Dependencies:   mbed

Fork of 02_LAB_serial_protocol by ferney alberto beltran molina

main.cpp@18:acc0ff6b308d, 2017-11-11 (annotated)

Committer:
manuelitoys
Date:
Sat Nov 11 14:45:29 2017 +0000
Revision:
18:acc0ff6b308d
Parent:
17:53c6058d2021 
Probar pausa con p

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
manuelitoys 18:acc0ff6b308d 13 InterruptIn button1(USER_BUTTON); // interrupcion boton stop
manuelitoys 18:acc0ff6b308d 14 volatile bool button1_pressed = false; // Used in the main loop
manuelitoys 18:acc0ff6b308d 15 volatile bool button1_enabled = true; // Used for debouncing
manuelitoys 18:acc0ff6b308d 16 Timeout button1_timeout;
Vanessa620 12:92abcf61971b 17
fabeltranm 0:55d11eeb0faf 18 //*****************************************************************************
Vanessa620 13:94013a19713a 19 // COMANDO MOVER MOTOR
Vanessa620 13:94013a19713a 20 //
fabeltranm 0:55d11eeb0faf 21 // |POS 1|POS 2|POS 3|POS 4| POS 5|
fabeltranm 7:fab201aa45b7 22 // | < | #C | a | b | > |
Vanessa620 11:b4a3db5a0305 23 // | 3C |0 al A| D1 | D2 | D3 | D4 | 3E |
Vanessa620 13:94013a19713a 24 // #C -> indica el comando. En este caso es de 0 a 10 (en hexadesimal)
fabeltranm 6:cb6b868465c3 25 // a,b,c,d parametros del comando
fabeltranm 6:cb6b868465c3 26 // <,> -> inicio, y fin de comando
fabeltranm 0:55d11eeb0faf 27 // el inicio de comando no se almacena en el buffer
fabeltranm 0:55d11eeb0faf 28 //*****************************************************************************
fabeltranm 0:55d11eeb0faf 29
Vanessa620 13:94013a19713a 30 //***************************VARIABLES DEFINIDAS*******************************/
Vanessa620 13:94013a19713a 31
Vanessa620 12:92abcf61971b 32 #define BUFF_SIZE 6 // tamaño del comando enviado desde CoolTerm
Vanessa620 12:92abcf61971b 33 #define COMM_N 0 //
Vanessa620 12:92abcf61971b 34 #define INITPARAMETER 1 //
Vanessa620 12:92abcf61971b 35 #define MAXPOS 50 // Posicion maxima de la matriz de dibujo x,y en mm
Vanessa620 14:f217d9874f8f 36 #define POSDRAW 30 // Posicion del servomotor Z para dibujar
Vanessa620 13:94013a19713a 37 //#define SS_TIME 100 // no puedo degarla declarada cte. pues se debe mod por teclado
Vanessa620 12:92abcf61971b 38 #define PI 3.1415926 //
Vanessa620 13:94013a19713a 39 uint8_t RSTEP = 5; // Ini. variable de Resolucion para el dibujo
Vanessa620 13:94013a19713a 40 uint8_t DET=1;
Vanessa620 13:94013a19713a 41 uint8_t posx_old=0; // posición anterior del eje X
Vanessa620 13:94013a19713a 42 uint8_t posy_old=0; // posición anterior del eje Y
Vanessa620 13:94013a19713a 43 uint8_t SSTIME = 100;
Vanessa620 11:b4a3db5a0305 44
Vanessa620 13:94013a19713a 45 /****************************COMANDOS DE TECLADO*******************************/
Vanessa620 13:94013a19713a 46
Vanessa620 11:b4a3db5a0305 47 #define LED_NC 0 //ok
Vanessa620 11:b4a3db5a0305 48 #define DOT_NC 1 //ok
Vanessa620 11:b4a3db5a0305 49 #define LINE_NC 2 //ok
Vanessa620 12:92abcf61971b 50 #define RECTANGULO_NC 3 //ok
Vanessa620 13:94013a19713a 51 #define CIRCLE_NC 4 //ok
Vanessa620 11:b4a3db5a0305 52 #define HOME_NC 5 //ok
Vanessa620 13:94013a19713a 53 #define RESOLUCION_NC 6 //ok
Vanessa620 13:94013a19713a 54 #define TIEMPOPASOS_NC 7 // No se a que se refiere.
Vanessa620 12:92abcf61971b 55 #define STOP_NC 8 // Pendiente
Vanessa620 12:92abcf61971b 56 #define PAUSA_NC 9 // Pendiente
Vanessa620 13:94013a19713a 57 #define REANUDAR_NC 10 // Pendiente
Vanessa620 13:94013a19713a 58 //#define MOVER_NC 11 //Se refiere a el movimiento del motor paso a paso
Vanessa620 11:b4a3db5a0305 59
fabeltranm 7:fab201aa45b7 60
Vanessa620 13:94013a19713a 61 /**************FUNCIONES PARA MOVER LOS SERVOS X, Y, Z*************************/
Vanessa620 13:94013a19713a 62
Vanessa620 12:92abcf61971b 63 int coord2pulse(float coord) //
Vanessa620 12:92abcf61971b 64 {
manuelitoys 8:38ae341e2a4f 65 if(0 <= coord <= MAXPOS)
manuelitoys 8:38ae341e2a4f 66 return int(750+coord*1900/50);// u6
Vanessa620 14:f217d9874f8f 67 return 750;
Vanessa620 12:92abcf61971b 68 }
Vanessa620 12:92abcf61971b 69
Vanessa620 12:92abcf61971b 70 void vertex2d(float x, float y) // Funcion para enviarle la posicion a (x,y)
Vanessa620 12:92abcf61971b 71 {
Vanessa620 14:f217d9874f8f 72 wait_ms(SSTIME);
manuelitoys 9:5008f9501dbf 73 int pulseY = coord2pulse(y);
Vanessa620 14:f217d9874f8f 74 int pulseX = coord2pulse(x);
manuelitoys 9:5008f9501dbf 75
Vanessa620 14:f217d9874f8f 76 myServoY.pulsewidth_us(pulseY);
manuelitoys 9:5008f9501dbf 77 myServoX.pulsewidth_us(pulseX);
Vanessa620 14:f217d9874f8f 78
Vanessa620 12:92abcf61971b 79 }
Vanessa620 12:92abcf61971b 80
Vanessa620 12:92abcf61971b 81 void draw() // Funcion para enviarle la posicion de dibujo a z.
Vanessa620 12:92abcf61971b 82 {
Vanessa620 14:f217d9874f8f 83 wait_ms(SSTIME*10);
Vanessa620 14:f217d9874f8f 84 int pulseZ=coord2pulse(POSDRAW);
Vanessa620 14:f217d9874f8f 85 myServoZ.pulsewidth_us(pulseZ);
Vanessa620 14:f217d9874f8f 86 wait_ms(SSTIME);
Vanessa620 12:92abcf61971b 87 }
Vanessa620 12:92abcf61971b 88 void nodraw() // Funcion para enviarle la posicion de no dibujar a z.
Vanessa620 14:f217d9874f8f 89 {
Vanessa620 14:f217d9874f8f 90 wait_ms(SSTIME*10);
Vanessa620 14:f217d9874f8f 91 int pulseZ=coord2pulse(0);
Vanessa620 14:f217d9874f8f 92 myServoZ.pulsewidth_us(pulseZ);
Vanessa620 14:f217d9874f8f 93
Vanessa620 12:92abcf61971b 94 }
Vanessa620 13:94013a19713a 95
Vanessa620 13:94013a19713a 96 void initdraw(float x, float y)//ok
Vanessa620 13:94013a19713a 97 {
Vanessa620 14:f217d9874f8f 98 vertex2d(x,y);
Vanessa620 14:f217d9874f8f 99 wait_ms(SSTIME);
Vanessa620 13:94013a19713a 100 draw();
Vanessa620 13:94013a19713a 101 }
fabeltranm 7:fab201aa45b7 102
Vanessa620 13:94013a19713a 103 /*****************FUNCIONES PARA OBTENER E IMPRIMIR EL COMANDO*****************/
Vanessa620 13:94013a19713a 104
Vanessa620 13:94013a19713a 105 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0}; // Matriz del Comando enviado
fabeltranm 1:0bcd96e56022 106
Vanessa620 12:92abcf61971b 107 void print_num(uint8_t val) //
Vanessa620 12:92abcf61971b 108 {
Vanessa620 12:92abcf61971b 109 if (val <10)
fabeltranm 1:0bcd96e56022 110 command.putc(val+0x30);
fabeltranm 1:0bcd96e56022 111 else
Vanessa620 12:92abcf61971b 112 command.putc(val-9+0x40);
Vanessa620 12:92abcf61971b 113 }
Vanessa620 12:92abcf61971b 114
Vanessa620 13:94013a19713a 115 void print_bin2hex (uint8_t val) // Imprimir el comando enviado en Hexadecimal
Vanessa620 12:92abcf61971b 116 {
fabeltranm 1:0bcd96e56022 117 command.printf(" 0x");
fabeltranm 1:0bcd96e56022 118 print_num(val>>4);
Vanessa620 12:92abcf61971b 119 print_num(val&0x0f);
Vanessa620 12:92abcf61971b 120 }
fabeltranm 1:0bcd96e56022 121
Vanessa620 13:94013a19713a 122 void Read_command() // Leer el comando que se digito en CoolTerm
Vanessa620 12:92abcf61971b 123 {
fabeltranm 0:55d11eeb0faf 124 for (uint8_t i=0; i<BUFF_SIZE;i++)
fabeltranm 0:55d11eeb0faf 125 buffer_command[i]=command.getc();
Vanessa620 12:92abcf61971b 126 }
fabeltranm 0:55d11eeb0faf 127
Vanessa620 12:92abcf61971b 128 void echo_command() //
Vanessa620 12:92abcf61971b 129 {
fabeltranm 0:55d11eeb0faf 130 for (uint8_t i=0; i<BUFF_SIZE;i++)
Vanessa620 12:92abcf61971b 131 print_bin2hex(buffer_command[i]);
Vanessa620 12:92abcf61971b 132 }
fabeltranm 0:55d11eeb0faf 133
Vanessa620 13:94013a19713a 134 uint8_t check_command() // Verifica el ultimo valor del comando enviado '>'
Vanessa620 12:92abcf61971b 135 {
Vanessa620 12:92abcf61971b 136 if (buffer_command[BUFF_SIZE-1]== '>'){
fabeltranm 0:55d11eeb0faf 137 #if DEBUG
Vanessa620 12:92abcf61971b 138 command.printf("\nComando: ");
Vanessa620 12:92abcf61971b 139 print_bin2hex(buffer_command[COMM_N]);
Vanessa620 12:92abcf61971b 140 command.printf(" -> ");
Vanessa620 12:92abcf61971b 141 #endif
Vanessa620 12:92abcf61971b 142 return 1;
Vanessa620 12:92abcf61971b 143 }
Vanessa620 12:92abcf61971b 144 #if DEBUG
Vanessa620 13:94013a19713a 145 command.printf("\n !!!!!!!!ERROR EN EL COMANDO!!!!!!!!!!!!!! -> ");
Vanessa620 12:92abcf61971b 146 echo_command();
fabeltranm 0:55d11eeb0faf 147 #endif
Vanessa620 12:92abcf61971b 148 return 0;
Vanessa620 12:92abcf61971b 149 }
Vanessa620 12:92abcf61971b 150
Vanessa620 13:94013a19713a 151 /********************FUNCIONES PARA DIBUJAR EN EL PICOLO************************/
Vanessa620 13:94013a19713a 152
Vanessa620 13:94013a19713a 153 void Led(int tm) //Funcion para definir el tiempo de led en milisegundos
Vanessa620 12:92abcf61971b 154 {
Vanessa620 11:b4a3db5a0305 155 #if DEBUG
Vanessa620 13:94013a19713a 156 command.printf("\nTiempo led: %i seg\n", tm);
Vanessa620 11:b4a3db5a0305 157 #endif
Vanessa620 12:92abcf61971b 158 led=1;
Vanessa620 12:92abcf61971b 159 wait(tm);
Vanessa620 12:92abcf61971b 160 led=0;
Vanessa620 11:b4a3db5a0305 161 }
Vanessa620 12:92abcf61971b 162
Vanessa620 12:92abcf61971b 163 void punto(uint8_t x, uint8_t y) //Funcion para dibujar un punto
Vanessa620 12:92abcf61971b 164 {
Vanessa620 12:92abcf61971b 165 #if DEBUG
Vanessa620 13:94013a19713a 166 command.printf("\nCoordenadas x=%i, y=%i\n",x,y);
Vanessa620 12:92abcf61971b 167 #endif
Vanessa620 13:94013a19713a 168 initdraw(x,y);
Vanessa620 11:b4a3db5a0305 169 nodraw();
Vanessa620 13:94013a19713a 170 }
Vanessa620 11:b4a3db5a0305 171
Vanessa620 13:94013a19713a 172 void linea(float xi, float yi, float xf, float yf)
Vanessa620 13:94013a19713a 173 {
Vanessa620 12:92abcf61971b 174 #if DEBUG
Vanessa620 13:94013a19713a 175 command.printf("\nCoordenadas xi=%f, yi=%f, xf=%f, yf=%f, resolucion: %i \n", xi,yi,xf,yf,RSTEP);
Vanessa620 13:94013a19713a 176 #endif
Vanessa620 13:94013a19713a 177 float xp,yp;
Vanessa620 13:94013a19713a 178 float m=(yf-yi)/(xf-xi);
Vanessa620 13:94013a19713a 179 float b=yf-(m*xf);
Vanessa620 13:94013a19713a 180 #if DEBUG
Vanessa620 13:94013a19713a 181 command.printf("\n b =%f, m=%f \n", b,m);
Vanessa620 13:94013a19713a 182 #endif
Vanessa620 13:94013a19713a 183 float nstep =(m/RSTEP);
Vanessa620 13:94013a19713a 184 //nstep=RSTEP;
Vanessa620 13:94013a19713a 185 #if DEBUG
Vanessa620 13:94013a19713a 186 command.printf("\nstep = %f \n", nstep);
Vanessa620 12:92abcf61971b 187 #endif
Vanessa620 13:94013a19713a 188 if ((abs(xf-xi))>abs(yf-yi)){
Vanessa620 13:94013a19713a 189 if (xf>xi){
Vanessa620 14:f217d9874f8f 190 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 191 for (xp=xi; xp<=xf; xp+=RSTEP){
Vanessa620 13:94013a19713a 192 yp =m*xp+b;
Vanessa620 13:94013a19713a 193 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 194 #if DEBUG
Vanessa620 13:94013a19713a 195 command.printf(" CASO 1: ( dx>dy & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 196 #endif
Vanessa620 13:94013a19713a 197 } }
Vanessa620 13:94013a19713a 198 else{
Vanessa620 13:94013a19713a 199 float temp = xi;
Vanessa620 13:94013a19713a 200 xi = xf;
Vanessa620 13:94013a19713a 201 xf = temp;
Vanessa620 14:f217d9874f8f 202 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 203 for (xp=xi; xp<=xf; xp+=RSTEP){
Vanessa620 13:94013a19713a 204 yp =m*xp+b;
Vanessa620 13:94013a19713a 205 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 206 #if DEBUG
Vanessa620 13:94013a19713a 207 command.printf(" CASO 2: ( dx>dy & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 208 #endif
Vanessa620 13:94013a19713a 209 }}}
Vanessa620 13:94013a19713a 210 else {
Vanessa620 13:94013a19713a 211 if (yf>yi){
Vanessa620 14:f217d9874f8f 212 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 213 for (yp=yi; yp<=yf; yp+=RSTEP){
Vanessa620 13:94013a19713a 214 xp=(yp-b)/m;
Vanessa620 13:94013a19713a 215 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 216 #if DEBUG
Vanessa620 13:94013a19713a 217 command.printf(" CASO 3: ( dy>dx & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 218 #endif
Vanessa620 13:94013a19713a 219 }}
Vanessa620 13:94013a19713a 220 else{
Vanessa620 13:94013a19713a 221 float tempo = yi;
Vanessa620 13:94013a19713a 222 yi = yf;
Vanessa620 13:94013a19713a 223 yf = tempo;
Vanessa620 14:f217d9874f8f 224 initdraw(xp,yp);
Vanessa620 14:f217d9874f8f 225 for (yp=yi; yp<=yf; yp+=RSTEP){
Vanessa620 13:94013a19713a 226 xp=(yp-b)/m;
Vanessa620 13:94013a19713a 227 vertex2d(xp,yp);
Vanessa620 13:94013a19713a 228 #if DEBUG
Vanessa620 13:94013a19713a 229 command.printf(" CASO 4: ( dy>dx & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 13:94013a19713a 230 #endif
Vanessa620 13:94013a19713a 231 }
Vanessa620 13:94013a19713a 232 }
Vanessa620 11:b4a3db5a0305 233 }
Vanessa620 13:94013a19713a 234 nodraw();
Vanessa620 12:92abcf61971b 235 }
Vanessa620 12:92abcf61971b 236
Vanessa620 12:92abcf61971b 237 void Rectangulo(uint8_t x, uint8_t y, uint8_t a, uint8_t h)
Vanessa620 12:92abcf61971b 238 {
Vanessa620 12:92abcf61971b 239 #if DEBUG
Vanessa620 13:94013a19713a 240 command.printf("\nCoordenadas x=%i, y=%i, ancho=%i, alto=%i, resolucion=%i\n", x,y,a,h,RSTEP);
Vanessa620 15:ddcdd0f50b2b 241 #endif
Vanessa620 15:ddcdd0f50b2b 242 uint8_t A=x+a;
Vanessa620 15:ddcdd0f50b2b 243 uint8_t B=y+h;
Vanessa620 13:94013a19713a 244 initdraw(x,y);
Vanessa620 13:94013a19713a 245
Vanessa620 14:f217d9874f8f 246 for(uint8_t xi=x; xi<=(x+a); xi+=RSTEP){
Vanessa620 13:94013a19713a 247 vertex2d(xi,y);
Vanessa620 12:92abcf61971b 248 #if DEBUG
Vanessa620 15:ddcdd0f50b2b 249 command.printf("Coordenadas x=%i,y=%i for 1\n", xi,y);
Vanessa620 13:94013a19713a 250 #endif
Vanessa620 13:94013a19713a 251 }
Vanessa620 13:94013a19713a 252 for (uint8_t yi=y; yi<=(y+h); yi+=RSTEP){
Vanessa620 13:94013a19713a 253 vertex2d(x+a,yi);
Vanessa620 12:92abcf61971b 254 #if DEBUG
Vanessa620 15:ddcdd0f50b2b 255 command.printf("Coordenadas x=%i,y=%i for 2\n", x+a,yi);
Vanessa620 13:94013a19713a 256 #endif
Vanessa620 12:92abcf61971b 257 }
Vanessa620 15:ddcdd0f50b2b 258 for(uint8_t xf=A; xf>x; xf= xf - RSTEP){
Vanessa620 15:ddcdd0f50b2b 259 vertex2d(xf,B);
Vanessa620 12:92abcf61971b 260 #if DEBUG
Vanessa620 15:ddcdd0f50b2b 261 command.printf("Coordenadas x=%i,y=%i for 3\n", xf,B);
Vanessa620 13:94013a19713a 262 #endif
Vanessa620 13:94013a19713a 263 }
Vanessa620 15:ddcdd0f50b2b 264 for (uint8_t yf=(y+h); yf>y; yf-=RSTEP){
Vanessa620 14:f217d9874f8f 265 vertex2d(x,yf);
Vanessa620 13:94013a19713a 266 #if DEBUG
Vanessa620 15:ddcdd0f50b2b 267 command.printf("Coordenadas x=%i,y=%i for 4\n", x,yf);
Vanessa620 13:94013a19713a 268 #endif
Vanessa620 13:94013a19713a 269 }
Vanessa620 15:ddcdd0f50b2b 270 vertex2d(x,y);
Vanessa620 15:ddcdd0f50b2b 271 #if DEBUG
Vanessa620 15:ddcdd0f50b2b 272 command.printf("Coordenadas x=%i,y=%i for 4\n", x,y);
Vanessa620 15:ddcdd0f50b2b 273 #endif
Vanessa620 13:94013a19713a 274 nodraw();
Vanessa620 10:3eab080dbc12 275 }
Vanessa620 11:b4a3db5a0305 276
Vanessa620 10:3eab080dbc12 277 void circle(uint8_t cx, uint8_t cy, uint8_t radio)
Vanessa620 10:3eab080dbc12 278 {
Vanessa620 10:3eab080dbc12 279 int y;
Vanessa620 10:3eab080dbc12 280 int x;
Vanessa620 13:94013a19713a 281 vertex2d(cx,cy);
Vanessa620 13:94013a19713a 282 #if DEBUG
Vanessa620 13:94013a19713a 283 command.printf("\nCoordenadas xc =%i, yc =%i, Radio=%i \n",cx,cy,radio);
Vanessa620 13:94013a19713a 284 #endif
Vanessa620 11:b4a3db5a0305 285
Vanessa620 13:94013a19713a 286 for(double i=0; i<=PI/2 ;i+=((PI/2)/RSTEP))
Vanessa620 13:94013a19713a 287 {
Vanessa620 13:94013a19713a 288 x=radio*cos(i);
Vanessa620 13:94013a19713a 289 y=radio*sin(i);
Vanessa620 13:94013a19713a 290 initdraw(x+cx,y+cy);
Vanessa620 13:94013a19713a 291 #if DEBUG
Vanessa620 13:94013a19713a 292 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
Vanessa620 13:94013a19713a 293 #endif
Vanessa620 13:94013a19713a 294 }
Vanessa620 13:94013a19713a 295 #if DEBUG
Vanessa620 13:94013a19713a 296 command.printf("\n");
Vanessa620 13:94013a19713a 297 #endif
Vanessa620 13:94013a19713a 298 for(double i=PI/2; i<=PI ;i+=((PI/2)/RSTEP))
Vanessa620 12:92abcf61971b 299 {
Vanessa620 10:3eab080dbc12 300 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 301 y=radio*sin(i);
Vanessa620 10:3eab080dbc12 302 vertex2d(x+cx,y+cy);
Vanessa620 11:b4a3db5a0305 303 #if DEBUG
Vanessa620 13:94013a19713a 304 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio, ((PI/2)/RSTEP));
Vanessa620 12:92abcf61971b 305 #endif
Vanessa620 13:94013a19713a 306 }
Vanessa620 13:94013a19713a 307 #if DEBUG
Vanessa620 13:94013a19713a 308 command.printf("\n");
Vanessa620 13:94013a19713a 309 #endif
Vanessa620 13:94013a19713a 310 for(double i=PI; i<=((3*PI)/2) ;i+=((PI/2)/RSTEP))
Vanessa620 12:92abcf61971b 311 {
Vanessa620 11:b4a3db5a0305 312 x=radio*cos(i);
Vanessa620 11:b4a3db5a0305 313 y=radio*sin(i);
Vanessa620 13:94013a19713a 314 initdraw(x+cx,y+cy);
Vanessa620 11:b4a3db5a0305 315 #if DEBUG
Vanessa620 13:94013a19713a 316 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
Vanessa620 11:b4a3db5a0305 317 #endif
Vanessa620 11:b4a3db5a0305 318 }
Vanessa620 11:b4a3db5a0305 319 #if DEBUG
Vanessa620 13:94013a19713a 320 command.printf("\n");
Vanessa620 11:b4a3db5a0305 321 #endif
Vanessa620 13:94013a19713a 322 for(double i=((3*PI)/2); i<=(2*PI) ;i+=((PI/2)/RSTEP))
Vanessa620 12:92abcf61971b 323 {
Vanessa620 10:3eab080dbc12 324 x=radio*cos(i);
Vanessa620 10:3eab080dbc12 325 y=radio*sin(i);
Vanessa620 13:94013a19713a 326 initdraw(x+cx,y+cy);
Vanessa620 11:b4a3db5a0305 327 #if DEBUG
Vanessa620 13:94013a19713a 328 command.printf("Coordenadas x =%li, y =%li, R=%i, Resolcion:%i \n",x+cx,y+cy,radio,((PI/2)/RSTEP));
Vanessa620 11:b4a3db5a0305 329 #endif
Vanessa620 13:94013a19713a 330 }
Vanessa620 13:94013a19713a 331 nodraw();
Vanessa620 10:3eab080dbc12 332 }
Vanessa620 13:94013a19713a 333
Vanessa620 13:94013a19713a 334 void home()
Vanessa620 13:94013a19713a 335 {
Vanessa620 13:94013a19713a 336 nodraw();
Vanessa620 13:94013a19713a 337 vertex2d(0,0);
Vanessa620 13:94013a19713a 338 #if DEBUG
Vanessa620 13:94013a19713a 339 command.printf("\nCoordenada HOME x=0, y =0");
Vanessa620 13:94013a19713a 340 #endif
Vanessa620 13:94013a19713a 341 }
Vanessa620 13:94013a19713a 342
Vanessa620 13:94013a19713a 343 void resolucion(int res) // Funcion para definir la resolucion del dibujo
Vanessa620 13:94013a19713a 344 {
Vanessa620 13:94013a19713a 345 RSTEP = res;
Vanessa620 13:94013a19713a 346 #if DEBUG
Vanessa620 13:94013a19713a 347 command.printf("\nResolucion definida en=%i \n", RSTEP);
Vanessa620 13:94013a19713a 348 #endif
Vanessa620 13:94013a19713a 349 }
Vanessa620 12:92abcf61971b 350
Vanessa620 13:94013a19713a 351 void TiempoPasos(int SST) //Funcion para definir el tiempo de led
Vanessa620 13:94013a19713a 352 {
Vanessa620 13:94013a19713a 353 SSTIME=SST;
Vanessa620 13:94013a19713a 354 #if DEBUG
Vanessa620 13:94013a19713a 355 command.printf("\nTiempo en pasos definida en:%i\n",SSTIME);
Vanessa620 13:94013a19713a 356 #endif
Vanessa620 13:94013a19713a 357 }
Vanessa620 15:ddcdd0f50b2b 358 void sstime(uint8_t x, uint8_t y) //
Vanessa620 13:94013a19713a 359 {
Vanessa620 13:94013a19713a 360 double dx=abs(x-posx_old);
Vanessa620 13:94013a19713a 361 double dy=abs(y-posy_old);
Vanessa620 13:94013a19713a 362 double dist= sqrt(dx*dx+dy*dy);
Vanessa620 13:94013a19713a 363 wait_ms((int)(SSTIME*dist));
Vanessa620 13:94013a19713a 364 posx_old =x;
Vanessa620 13:94013a19713a 365 posy_old=y;
Vanessa620 14:f217d9874f8f 366
Vanessa620 15:ddcdd0f50b2b 367 }
Vanessa620 15:ddcdd0f50b2b 368
Vanessa620 15:ddcdd0f50b2b 369 void Stop() //Funcion para detener el programa
Vanessa620 13:94013a19713a 370 {
Vanessa620 17:53c6058d2021 371 exit(0);
manuelitoys 18:acc0ff6b308d 372
Vanessa620 13:94013a19713a 373 }
Vanessa620 13:94013a19713a 374
manuelitoys 18:acc0ff6b308d 375 void Pausa(void) //Funcion para definir el tiempo de led
Vanessa620 17:53c6058d2021 376 {
manuelitoys 18:acc0ff6b308d 377 button1_enabled = true;
Vanessa620 12:92abcf61971b 378 #if DEBUG
Vanessa620 17:53c6058d2021 379 command.printf("\n...Pausado...\n");
Vanessa620 12:92abcf61971b 380 #endif
Vanessa620 17:53c6058d2021 381 }
manuelitoys 18:acc0ff6b308d 382
manuelitoys 18:acc0ff6b308d 383 void Reanudar(void) //Funcion para definir el tiempo de led
Vanessa620 13:94013a19713a 384 {
manuelitoys 18:acc0ff6b308d 385 if (button1_enabled) { // Disabled while the button is bouncing
manuelitoys 18:acc0ff6b308d 386 button1_enabled = false;
manuelitoys 18:acc0ff6b308d 387 button1_pressed = true; // To be read by the main loop
manuelitoys 18:acc0ff6b308d 388 button1_timeout.attach(callback(Pausa), 0.3); // Debounce time 300 ms
manuelitoys 18:acc0ff6b308d 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
manuelitoys 18:acc0ff6b308d 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
manuelitoys 18:acc0ff6b308d 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;
manuelitoys 18:acc0ff6b308d 514 char estado = '0';
manuelitoys 18:acc0ff6b308d 515 button1.fall(&Stop);
manuelitoys 18:acc0ff6b308d 516 button1.fall(callback(Reanudar));
manuelitoys 18:acc0ff6b308d 517 int idx = 0;
Vanessa620 17:53c6058d2021 518 while(1){
manuelitoys 18:acc0ff6b308d 519
manuelitoys 18:acc0ff6b308d 520
fabeltranm 0:55d11eeb0faf 521 val=command.getc();
manuelitoys 18:acc0ff6b308d 522 estado = command.getc();
manuelitoys 18:acc0ff6b308d 523 switch(estado){
manuelitoys 18:acc0ff6b308d 524 case '0':
manuelitoys 18:acc0ff6b308d 525 if (val== '<'){
Vanessa620 13:94013a19713a 526 Read_command();
fabeltranm 4:bcc2d1bebb95 527 if (check_command()){
fabeltranm 7:fab201aa45b7 528 command_exe();
fabeltranm 6:cb6b868465c3 529 #if DEBUG
Vanessa620 12:92abcf61971b 530 echo_command();
Vanessa620 13:94013a19713a 531 #endif
manuelitoys 18:acc0ff6b308d 532
Vanessa620 12:92abcf61971b 533 }
manuelitoys 18:acc0ff6b308d 534 }
manuelitoys 18:acc0ff6b308d 535 break;
manuelitoys 18:acc0ff6b308d 536
manuelitoys 18:acc0ff6b308d 537 case 'p':
manuelitoys 18:acc0ff6b308d 538 button1_pressed = false;
manuelitoys 18:acc0ff6b308d 539 printf("Button pressed %d\n", idx++);
manuelitoys 18:acc0ff6b308d 540 break;
manuelitoys 18:acc0ff6b308d 541 }
manuelitoys 18:acc0ff6b308d 542
Vanessa620 12:92abcf61971b 543 }
fabeltranm 0:55d11eeb0faf 544 }
fabeltranm 0:55d11eeb0faf 545