Esta última actualización contiene el código funcional en las 10 funciones.
Dependencies: mbed
Fork of jc by
main.cpp@0:9aa960516df7, 2017-11-20 (annotated)
- Committer:
- VaLaraC
- Date:
- Mon Nov 20 04:28:42 2017 +0000
- Revision:
- 0:9aa960516df7
- Child:
- 1:a9b3fdab473e
Drawing Functions
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
VaLaraC | 0:9aa960516df7 | 1 | #include "mbed.h" |
VaLaraC | 0:9aa960516df7 | 2 | #include "math.h" |
VaLaraC | 0:9aa960516df7 | 3 | #include "cstdlib" |
VaLaraC | 0:9aa960516df7 | 4 | #define MAXPOS 255 |
VaLaraC | 0:9aa960516df7 | 5 | #define SS_TIME 500 |
VaLaraC | 0:9aa960516df7 | 6 | #define POSDRAW 300 |
VaLaraC | 0:9aa960516df7 | 7 | #define BUFF_SIZE 6 |
VaLaraC | 0:9aa960516df7 | 8 | #define COMM_N 0 |
VaLaraC | 0:9aa960516df7 | 9 | #define INITPARAMETER 1 |
VaLaraC | 0:9aa960516df7 | 10 | #define DEBUG 1 |
VaLaraC | 0:9aa960516df7 | 11 | #define BUFF_POS 6 |
VaLaraC | 0:9aa960516df7 | 12 | |
VaLaraC | 0:9aa960516df7 | 13 | PwmOut myServoX(PB_3); //MOTOR EJE X |
VaLaraC | 0:9aa960516df7 | 14 | PwmOut myServoY(PB_4); //MOTOR EJE Y |
VaLaraC | 0:9aa960516df7 | 15 | PwmOut myServoZ(PB_10);//MOTOR EJE Z |
VaLaraC | 0:9aa960516df7 | 16 | |
VaLaraC | 0:9aa960516df7 | 17 | Serial command(USBTX, USBRX); //COMUNICACION SERIAL |
VaLaraC | 0:9aa960516df7 | 18 | Serial Sdebug(USBTX, USBRX); |
VaLaraC | 0:9aa960516df7 | 19 | DigitalOut led(LED1); //DECLARACION LED DE LA PLACA |
VaLaraC | 0:9aa960516df7 | 20 | InterruptIn mybutton(USER_BUTTON); |
VaLaraC | 0:9aa960516df7 | 21 | |
VaLaraC | 0:9aa960516df7 | 22 | // COMANDOS |
VaLaraC | 0:9aa960516df7 | 23 | #define LED 0 |
VaLaraC | 0:9aa960516df7 | 24 | #define PUNTO 1 |
VaLaraC | 0:9aa960516df7 | 25 | #define LINE 2 |
VaLaraC | 0:9aa960516df7 | 26 | #define RECTANGLE 3 |
VaLaraC | 0:9aa960516df7 | 27 | #define CIRCLE 4 |
VaLaraC | 0:9aa960516df7 | 28 | #define STOP 5 |
VaLaraC | 0:9aa960516df7 | 29 | #define PAUSE 6 |
VaLaraC | 0:9aa960516df7 | 30 | #define REANUDAR 7 |
VaLaraC | 0:9aa960516df7 | 31 | #define RESOLUCION 8 |
VaLaraC | 0:9aa960516df7 | 32 | #define HOME 9 |
VaLaraC | 0:9aa960516df7 | 33 | #define TIEMPO_DE_PASO 10 |
VaLaraC | 0:9aa960516df7 | 34 | |
VaLaraC | 0:9aa960516df7 | 35 | Ticker ticker1; |
VaLaraC | 0:9aa960516df7 | 36 | Timeout timeout; |
VaLaraC | 0:9aa960516df7 | 37 | |
VaLaraC | 0:9aa960516df7 | 38 | int rap; |
VaLaraC | 0:9aa960516df7 | 39 | int RSTEP = 5; |
VaLaraC | 0:9aa960516df7 | 40 | double abajo=7; |
VaLaraC | 0:9aa960516df7 | 41 | double arriba=0; |
VaLaraC | 0:9aa960516df7 | 42 | double k,l,r,tp=50,paso=0; |
VaLaraC | 0:9aa960516df7 | 43 | int p=0; |
VaLaraC | 0:9aa960516df7 | 44 | |
VaLaraC | 0:9aa960516df7 | 45 | //***************************************************************************** |
VaLaraC | 0:9aa960516df7 | 46 | // COMANDO MOVER MOTOR |
VaLaraC | 0:9aa960516df7 | 47 | // |POS 1|POS 2|POS 3|POS 4| POS 5| POS 6| POS 7| |
VaLaraC | 0:9aa960516df7 | 48 | // | < | #C | a | b | c | d | > | |
VaLaraC | 0:9aa960516df7 | 49 | // |
VaLaraC | 0:9aa960516df7 | 50 | // #C -> indica el comando |
VaLaraC | 0:9aa960516df7 | 51 | // a,b,c,d parametros del comando |
VaLaraC | 0:9aa960516df7 | 52 | // <,> -> inicio, y fin de comando |
VaLaraC | 0:9aa960516df7 | 53 | // el inicio de comando no se almacena en el buffer |
VaLaraC | 0:9aa960516df7 | 54 | //***************************************************************************** |
VaLaraC | 0:9aa960516df7 | 55 | // VARIABLES PARA DEFINIR EL COMMANDO |
VaLaraC | 0:9aa960516df7 | 56 | |
VaLaraC | 0:9aa960516df7 | 57 | uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0}; |
VaLaraC | 0:9aa960516df7 | 58 | float buffer_POS [BUFF_POS]= {0,0,0,0,0,0}; |
VaLaraC | 0:9aa960516df7 | 59 | |
VaLaraC | 0:9aa960516df7 | 60 | void savep(); |
VaLaraC | 0:9aa960516df7 | 61 | void interrupcion(); |
VaLaraC | 0:9aa960516df7 | 62 | |
VaLaraC | 0:9aa960516df7 | 63 | int coord2us(float coord) |
VaLaraC | 0:9aa960516df7 | 64 | { |
VaLaraC | 0:9aa960516df7 | 65 | if(0 <= coord <= MAXPOS) |
VaLaraC | 0:9aa960516df7 | 66 | return int(750+coord*1900/50);// u6 |
VaLaraC | 0:9aa960516df7 | 67 | return 750; |
VaLaraC | 0:9aa960516df7 | 68 | } |
VaLaraC | 0:9aa960516df7 | 69 | void print_num(uint8_t val) |
VaLaraC | 0:9aa960516df7 | 70 | { |
VaLaraC | 0:9aa960516df7 | 71 | if (val <10) |
VaLaraC | 0:9aa960516df7 | 72 | command.putc(val+0x30); |
VaLaraC | 0:9aa960516df7 | 73 | else |
VaLaraC | 0:9aa960516df7 | 74 | command.putc(val-9+0x40); |
VaLaraC | 0:9aa960516df7 | 75 | } |
VaLaraC | 0:9aa960516df7 | 76 | void print_bin2hex (uint8_t val) |
VaLaraC | 0:9aa960516df7 | 77 | { |
VaLaraC | 0:9aa960516df7 | 78 | command.printf(" 0x"); |
VaLaraC | 0:9aa960516df7 | 79 | print_num(val>>4); |
VaLaraC | 0:9aa960516df7 | 80 | print_num(val&0x0f); |
VaLaraC | 0:9aa960516df7 | 81 | } |
VaLaraC | 0:9aa960516df7 | 82 | void Read_command() |
VaLaraC | 0:9aa960516df7 | 83 | { |
VaLaraC | 0:9aa960516df7 | 84 | for (uint8_t i=0; i<BUFF_SIZE;i++) |
VaLaraC | 0:9aa960516df7 | 85 | { |
VaLaraC | 0:9aa960516df7 | 86 | buffer_command[i]=command.getc(); |
VaLaraC | 0:9aa960516df7 | 87 | |
VaLaraC | 0:9aa960516df7 | 88 | } |
VaLaraC | 0:9aa960516df7 | 89 | } |
VaLaraC | 0:9aa960516df7 | 90 | |
VaLaraC | 0:9aa960516df7 | 91 | void echo_command() |
VaLaraC | 0:9aa960516df7 | 92 | { |
VaLaraC | 0:9aa960516df7 | 93 | for (uint8_t i=0; i<BUFF_SIZE;i++) |
VaLaraC | 0:9aa960516df7 | 94 | { |
VaLaraC | 0:9aa960516df7 | 95 | print_bin2hex(buffer_command[i]); |
VaLaraC | 0:9aa960516df7 | 96 | } |
VaLaraC | 0:9aa960516df7 | 97 | } |
VaLaraC | 0:9aa960516df7 | 98 | uint8_t check_command() |
VaLaraC | 0:9aa960516df7 | 99 | { |
VaLaraC | 0:9aa960516df7 | 100 | if (buffer_command[BUFF_SIZE-1]== '>') |
VaLaraC | 0:9aa960516df7 | 101 | return 1; |
VaLaraC | 0:9aa960516df7 | 102 | return 0; |
VaLaraC | 0:9aa960516df7 | 103 | } |
VaLaraC | 0:9aa960516df7 | 104 | void command_led(uint8_t tm) |
VaLaraC | 0:9aa960516df7 | 105 | { |
VaLaraC | 0:9aa960516df7 | 106 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 107 | command.printf("%i, segundos", tm); |
VaLaraC | 0:9aa960516df7 | 108 | #endif |
VaLaraC | 0:9aa960516df7 | 109 | led=1; |
VaLaraC | 0:9aa960516df7 | 110 | wait(tm); |
VaLaraC | 0:9aa960516df7 | 111 | led=0; |
VaLaraC | 0:9aa960516df7 | 112 | } |
VaLaraC | 0:9aa960516df7 | 113 | void draw() |
VaLaraC | 0:9aa960516df7 | 114 | { |
VaLaraC | 0:9aa960516df7 | 115 | myServoZ.pulsewidth_us(POSDRAW); |
VaLaraC | 0:9aa960516df7 | 116 | wait(1); |
VaLaraC | 0:9aa960516df7 | 117 | } |
VaLaraC | 0:9aa960516df7 | 118 | void nodraw() |
VaLaraC | 0:9aa960516df7 | 119 | { |
VaLaraC | 0:9aa960516df7 | 120 | myServoZ.pulsewidth_us(MAXPOS); |
VaLaraC | 0:9aa960516df7 | 121 | wait(1); |
VaLaraC | 0:9aa960516df7 | 122 | } |
VaLaraC | 0:9aa960516df7 | 123 | uint8_t SSTIME = 100; |
VaLaraC | 0:9aa960516df7 | 124 | void tiempo_de_paso(int SST) |
VaLaraC | 0:9aa960516df7 | 125 | { |
VaLaraC | 0:9aa960516df7 | 126 | SSTIME=SST; |
VaLaraC | 0:9aa960516df7 | 127 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 128 | command.printf("\nTiempo en pasos definida en:%i\n",SSTIME); |
VaLaraC | 0:9aa960516df7 | 129 | #endif |
VaLaraC | 0:9aa960516df7 | 130 | } |
VaLaraC | 0:9aa960516df7 | 131 | void vertex2d(float x, float y, float z) |
VaLaraC | 0:9aa960516df7 | 132 | { |
VaLaraC | 0:9aa960516df7 | 133 | int pulseX = coord2us(x); |
VaLaraC | 0:9aa960516df7 | 134 | int pulseY = coord2us(y); |
VaLaraC | 0:9aa960516df7 | 135 | int pulseZ = coord2us(z); |
VaLaraC | 0:9aa960516df7 | 136 | myServoX.pulsewidth_us(pulseX); |
VaLaraC | 0:9aa960516df7 | 137 | myServoY.pulsewidth_us(pulseY); |
VaLaraC | 0:9aa960516df7 | 138 | myServoZ.pulsewidth_us(pulseZ); |
VaLaraC | 0:9aa960516df7 | 139 | wait_ms(SSTIME); |
VaLaraC | 0:9aa960516df7 | 140 | } |
VaLaraC | 0:9aa960516df7 | 141 | |
VaLaraC | 0:9aa960516df7 | 142 | uint8_t posx_old=0; // posición anterior del eje X |
VaLaraC | 0:9aa960516df7 | 143 | uint8_t posy_old=0; // posición anterior del eje Y |
VaLaraC | 0:9aa960516df7 | 144 | uint8_t posz_old=0; // posición anterior del eje Y |
VaLaraC | 0:9aa960516df7 | 145 | uint8_t ss_time=100; // tiempo de espera para moverse 1 mm en microsegundos |
VaLaraC | 0:9aa960516df7 | 146 | |
VaLaraC | 0:9aa960516df7 | 147 | void put_sstime(uint8_t vtime){ |
VaLaraC | 0:9aa960516df7 | 148 | ss_time=vtime; |
VaLaraC | 0:9aa960516df7 | 149 | } |
VaLaraC | 0:9aa960516df7 | 150 | void sstime(uint8_t x, uint8_t y, uint8_t z) |
VaLaraC | 0:9aa960516df7 | 151 | { |
VaLaraC | 0:9aa960516df7 | 152 | double dx=abs(x-posx_old); |
VaLaraC | 0:9aa960516df7 | 153 | double dy=abs(y-posy_old); |
VaLaraC | 0:9aa960516df7 | 154 | double dz=abs(z-posz_old); |
VaLaraC | 0:9aa960516df7 | 155 | double dist= sqrt(dx*dx+dy*dy+dz*dz); |
VaLaraC | 0:9aa960516df7 | 156 | wait_ms((int)(ss_time*dist)); |
VaLaraC | 0:9aa960516df7 | 157 | posx_old=x; |
VaLaraC | 0:9aa960516df7 | 158 | posy_old=y; |
VaLaraC | 0:9aa960516df7 | 159 | posy_old=z; |
VaLaraC | 0:9aa960516df7 | 160 | } |
VaLaraC | 0:9aa960516df7 | 161 | void initdraw(float x, float y) |
VaLaraC | 0:9aa960516df7 | 162 | { |
VaLaraC | 0:9aa960516df7 | 163 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 164 | Sdebug.printf("inicia dibujo \n"); |
VaLaraC | 0:9aa960516df7 | 165 | #endif |
VaLaraC | 0:9aa960516df7 | 166 | vertex2d (x,y,abajo); |
VaLaraC | 0:9aa960516df7 | 167 | wait(1); |
VaLaraC | 0:9aa960516df7 | 168 | draw(); |
VaLaraC | 0:9aa960516df7 | 169 | } |
VaLaraC | 0:9aa960516df7 | 170 | |
VaLaraC | 0:9aa960516df7 | 171 | void resolucion(int res){ |
VaLaraC | 0:9aa960516df7 | 172 | RSTEP = res; |
VaLaraC | 0:9aa960516df7 | 173 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 174 | command.printf("Resolucion definida en = %i \n", RSTEP); |
VaLaraC | 0:9aa960516df7 | 175 | #endif |
VaLaraC | 0:9aa960516df7 | 176 | } |
VaLaraC | 0:9aa960516df7 | 177 | void home() |
VaLaraC | 0:9aa960516df7 | 178 | { |
VaLaraC | 0:9aa960516df7 | 179 | vertex2d(0,0,0); |
VaLaraC | 0:9aa960516df7 | 180 | wait_ms(rap); |
VaLaraC | 0:9aa960516df7 | 181 | } |
VaLaraC | 0:9aa960516df7 | 182 | void maxpos() |
VaLaraC | 0:9aa960516df7 | 183 | { |
VaLaraC | 0:9aa960516df7 | 184 | vertex2d(250,250,150); |
VaLaraC | 0:9aa960516df7 | 185 | } |
VaLaraC | 0:9aa960516df7 | 186 | void punto(uint8_t x, uint8_t y) |
VaLaraC | 0:9aa960516df7 | 187 | { |
VaLaraC | 0:9aa960516df7 | 188 | command.printf("Coord x= %i, coord y=%i \n",x,y); |
VaLaraC | 0:9aa960516df7 | 189 | vertex2d(x,y,0); |
VaLaraC | 0:9aa960516df7 | 190 | wait_ms(1000); |
VaLaraC | 0:9aa960516df7 | 191 | vertex2d(x,y,11); |
VaLaraC | 0:9aa960516df7 | 192 | wait_ms(1000); |
VaLaraC | 0:9aa960516df7 | 193 | vertex2d(x,y,0); |
VaLaraC | 0:9aa960516df7 | 194 | wait_ms(1000); |
VaLaraC | 0:9aa960516df7 | 195 | home(); |
VaLaraC | 0:9aa960516df7 | 196 | } |
VaLaraC | 0:9aa960516df7 | 197 | |
VaLaraC | 0:9aa960516df7 | 198 | void line(float xi, float xf, float yi, float yf) |
VaLaraC | 0:9aa960516df7 | 199 | { |
VaLaraC | 0:9aa960516df7 | 200 | // los parametros son coordenadas iniciales y finales de la linea en milimetros |
VaLaraC | 0:9aa960516df7 | 201 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 202 | Sdebug.printf("linea Xi= %f,Xf= %f,Yi= %f,Yf= %f \n", xi,xf,yi,yf); |
VaLaraC | 0:9aa960516df7 | 203 | #endif |
VaLaraC | 0:9aa960516df7 | 204 | float xp; float yp; |
VaLaraC | 0:9aa960516df7 | 205 | float m= (yf-yi)/(xf-xi); |
VaLaraC | 0:9aa960516df7 | 206 | float b=(yf-m*xf); |
VaLaraC | 0:9aa960516df7 | 207 | vertex2d(0,0,4); |
VaLaraC | 0:9aa960516df7 | 208 | wait(1); |
VaLaraC | 0:9aa960516df7 | 209 | vertex2d(xi,yi,abajo); |
VaLaraC | 0:9aa960516df7 | 210 | wait(1); |
VaLaraC | 0:9aa960516df7 | 211 | for (xp =xi;xp<=xf;xp+=RSTEP) |
VaLaraC | 0:9aa960516df7 | 212 | { |
VaLaraC | 0:9aa960516df7 | 213 | yp =m*xp+b; |
VaLaraC | 0:9aa960516df7 | 214 | vertex2d(xp,yp,abajo); |
VaLaraC | 0:9aa960516df7 | 215 | // savep( buffer_POS[INITPARAMETER==02]buffer_POS[INITPARAMETER+1==xp], buffer_POS[INITPARAMETER+2==xf],buffer_POS[INITPARAMETER+3==yp], buffer_POS[INITPARAMETER+4==yf]); |
VaLaraC | 0:9aa960516df7 | 216 | |
VaLaraC | 0:9aa960516df7 | 217 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 218 | Sdebug.printf("coord X = %f,coord Y = %f \n", xp,yp); |
VaLaraC | 0:9aa960516df7 | 219 | #endif |
VaLaraC | 0:9aa960516df7 | 220 | } |
VaLaraC | 0:9aa960516df7 | 221 | nodraw(); |
VaLaraC | 0:9aa960516df7 | 222 | home(); |
VaLaraC | 0:9aa960516df7 | 223 | } |
VaLaraC | 0:9aa960516df7 | 224 | void rectangle(uint8_t xi, uint8_t yi,uint8_t a, uint8_t h) |
VaLaraC | 0:9aa960516df7 | 225 | { |
VaLaraC | 0:9aa960516df7 | 226 | int x,y; |
VaLaraC | 0:9aa960516df7 | 227 | x = a+ xi; |
VaLaraC | 0:9aa960516df7 | 228 | y = h+ yi; |
VaLaraC | 0:9aa960516df7 | 229 | |
VaLaraC | 0:9aa960516df7 | 230 | vertex2d(xi,yi,arriba); |
VaLaraC | 0:9aa960516df7 | 231 | wait_ms(600); |
VaLaraC | 0:9aa960516df7 | 232 | vertex2d(xi,yi,abajo); |
VaLaraC | 0:9aa960516df7 | 233 | wait_ms(600); |
VaLaraC | 0:9aa960516df7 | 234 | vertex2d(x,yi,abajo); |
VaLaraC | 0:9aa960516df7 | 235 | wait_ms(600); |
VaLaraC | 0:9aa960516df7 | 236 | vertex2d(x,y,abajo); |
VaLaraC | 0:9aa960516df7 | 237 | wait_ms(600); |
VaLaraC | 0:9aa960516df7 | 238 | vertex2d(xi,y,abajo); |
VaLaraC | 0:9aa960516df7 | 239 | wait_ms(600); |
VaLaraC | 0:9aa960516df7 | 240 | vertex2d(xi,yi,abajo); |
VaLaraC | 0:9aa960516df7 | 241 | wait_ms(600); |
VaLaraC | 0:9aa960516df7 | 242 | home(); |
VaLaraC | 0:9aa960516df7 | 243 | } |
VaLaraC | 0:9aa960516df7 | 244 | void circle(uint8_t x1, uint8_t y1,uint8_t r) |
VaLaraC | 0:9aa960516df7 | 245 | { |
VaLaraC | 0:9aa960516df7 | 246 | double x2,y2,i,j; |
VaLaraC | 0:9aa960516df7 | 247 | vertex2d(x1 ,y1,arriba); |
VaLaraC | 0:9aa960516df7 | 248 | wait(1); |
VaLaraC | 0:9aa960516df7 | 249 | wait_ms(rap); |
VaLaraC | 0:9aa960516df7 | 250 | |
VaLaraC | 0:9aa960516df7 | 251 | for ( x2=0;x2<=r;x2++){ |
VaLaraC | 0:9aa960516df7 | 252 | y2= pow(((r*r)-(x2*x2)),0.5); |
VaLaraC | 0:9aa960516df7 | 253 | i= x2+x1; |
VaLaraC | 0:9aa960516df7 | 254 | j=y1+y2; |
VaLaraC | 0:9aa960516df7 | 255 | vertex2d(i ,j,abajo) ; |
VaLaraC | 0:9aa960516df7 | 256 | wait_ms(30); |
VaLaraC | 0:9aa960516df7 | 257 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 258 | Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); |
VaLaraC | 0:9aa960516df7 | 259 | #endif |
VaLaraC | 0:9aa960516df7 | 260 | } |
VaLaraC | 0:9aa960516df7 | 261 | |
VaLaraC | 0:9aa960516df7 | 262 | for ( x2=r;x2>=0;x2--){ |
VaLaraC | 0:9aa960516df7 | 263 | y2= pow(((r*r)-(x2*x2)),0.5); |
VaLaraC | 0:9aa960516df7 | 264 | i= x2+x1; |
VaLaraC | 0:9aa960516df7 | 265 | j= y1 - y2; |
VaLaraC | 0:9aa960516df7 | 266 | vertex2d(i ,j,abajo) ; |
VaLaraC | 0:9aa960516df7 | 267 | wait_ms(30); |
VaLaraC | 0:9aa960516df7 | 268 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 269 | Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); |
VaLaraC | 0:9aa960516df7 | 270 | #endif |
VaLaraC | 0:9aa960516df7 | 271 | } |
VaLaraC | 0:9aa960516df7 | 272 | |
VaLaraC | 0:9aa960516df7 | 273 | for ( x2=0;x2<=r;x2++){ |
VaLaraC | 0:9aa960516df7 | 274 | y2= pow(((r*r)-(x2*x2)),0.5); |
VaLaraC | 0:9aa960516df7 | 275 | i= x1- x2; |
VaLaraC | 0:9aa960516df7 | 276 | j= y1 - y2; |
VaLaraC | 0:9aa960516df7 | 277 | vertex2d(i ,j,abajo) ; |
VaLaraC | 0:9aa960516df7 | 278 | wait_ms(30); |
VaLaraC | 0:9aa960516df7 | 279 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 280 | Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); |
VaLaraC | 0:9aa960516df7 | 281 | #endif |
VaLaraC | 0:9aa960516df7 | 282 | } |
VaLaraC | 0:9aa960516df7 | 283 | for ( x2=r;x2>=0;x2--){ |
VaLaraC | 0:9aa960516df7 | 284 | y2= pow(((r*r)-(x2*x2)),0.5); |
VaLaraC | 0:9aa960516df7 | 285 | i= x1-x2; |
VaLaraC | 0:9aa960516df7 | 286 | j=y1+y2; |
VaLaraC | 0:9aa960516df7 | 287 | vertex2d(i ,j,abajo) ; |
VaLaraC | 0:9aa960516df7 | 288 | wait_ms(30); |
VaLaraC | 0:9aa960516df7 | 289 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 290 | Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); |
VaLaraC | 0:9aa960516df7 | 291 | #endif |
VaLaraC | 0:9aa960516df7 | 292 | } |
VaLaraC | 0:9aa960516df7 | 293 | vertex2d(i ,j,arriba) ; |
VaLaraC | 0:9aa960516df7 | 294 | wait_ms(rap); |
VaLaraC | 0:9aa960516df7 | 295 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 296 | Sdebug.printf("coord X = %f,coord Y = %f \n", i,j); |
VaLaraC | 0:9aa960516df7 | 297 | #endif |
VaLaraC | 0:9aa960516df7 | 298 | |
VaLaraC | 0:9aa960516df7 | 299 | if(x2==r) |
VaLaraC | 0:9aa960516df7 | 300 | { |
VaLaraC | 0:9aa960516df7 | 301 | wait(1); |
VaLaraC | 0:9aa960516df7 | 302 | } |
VaLaraC | 0:9aa960516df7 | 303 | home(); |
VaLaraC | 0:9aa960516df7 | 304 | } |
VaLaraC | 0:9aa960516df7 | 305 | |
VaLaraC | 0:9aa960516df7 | 306 | |
VaLaraC | 0:9aa960516df7 | 307 | |
VaLaraC | 0:9aa960516df7 | 308 | void savep(float pos){ |
VaLaraC | 0:9aa960516df7 | 309 | for (uint8_t i=0; i<BUFF_POS;i++) |
VaLaraC | 0:9aa960516df7 | 310 | { |
VaLaraC | 0:9aa960516df7 | 311 | buffer_command[i]=command.getc(); |
VaLaraC | 0:9aa960516df7 | 312 | |
VaLaraC | 0:9aa960516df7 | 313 | } |
VaLaraC | 0:9aa960516df7 | 314 | |
VaLaraC | 0:9aa960516df7 | 315 | } |
VaLaraC | 0:9aa960516df7 | 316 | |
VaLaraC | 0:9aa960516df7 | 317 | void reanudar(){ |
VaLaraC | 0:9aa960516df7 | 318 | led =0; |
VaLaraC | 0:9aa960516df7 | 319 | p=0; |
VaLaraC | 0:9aa960516df7 | 320 | |
VaLaraC | 0:9aa960516df7 | 321 | |
VaLaraC | 0:9aa960516df7 | 322 | } |
VaLaraC | 0:9aa960516df7 | 323 | |
VaLaraC | 0:9aa960516df7 | 324 | |
VaLaraC | 0:9aa960516df7 | 325 | void pause(){ |
VaLaraC | 0:9aa960516df7 | 326 | p=1; |
VaLaraC | 0:9aa960516df7 | 327 | led = !led; |
VaLaraC | 0:9aa960516df7 | 328 | timeout.attach(&pause, 300); |
VaLaraC | 0:9aa960516df7 | 329 | // while(1); |
VaLaraC | 0:9aa960516df7 | 330 | |
VaLaraC | 0:9aa960516df7 | 331 | |
VaLaraC | 0:9aa960516df7 | 332 | } |
VaLaraC | 0:9aa960516df7 | 333 | |
VaLaraC | 0:9aa960516df7 | 334 | void stop() //Funcion para detener el programa |
VaLaraC | 0:9aa960516df7 | 335 | { |
VaLaraC | 0:9aa960516df7 | 336 | exit(0); |
VaLaraC | 0:9aa960516df7 | 337 | home(); |
VaLaraC | 0:9aa960516df7 | 338 | } |
VaLaraC | 0:9aa960516df7 | 339 | |
VaLaraC | 0:9aa960516df7 | 340 | void command_exe() |
VaLaraC | 0:9aa960516df7 | 341 | { |
VaLaraC | 0:9aa960516df7 | 342 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 343 | command.printf("Ejecutando comando: "); |
VaLaraC | 0:9aa960516df7 | 344 | #endif |
VaLaraC | 0:9aa960516df7 | 345 | switch (buffer_command[COMM_N]) |
VaLaraC | 0:9aa960516df7 | 346 | { |
VaLaraC | 0:9aa960516df7 | 347 | case (LED): |
VaLaraC | 0:9aa960516df7 | 348 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 349 | command.printf("LED on\n"); |
VaLaraC | 0:9aa960516df7 | 350 | #endif |
VaLaraC | 0:9aa960516df7 | 351 | command_led(buffer_command[INITPARAMETER]); |
VaLaraC | 0:9aa960516df7 | 352 | break; |
VaLaraC | 0:9aa960516df7 | 353 | |
VaLaraC | 0:9aa960516df7 | 354 | case (PUNTO): |
VaLaraC | 0:9aa960516df7 | 355 | punto(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]); |
VaLaraC | 0:9aa960516df7 | 356 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 357 | command.printf("Dibujando Punto"); |
VaLaraC | 0:9aa960516df7 | 358 | #endif |
VaLaraC | 0:9aa960516df7 | 359 | break; |
VaLaraC | 0:9aa960516df7 | 360 | |
VaLaraC | 0:9aa960516df7 | 361 | case LINE: |
VaLaraC | 0:9aa960516df7 | 362 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 363 | command.printf("Dibujando Linea\n"); |
VaLaraC | 0:9aa960516df7 | 364 | #endif |
VaLaraC | 0:9aa960516df7 | 365 | line(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]); |
VaLaraC | 0:9aa960516df7 | 366 | |
VaLaraC | 0:9aa960516df7 | 367 | |
VaLaraC | 0:9aa960516df7 | 368 | break; |
VaLaraC | 0:9aa960516df7 | 369 | |
VaLaraC | 0:9aa960516df7 | 370 | case RECTANGLE: |
VaLaraC | 0:9aa960516df7 | 371 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 372 | command.printf("Dibujando Rectangulo\n"); |
VaLaraC | 0:9aa960516df7 | 373 | #endif |
VaLaraC | 0:9aa960516df7 | 374 | rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]); |
VaLaraC | 0:9aa960516df7 | 375 | break; |
VaLaraC | 0:9aa960516df7 | 376 | |
VaLaraC | 0:9aa960516df7 | 377 | case (CIRCLE): |
VaLaraC | 0:9aa960516df7 | 378 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 379 | command.printf("Dibujando Circulo\n"); |
VaLaraC | 0:9aa960516df7 | 380 | #endif |
VaLaraC | 0:9aa960516df7 | 381 | circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]); |
VaLaraC | 0:9aa960516df7 | 382 | break; |
VaLaraC | 0:9aa960516df7 | 383 | |
VaLaraC | 0:9aa960516df7 | 384 | case (STOP): |
VaLaraC | 0:9aa960516df7 | 385 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 386 | command.printf(" STOP\n"); |
VaLaraC | 0:9aa960516df7 | 387 | #endif |
VaLaraC | 0:9aa960516df7 | 388 | break; |
VaLaraC | 0:9aa960516df7 | 389 | |
VaLaraC | 0:9aa960516df7 | 390 | case (PAUSE): |
VaLaraC | 0:9aa960516df7 | 391 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 392 | command.printf("programa pausado\n"); |
VaLaraC | 0:9aa960516df7 | 393 | #endif |
VaLaraC | 0:9aa960516df7 | 394 | pause(); |
VaLaraC | 0:9aa960516df7 | 395 | break; |
VaLaraC | 0:9aa960516df7 | 396 | |
VaLaraC | 0:9aa960516df7 | 397 | case (REANUDAR): |
VaLaraC | 0:9aa960516df7 | 398 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 399 | command.printf("draw reanudar programa\n"); |
VaLaraC | 0:9aa960516df7 | 400 | #endif |
VaLaraC | 0:9aa960516df7 | 401 | reanudar(); |
VaLaraC | 0:9aa960516df7 | 402 | break; |
VaLaraC | 0:9aa960516df7 | 403 | |
VaLaraC | 0:9aa960516df7 | 404 | case (RESOLUCION): |
VaLaraC | 0:9aa960516df7 | 405 | resolucion(buffer_command[INITPARAMETER]); |
VaLaraC | 0:9aa960516df7 | 406 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 407 | command.printf("Resolucion definida"); |
VaLaraC | 0:9aa960516df7 | 408 | #endif |
VaLaraC | 0:9aa960516df7 | 409 | break; |
VaLaraC | 0:9aa960516df7 | 410 | case (HOME): |
VaLaraC | 0:9aa960516df7 | 411 | home(); |
VaLaraC | 0:9aa960516df7 | 412 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 413 | command.printf("Posicion Home"); |
VaLaraC | 0:9aa960516df7 | 414 | #endif |
VaLaraC | 0:9aa960516df7 | 415 | break; |
VaLaraC | 0:9aa960516df7 | 416 | |
VaLaraC | 0:9aa960516df7 | 417 | case (TIEMPO_DE_PASO): |
VaLaraC | 0:9aa960516df7 | 418 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 419 | command.printf("Nuevo tiempo para paso:\res"); |
VaLaraC | 0:9aa960516df7 | 420 | #endif |
VaLaraC | 0:9aa960516df7 | 421 | tiempo_de_paso(buffer_command[INITPARAMETER]); |
VaLaraC | 0:9aa960516df7 | 422 | break; |
VaLaraC | 0:9aa960516df7 | 423 | |
VaLaraC | 0:9aa960516df7 | 424 | default: |
VaLaraC | 0:9aa960516df7 | 425 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 426 | command.printf("comando no encontrado\n"); |
VaLaraC | 0:9aa960516df7 | 427 | #endif |
VaLaraC | 0:9aa960516df7 | 428 | |
VaLaraC | 0:9aa960516df7 | 429 | } |
VaLaraC | 0:9aa960516df7 | 430 | } |
VaLaraC | 0:9aa960516df7 | 431 | |
VaLaraC | 0:9aa960516df7 | 432 | |
VaLaraC | 0:9aa960516df7 | 433 | |
VaLaraC | 0:9aa960516df7 | 434 | |
VaLaraC | 0:9aa960516df7 | 435 | |
VaLaraC | 0:9aa960516df7 | 436 | |
VaLaraC | 0:9aa960516df7 | 437 | |
VaLaraC | 0:9aa960516df7 | 438 | |
VaLaraC | 0:9aa960516df7 | 439 | |
VaLaraC | 0:9aa960516df7 | 440 | |
VaLaraC | 0:9aa960516df7 | 441 | int main() { |
VaLaraC | 0:9aa960516df7 | 442 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 443 | command.printf("inicio con debug\n"); |
VaLaraC | 0:9aa960516df7 | 444 | #else |
VaLaraC | 0:9aa960516df7 | 445 | command.printf("inicio sin debug\n"); |
VaLaraC | 0:9aa960516df7 | 446 | #endif |
VaLaraC | 0:9aa960516df7 | 447 | uint8_t val; |
VaLaraC | 0:9aa960516df7 | 448 | // mybutton.rise(&stop); |
VaLaraC | 0:9aa960516df7 | 449 | while(1){ |
VaLaraC | 0:9aa960516df7 | 450 | val=command.getc(); |
VaLaraC | 0:9aa960516df7 | 451 | if (val== '<'){ |
VaLaraC | 0:9aa960516df7 | 452 | Read_command(); |
VaLaraC | 0:9aa960516df7 | 453 | // ticker1.attach(&Read_command,05); |
VaLaraC | 0:9aa960516df7 | 454 | if (check_command()){ |
VaLaraC | 0:9aa960516df7 | 455 | command_exe(); |
VaLaraC | 0:9aa960516df7 | 456 | } |
VaLaraC | 0:9aa960516df7 | 457 | else{ |
VaLaraC | 0:9aa960516df7 | 458 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 459 | command.printf("\n ERROR COMANDO -> "); |
VaLaraC | 0:9aa960516df7 | 460 | echo_command(); |
VaLaraC | 0:9aa960516df7 | 461 | #endif |
VaLaraC | 0:9aa960516df7 | 462 | } |
VaLaraC | 0:9aa960516df7 | 463 | } |
VaLaraC | 0:9aa960516df7 | 464 | else{ |
VaLaraC | 0:9aa960516df7 | 465 | #if DEBUG |
VaLaraC | 0:9aa960516df7 | 466 | command.printf("error de inicio de trama: "); |
VaLaraC | 0:9aa960516df7 | 467 | command.putc(val); |
VaLaraC | 0:9aa960516df7 | 468 | #endif |
VaLaraC | 0:9aa960516df7 | 469 | } |
VaLaraC | 0:9aa960516df7 | 470 | |
VaLaraC | 0:9aa960516df7 | 471 | } |
VaLaraC | 0:9aa960516df7 | 472 | |
VaLaraC | 0:9aa960516df7 | 473 | |
VaLaraC | 0:9aa960516df7 | 474 | //ticker1.attach(&main,05); |
VaLaraC | 0:9aa960516df7 | 475 | |
VaLaraC | 0:9aa960516df7 | 476 | |
VaLaraC | 0:9aa960516df7 | 477 | } |