Piccolo / Mbed 2 deprecated Piccolo_10

codigo

Dependencies:   mbed

main.cpp@2:b0ed0b16b9c8, 2017-11-24 (annotated)

Committer:
Iducanyi
Date:
Fri Nov 24 00:14:52 2017 +0000
Revision:
2:b0ed0b16b9c8
Parent:
1:c0ff30bf8db2 
c?digo PICCOLO 3er Corte

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Vanessa620 0:02b166bd1478 1 #include "mbed.h"
Vanessa620 0:02b166bd1478 2 #include "InterruptIn.h"
Vanessa620 0:02b166bd1478 3 #include "stm32f4xx_hal.h"
Vanessa620 0:02b166bd1478 4
Vanessa620 0:02b166bd1478 5 //Serial blue(PA_9, PA_10); //PA_10=TX , PA_9=RX ... D8 y D2 respectivamente en la nucleo tx1
Vanessa620 0:02b166bd1478 6 Serial pc(PA_9, PA_10);
Iducanyi 1:c0ff30bf8db2 7 Serial device(D1, D0, 9600); // tx rx
Iducanyi 1:c0ff30bf8db2 8 Serial command(D8,D2);
Vanessa620 0:02b166bd1478 9 DigitalOut led(LED1);
Vanessa620 0:02b166bd1478 10
Iducanyi 1:c0ff30bf8db2 11
Vanessa620 0:02b166bd1478 12 void command_led();
Vanessa620 0:02b166bd1478 13 void punto();
Vanessa620 0:02b166bd1478 14 void linea();
Vanessa620 0:02b166bd1478 15 void rectangle();
Vanessa620 0:02b166bd1478 16 void circle();
Vanessa620 0:02b166bd1478 17 void home();
Vanessa620 0:02b166bd1478 18 void resolucion();
Vanessa620 0:02b166bd1478 19 void tiempo_pasos();
Vanessa620 0:02b166bd1478 20 void stop();
Vanessa620 0:02b166bd1478 21 void pausa();
Vanessa620 0:02b166bd1478 22 void reanudar();
Vanessa620 0:02b166bd1478 23 void mover();
Vanessa620 0:02b166bd1478 24 void recibeByte();
Vanessa620 0:02b166bd1478 25 void stepmotor();
Vanessa620 0:02b166bd1478 26 void command_exe();
Vanessa620 0:02b166bd1478 27
Vanessa620 0:02b166bd1478 28 #define DEBUG 1 // Servomotores
Vanessa620 0:02b166bd1478 29 PwmOut myServoX(PB_3);
Vanessa620 0:02b166bd1478 30 PwmOut myServoY(PB_5);
Vanessa620 0:02b166bd1478 31 PwmOut myServoZ(PB_4);
Vanessa620 0:02b166bd1478 32 //*****************************************************************************
Vanessa620 0:02b166bd1478 33 // COMANDO MOVER MOTOR
Vanessa620 0:02b166bd1478 34 // |POS 1|POS 2|POS 3|POS 4| POS 5|
Vanessa620 0:02b166bd1478 35 // | < | #C | a | b | > |
Vanessa620 0:02b166bd1478 36 // | 3C |0 al A| D1 | D2 | D3 | D4 | 3E |
Vanessa620 0:02b166bd1478 37 // #C -> indica el comando. En este caso es de 0 a 10 (en hexadesimal para enviar comando desde techado de 0 a A)
Vanessa620 0:02b166bd1478 38 // a,b,c,d parametros del comando
Vanessa620 0:02b166bd1478 39 // <,> -> inicio, y fin de comando
Vanessa620 0:02b166bd1478 40 // el inicio de comando no se almacena en el buffer
Vanessa620 0:02b166bd1478 41 //*****************************************************************************
Vanessa620 0:02b166bd1478 42
Vanessa620 0:02b166bd1478 43 // VARIABLES PARA DEFINIR EL COMMANDO
Vanessa620 0:02b166bd1478 44 #define BUFF_SIZE 6
Vanessa620 0:02b166bd1478 45 #define COMM_N 0
Vanessa620 0:02b166bd1478 46 #define INITPARAMETER 1
Vanessa620 0:02b166bd1478 47 #define MAXPOS 50
Vanessa620 0:02b166bd1478 48 #define POSDRAW 00
Vanessa620 0:02b166bd1478 49 #define SS_TIME 500
Vanessa620 0:02b166bd1478 50 #define PI 3.1415926
Vanessa620 0:02b166bd1478 51 int RSTEP = 1; // en milimetros
Vanessa620 0:02b166bd1478 52 int cont = 1;
Vanessa620 0:02b166bd1478 53 int tiempo = 5;
Vanessa620 0:02b166bd1478 54 //int time = 0;
Vanessa620 0:02b166bd1478 55 int pause = 1; // Tiempo en us (tiempo entre pasos) nos da la velocidad
Vanessa620 0:02b166bd1478 56 uint8_t val;
Iducanyi 1:c0ff30bf8db2 57 uint8_t estado = 1;
Vanessa620 0:02b166bd1478 58 // COMANDOS
Vanessa620 0:02b166bd1478 59 #define LED_NC 0 //ok
Vanessa620 0:02b166bd1478 60 #define DOT_NC 1 //ok
Vanessa620 0:02b166bd1478 61 #define LINE_NC 2 //ok
Vanessa620 0:02b166bd1478 62 #define RECTANGLE_NC 3 //ok
Vanessa620 0:02b166bd1478 63 #define CICLE_NC 4 //ok
Vanessa620 0:02b166bd1478 64 #define HOME_NC 5 //ok
Vanessa620 0:02b166bd1478 65 #define RESOLUCION_NC 6 //ok
Vanessa620 0:02b166bd1478 66 #define TIEMPOPASOS_NC 7
Vanessa620 0:02b166bd1478 67 #define STOP_NC 8
Vanessa620 0:02b166bd1478 68 #define PAUSA_NC 9
Vanessa620 0:02b166bd1478 69 #define REANUDAR_NC 10
Vanessa620 0:02b166bd1478 70 #define MOVER_CN 11
Vanessa620 0:02b166bd1478 71
Vanessa620 0:02b166bd1478 72 //////////////////Funciones para mover los servos/////////////////////
Vanessa620 0:02b166bd1478 73
Vanessa620 0:02b166bd1478 74 int coord2pulsex(float coord)
Vanessa620 0:02b166bd1478 75 { if(0 <= coord <= MAXPOS)
Vanessa620 0:02b166bd1478 76 return int(800+coord*1850/50);// u6
Vanessa620 0:02b166bd1478 77 return 750; }
Vanessa620 0:02b166bd1478 78
Vanessa620 0:02b166bd1478 79 int coord2pulsey(float coord)
Vanessa620 0:02b166bd1478 80 { if(0 <= coord <= MAXPOS)
Vanessa620 0:02b166bd1478 81 return int(750+coord*1400/42);// u6
Vanessa620 0:02b166bd1478 82 return 750; }
Vanessa620 0:02b166bd1478 83
Vanessa620 0:02b166bd1478 84 void vertex2d(float x, float y){ // Funcion para enviarle la posicion a (x,y)
Vanessa620 0:02b166bd1478 85
Vanessa620 0:02b166bd1478 86 int pulseX = coord2pulsex(x);
Vanessa620 0:02b166bd1478 87 int pulseY = coord2pulsey(y);
Vanessa620 0:02b166bd1478 88
Vanessa620 0:02b166bd1478 89 myServoX.pulsewidth_us(pulseX);
Vanessa620 0:02b166bd1478 90 myServoY.pulsewidth_us(pulseY+8); }
Vanessa620 0:02b166bd1478 91
Vanessa620 0:02b166bd1478 92 uint8_t posx_old=0; // posición anterior del eje X
Vanessa620 0:02b166bd1478 93 uint8_t posy_old=0; // posición anterior del eje Y
Vanessa620 0:02b166bd1478 94 uint8_t ss_time=100; // tiempo de espera para moverse 1 mm en microsegundos
Vanessa620 0:02b166bd1478 95 uint8_t buffer_command[BUFF_SIZE]={0,0,0,0,0,0}; // Matriz del Comando enviado
Vanessa620 0:02b166bd1478 96 uint8_t buffer[BUFF_SIZE]={0,0,0,0,0,0};
Vanessa620 0:02b166bd1478 97
Vanessa620 0:02b166bd1478 98 void put_sstime(uint8_t vtime){
Vanessa620 0:02b166bd1478 99 ss_time=vtime; }
Vanessa620 0:02b166bd1478 100
Vanessa620 0:02b166bd1478 101 void sstime(uint8_t x, uint8_t y)
Vanessa620 0:02b166bd1478 102 {
Vanessa620 0:02b166bd1478 103 double dx=abs(x-posx_old);
Vanessa620 0:02b166bd1478 104 double dy=abs(y-posy_old);
Vanessa620 0:02b166bd1478 105 double dist= sqrt(dx*dx+dy*dy);
Vanessa620 0:02b166bd1478 106 wait_ms((int)(ss_time*dist));
Vanessa620 0:02b166bd1478 107 posx_old =x;
Vanessa620 0:02b166bd1478 108 posy_old=y; }
Vanessa620 0:02b166bd1478 109
Vanessa620 0:02b166bd1478 110 void draw(){
Vanessa620 0:02b166bd1478 111 wait(2);
Vanessa620 0:02b166bd1478 112 myServoZ.pulsewidth_ms(2);
Vanessa620 0:02b166bd1478 113 wait_ms(ss_time*2); }
Vanessa620 0:02b166bd1478 114
Vanessa620 0:02b166bd1478 115 void nodraw(){
Vanessa620 0:02b166bd1478 116 myServoZ.pulsewidth_ms(1);
Vanessa620 0:02b166bd1478 117 wait_ms(ss_time*2);
Vanessa620 0:02b166bd1478 118 wait(1);
Vanessa620 0:02b166bd1478 119 //home();
Vanessa620 0:02b166bd1478 120 }
Vanessa620 0:02b166bd1478 121
Vanessa620 0:02b166bd1478 122
Vanessa620 0:02b166bd1478 123 void initdraw(float x, float y)//ok
Vanessa620 0:02b166bd1478 124 {
Vanessa620 0:02b166bd1478 125 vertex2d(x,y);
Vanessa620 0:02b166bd1478 126 wait_ms(ss_time);
Vanessa620 0:02b166bd1478 127 draw();
Vanessa620 0:02b166bd1478 128 }
Vanessa620 0:02b166bd1478 129
Vanessa620 0:02b166bd1478 130 ///////////////FUNCIONES PARA OBTENER E IMPRIMIR EL COMANDO///////////////////
Vanessa620 0:02b166bd1478 131
Vanessa620 0:02b166bd1478 132 void print_num(uint8_t val)
Vanessa620 0:02b166bd1478 133 { if (val <10){
Iducanyi 1:c0ff30bf8db2 134 device.putc(val+0x30);
Vanessa620 0:02b166bd1478 135 command.putc(val+0x30);
Vanessa620 0:02b166bd1478 136 pc.putc(val+0x30);}
Vanessa620 0:02b166bd1478 137 else {
Iducanyi 1:c0ff30bf8db2 138 device.putc(val-9+0x40);
Vanessa620 0:02b166bd1478 139 command.putc(val-9+0x40);
Vanessa620 0:02b166bd1478 140 pc.putc(val-9+0x40); }}
Vanessa620 0:02b166bd1478 141
Vanessa620 0:02b166bd1478 142 void print_bin2hex (uint8_t val)// Imprimir el comando enviado en Hexadecimal
Vanessa620 0:02b166bd1478 143 {
Vanessa620 0:02b166bd1478 144 command.printf(" 0x");
Vanessa620 0:02b166bd1478 145 print_num(val>>4);
Vanessa620 0:02b166bd1478 146 print_num(val&0x0f); }
Vanessa620 0:02b166bd1478 147
Vanessa620 0:02b166bd1478 148 // TODO : TIMEOUT UART SERIAL
Vanessa620 0:02b166bd1478 149
Vanessa620 0:02b166bd1478 150 void Read_command() // Leer el comando que se digito en CoolTerm
Vanessa620 0:02b166bd1478 151 {
Iducanyi 1:c0ff30bf8db2 152 for (uint8_t i=0; i<BUFF_SIZE;i++){
Iducanyi 1:c0ff30bf8db2 153 buffer_command[i]=command.getc();
Iducanyi 1:c0ff30bf8db2 154 }
Iducanyi 1:c0ff30bf8db2 155 /*for (uint8_t i=0; i<BUFF_SIZE;i++){
Iducanyi 1:c0ff30bf8db2 156 buffer_command[i]=device.getc();
Iducanyi 1:c0ff30bf8db2 157 }*/
Iducanyi 1:c0ff30bf8db2 158
Iducanyi 1:c0ff30bf8db2 159 }
Vanessa620 0:02b166bd1478 160
Vanessa620 0:02b166bd1478 161 void echo_command()
Vanessa620 0:02b166bd1478 162 {
Vanessa620 0:02b166bd1478 163 for (uint8_t i=0; i<BUFF_SIZE;i++)
Vanessa620 0:02b166bd1478 164 print_bin2hex(buffer_command[i]); }
Vanessa620 0:02b166bd1478 165
Vanessa620 0:02b166bd1478 166
Vanessa620 0:02b166bd1478 167 uint8_t check_command() //Verifica el ultimo valor del comando enviado '>'
Vanessa620 0:02b166bd1478 168 {
Vanessa620 0:02b166bd1478 169 if (buffer_command[BUFF_SIZE-1]== '>'){
Vanessa620 0:02b166bd1478 170
Vanessa620 0:02b166bd1478 171 #if DEBUG
Vanessa620 0:02b166bd1478 172 command.printf("\nComando:");
Vanessa620 0:02b166bd1478 173 print_bin2hex(buffer_command[COMM_N]);
Vanessa620 0:02b166bd1478 174 command.printf(" -> ");
Vanessa620 0:02b166bd1478 175 #endif
Vanessa620 0:02b166bd1478 176 return 1;
Vanessa620 0:02b166bd1478 177 }
Vanessa620 0:02b166bd1478 178 #if DEBUG
Vanessa620 0:02b166bd1478 179 command.printf("\n ERROR COMANDO -> ");
Vanessa620 0:02b166bd1478 180 echo_command();
Vanessa620 0:02b166bd1478 181 #endif
Vanessa620 0:02b166bd1478 182 return 0; }
Vanessa620 0:02b166bd1478 183
Vanessa620 0:02b166bd1478 184 ///////////////Funciones para dibujar////////////////
Vanessa620 0:02b166bd1478 185
Vanessa620 0:02b166bd1478 186 void command_led(int tm){
Vanessa620 0:02b166bd1478 187
Vanessa620 0:02b166bd1478 188 led=1;
Vanessa620 0:02b166bd1478 189 wait(tm);
Vanessa620 0:02b166bd1478 190 led=0; }
Vanessa620 0:02b166bd1478 191
Vanessa620 0:02b166bd1478 192 void punto(uint8_t x, uint8_t y){
Vanessa620 0:02b166bd1478 193
Vanessa620 0:02b166bd1478 194 vertex2d(x,y);
Vanessa620 0:02b166bd1478 195 wait(1);
Vanessa620 0:02b166bd1478 196 draw();
Vanessa620 0:02b166bd1478 197 wait(1);
Vanessa620 0:02b166bd1478 198 nodraw();
Vanessa620 0:02b166bd1478 199 #if DEBUG
Vanessa620 0:02b166bd1478 200 command.printf("Coord x= %i, coord y=%i \n",x,y);
Vanessa620 0:02b166bd1478 201 #endif
Vanessa620 0:02b166bd1478 202
Vanessa620 0:02b166bd1478 203 }
Vanessa620 0:02b166bd1478 204
Vanessa620 0:02b166bd1478 205 void linea(float xi, float yi, float xf, float yf)
Vanessa620 0:02b166bd1478 206 {
Vanessa620 0:02b166bd1478 207 #if DEBUG
Vanessa620 0:02b166bd1478 208 command.printf("\nCoordenadas xi=%f, yi=%f, xf=%f, yf=%f, resolucion: %i \n", xi,yi,xf,yf,RSTEP);
Vanessa620 0:02b166bd1478 209 #endif
Vanessa620 0:02b166bd1478 210 float xp,yp;
Vanessa620 0:02b166bd1478 211 float m=(yf-yi)/(xf-xi);
Vanessa620 0:02b166bd1478 212 float b=yf-(m*xf);
Vanessa620 0:02b166bd1478 213 #if DEBUG
Vanessa620 0:02b166bd1478 214 command.printf("\n b =%f, m=%f \n", b,m);
Vanessa620 0:02b166bd1478 215 #endif
Vanessa620 0:02b166bd1478 216 float nstep =(m/RSTEP);
Vanessa620 0:02b166bd1478 217 //nstep=RSTEP;
Vanessa620 0:02b166bd1478 218 #if DEBUG
Vanessa620 0:02b166bd1478 219 command.printf("\nstep = %f \n", nstep);
Vanessa620 0:02b166bd1478 220 #endif
Vanessa620 0:02b166bd1478 221 if ((abs(xf-xi))>abs(yf-yi)){
Vanessa620 0:02b166bd1478 222 if (xf>xi){
Iducanyi 2:b0ed0b16b9c8 223 initdraw(xi,yi);//se cambio xp,yp por xi,yi
Vanessa620 0:02b166bd1478 224 for (xp=xi; xp<=xf; xp+=RSTEP){
Vanessa620 0:02b166bd1478 225 yp =m*xp+b;
Vanessa620 0:02b166bd1478 226 vertex2d(xp,yp);
Vanessa620 0:02b166bd1478 227 #if DEBUG
Vanessa620 0:02b166bd1478 228 command.printf(" CASO 1: ( dx>dy & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 0:02b166bd1478 229 #endif
Vanessa620 0:02b166bd1478 230 } }
Vanessa620 0:02b166bd1478 231 else{
Vanessa620 0:02b166bd1478 232 float temp = xi;
Vanessa620 0:02b166bd1478 233 xi = xf;
Vanessa620 0:02b166bd1478 234 xf = temp;
Iducanyi 2:b0ed0b16b9c8 235 initdraw(xi,yi);//se cambio xp,yp por xi,yi
Vanessa620 0:02b166bd1478 236 for (xp=xi; xp<=xf; xp+=RSTEP){
Vanessa620 0:02b166bd1478 237 yp =m*xp+b;
Vanessa620 0:02b166bd1478 238 vertex2d(xp,yp);
Vanessa620 0:02b166bd1478 239 #if DEBUG
Vanessa620 0:02b166bd1478 240 command.printf(" CASO 2: ( dx>dy & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 0:02b166bd1478 241 #endif
Vanessa620 0:02b166bd1478 242 }}}
Vanessa620 0:02b166bd1478 243 else {
Vanessa620 0:02b166bd1478 244 if (yf>yi){
Iducanyi 2:b0ed0b16b9c8 245 initdraw(xp,yp);//se cambio xp,yp por xi,yi
Vanessa620 0:02b166bd1478 246 for (yp=yi; yp<=yf; yp+=RSTEP){
Vanessa620 0:02b166bd1478 247 xp=(yp-b)/m;
Vanessa620 0:02b166bd1478 248 vertex2d(xp,yp);
Vanessa620 0:02b166bd1478 249 #if DEBUG
Vanessa620 0:02b166bd1478 250 command.printf(" CASO 3: ( dy>dx & xf>xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 0:02b166bd1478 251 #endif
Vanessa620 0:02b166bd1478 252 }}
Vanessa620 0:02b166bd1478 253 else{
Vanessa620 0:02b166bd1478 254 float tempo = yi;
Vanessa620 0:02b166bd1478 255 yi = yf;
Vanessa620 0:02b166bd1478 256 yf = tempo;
Iducanyi 2:b0ed0b16b9c8 257 initdraw(xi,yi);//se cambio xp,yp por xi,yi
Vanessa620 0:02b166bd1478 258 for (yp=yi; yp<=yf; yp+=RSTEP){
Vanessa620 0:02b166bd1478 259 xp=(yp-b)/m;
Vanessa620 0:02b166bd1478 260 vertex2d(xp,yp);
Vanessa620 0:02b166bd1478 261 #if DEBUG
Vanessa620 0:02b166bd1478 262 command.printf(" CASO 4: ( dy>dx & xf<xi ) Coordenadas x=%f,y=%f \n", xp,yp);
Vanessa620 0:02b166bd1478 263 #endif
Vanessa620 0:02b166bd1478 264 }
Vanessa620 0:02b166bd1478 265 }
Vanessa620 0:02b166bd1478 266 }
Vanessa620 0:02b166bd1478 267 nodraw();
Vanessa620 0:02b166bd1478 268 }
Vanessa620 0:02b166bd1478 269
Vanessa620 0:02b166bd1478 270 void rectangle(uint8_t x, uint8_t y, uint8_t a, uint8_t h)
Vanessa620 0:02b166bd1478 271 {
Vanessa620 0:02b166bd1478 272 #if DEBUG
Vanessa620 0:02b166bd1478 273 command.printf("\nCoordenadas x=%i, y=%i, ancho=%i, alto=%i, resolucion=%i\n", x,y,a,h,RSTEP);
Vanessa620 0:02b166bd1478 274 #endif
Vanessa620 0:02b166bd1478 275 uint8_t A=x+a;
Vanessa620 0:02b166bd1478 276 uint8_t B=y+h;
Vanessa620 0:02b166bd1478 277 initdraw(x,y);
Vanessa620 0:02b166bd1478 278
Vanessa620 0:02b166bd1478 279 for(uint8_t xi=x; xi<=(x+a); xi+=RSTEP){
Vanessa620 0:02b166bd1478 280 vertex2d(xi,y);
Vanessa620 0:02b166bd1478 281 #if DEBUG
Vanessa620 0:02b166bd1478 282 command.printf("Coordenadas x=%i,y=%i for 1\n", xi,y);
Vanessa620 0:02b166bd1478 283 #endif
Vanessa620 0:02b166bd1478 284 }
Vanessa620 0:02b166bd1478 285 for (uint8_t yi=y; yi<=(y+h); yi+=RSTEP){
Vanessa620 0:02b166bd1478 286 vertex2d(x+a,yi);
Vanessa620 0:02b166bd1478 287 #if DEBUG
Vanessa620 0:02b166bd1478 288 command.printf("Coordenadas x=%i,y=%i for 2\n", x+a,yi);
Vanessa620 0:02b166bd1478 289 #endif
Vanessa620 0:02b166bd1478 290 }
Vanessa620 0:02b166bd1478 291 for(uint8_t xf=A; xf>x; xf= xf - RSTEP){
Vanessa620 0:02b166bd1478 292 vertex2d(xf,B);
Vanessa620 0:02b166bd1478 293 #if DEBUG
Vanessa620 0:02b166bd1478 294 command.printf("Coordenadas x=%i,y=%i for 3\n", xf,B);
Vanessa620 0:02b166bd1478 295 #endif
Vanessa620 0:02b166bd1478 296 }
Vanessa620 0:02b166bd1478 297 for (uint8_t yf=(y+h); yf>y; yf-=RSTEP){
Vanessa620 0:02b166bd1478 298 vertex2d(x,yf);
Vanessa620 0:02b166bd1478 299 #if DEBUG
Vanessa620 0:02b166bd1478 300 command.printf("Coordenadas x=%i,y=%i for 4\n", x,yf);
Vanessa620 0:02b166bd1478 301 #endif
Vanessa620 0:02b166bd1478 302 }
Vanessa620 0:02b166bd1478 303 vertex2d(x,y);
Vanessa620 0:02b166bd1478 304 #if DEBUG
Vanessa620 0:02b166bd1478 305 command.printf("Coordenadas x=%i,y=%i for 4\n", x,y);
Vanessa620 0:02b166bd1478 306 #endif
Vanessa620 0:02b166bd1478 307 nodraw();
Vanessa620 0:02b166bd1478 308 }
Vanessa620 0:02b166bd1478 309 void circle(float cx, float cy, float radio)
Vanessa620 0:02b166bd1478 310 {
Vanessa620 0:02b166bd1478 311 int y;
Vanessa620 0:02b166bd1478 312 int x;
Vanessa620 0:02b166bd1478 313 vertex2d(cx,cy);
Iducanyi 2:b0ed0b16b9c8 314 x=radio*cos(0*3.1416/180);
Iducanyi 2:b0ed0b16b9c8 315 y=radio*sin(0*3.1416/180);
Iducanyi 2:b0ed0b16b9c8 316 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 317 draw();
Iducanyi 2:b0ed0b16b9c8 318
Vanessa620 0:02b166bd1478 319 wait(1);
Vanessa620 0:02b166bd1478 320 for(double i=0; i<=90 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 321 {
Vanessa620 0:02b166bd1478 322 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 323 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 324 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 325 #if DEBUG
Vanessa620 0:02b166bd1478 326 command.printf("position x =%lf ; position y =%lf \n",cos(i*3.1416/180),sin(i*3.1416/180));
Vanessa620 0:02b166bd1478 327 #endif
Vanessa620 0:02b166bd1478 328 wait_ms(10);
Vanessa620 0:02b166bd1478 329 }
Vanessa620 0:02b166bd1478 330 for(double i=90; i<=180 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 331 {
Vanessa620 0:02b166bd1478 332 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 333 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 334 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 335 #if DEBUG
Vanessa620 0:02b166bd1478 336 command.printf("position x =%li ; position y =%li \n",x,y);
Vanessa620 0:02b166bd1478 337 #endif
Vanessa620 0:02b166bd1478 338 wait_ms(10);
Vanessa620 0:02b166bd1478 339 }
Vanessa620 0:02b166bd1478 340 for(double i=180; i<=270 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 341 {
Vanessa620 0:02b166bd1478 342 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 343 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 344 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 345 #if DEBUG
Vanessa620 0:02b166bd1478 346 command.printf("position x =%li ; position y =%li\n",x,y);
Vanessa620 0:02b166bd1478 347 #endif
Vanessa620 0:02b166bd1478 348 wait_ms(10);
Vanessa620 0:02b166bd1478 349 }
Vanessa620 0:02b166bd1478 350
Vanessa620 0:02b166bd1478 351 for(double i=270; i<=360 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 352 {
Vanessa620 0:02b166bd1478 353 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 354 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 355 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 356 #if DEBUG
Vanessa620 0:02b166bd1478 357 command.printf("position x =%li , position y =%li \n",x,y);
Vanessa620 0:02b166bd1478 358 #endif
Vanessa620 0:02b166bd1478 359 wait_ms(10);
Vanessa620 0:02b166bd1478 360 }
Vanessa620 0:02b166bd1478 361 wait(1);
Vanessa620 0:02b166bd1478 362 nodraw();
Vanessa620 0:02b166bd1478 363 }
Vanessa620 0:02b166bd1478 364
Vanessa620 0:02b166bd1478 365 void home(){
Vanessa620 0:02b166bd1478 366 vertex2d(0,0);
Vanessa620 0:02b166bd1478 367 nodraw();
Vanessa620 0:02b166bd1478 368
Vanessa620 0:02b166bd1478 369 }
Vanessa620 0:02b166bd1478 370
Vanessa620 0:02b166bd1478 371 void resolucion(int res){
Vanessa620 0:02b166bd1478 372
Vanessa620 0:02b166bd1478 373 RSTEP = res;
Vanessa620 0:02b166bd1478 374 #if DEBUG
Vanessa620 0:02b166bd1478 375 command.printf("Resolucion definida en =%i \n", RSTEP);
Vanessa620 0:02b166bd1478 376 #endif
Vanessa620 0:02b166bd1478 377 }
Vanessa620 0:02b166bd1478 378
Vanessa620 0:02b166bd1478 379 void command_sstime(){
Vanessa620 0:02b166bd1478 380 #if DEBUG
Vanessa620 0:02b166bd1478 381 command.printf("config tiempo entre movimientos de los servos: ");
Vanessa620 0:02b166bd1478 382 command.printf("SSTIME = %i\n", buffer_command[1]);
Vanessa620 0:02b166bd1478 383 #endif
Vanessa620 0:02b166bd1478 384 put_sstime(buffer_command[1]);
Vanessa620 0:02b166bd1478 385 }
Vanessa620 0:02b166bd1478 386
Vanessa620 0:02b166bd1478 387 void stop(){
Vanessa620 0:02b166bd1478 388
Vanessa620 0:02b166bd1478 389 #if DEBUG
Vanessa620 0:02b166bd1478 390 command.printf("stop");
Vanessa620 0:02b166bd1478 391 #endif
Vanessa620 0:02b166bd1478 392 exit(0);
Vanessa620 0:02b166bd1478 393 }
Vanessa620 0:02b166bd1478 394
Vanessa620 0:02b166bd1478 395 void pausa(){
Iducanyi 1:c0ff30bf8db2 396 estado = 0;
Iducanyi 2:b0ed0b16b9c8 397 wait (2);
Vanessa620 0:02b166bd1478 398 }
Vanessa620 0:02b166bd1478 399
Vanessa620 0:02b166bd1478 400 void reanudar(){
Vanessa620 0:02b166bd1478 401
Iducanyi 1:c0ff30bf8db2 402 estado = 1;
Vanessa620 0:02b166bd1478 403 #if DEBUG
Vanessa620 0:02b166bd1478 404 command.printf("reanuda piccolo ");
Vanessa620 0:02b166bd1478 405 #endif
Iducanyi 2:b0ed0b16b9c8 406 }
Vanessa620 0:02b166bd1478 407
Vanessa620 0:02b166bd1478 408 //*********Interrupcion*******//
Vanessa620 0:02b166bd1478 409
Vanessa620 0:02b166bd1478 410 void interrup(){
Iducanyi 1:c0ff30bf8db2 411 //if(estado == 1){
Vanessa620 0:02b166bd1478 412 val=command.getc();
Vanessa620 0:02b166bd1478 413 if (val== '<'){
Vanessa620 0:02b166bd1478 414 Read_command();
Vanessa620 0:02b166bd1478 415 if (check_command()){
Vanessa620 0:02b166bd1478 416 command_exe();
Vanessa620 0:02b166bd1478 417 #if DEBUG
Vanessa620 0:02b166bd1478 418 echo_command();
Vanessa620 0:02b166bd1478 419 #endif
Vanessa620 0:02b166bd1478 420 command.printf("%s", buffer_command);
Vanessa620 0:02b166bd1478 421
Vanessa620 0:02b166bd1478 422 }
Vanessa620 0:02b166bd1478 423 }
Vanessa620 0:02b166bd1478 424 else command.printf("error inicio trama: %d ",val);
Vanessa620 0:02b166bd1478 425 command.putc(val);
Iducanyi 1:c0ff30bf8db2 426 return;
Iducanyi 1:c0ff30bf8db2 427 //}
Iducanyi 1:c0ff30bf8db2 428
Vanessa620 0:02b166bd1478 429
Vanessa620 0:02b166bd1478 430 }
Vanessa620 0:02b166bd1478 431
Vanessa620 0:02b166bd1478 432 /////////CASOS PARA LA SELECCION DE COMANDOS/////////////
Vanessa620 0:02b166bd1478 433
Vanessa620 0:02b166bd1478 434 void command_exe()
Iducanyi 2:b0ed0b16b9c8 435 {
Iducanyi 2:b0ed0b16b9c8 436 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 437 command.printf("Ejecutando comando: \n");
Iducanyi 2:b0ed0b16b9c8 438 #endif
Vanessa620 0:02b166bd1478 439
Vanessa620 0:02b166bd1478 440 switch (buffer_command[COMM_N]){
Vanessa620 0:02b166bd1478 441
Vanessa620 0:02b166bd1478 442 case (LED_NC):
Iducanyi 2:b0ed0b16b9c8 443 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 444 command.printf("led on 7 off\n");
Iducanyi 2:b0ed0b16b9c8 445 #endif
Iducanyi 2:b0ed0b16b9c8 446 command_led(buffer_command[INITPARAMETER]);
Vanessa620 0:02b166bd1478 447 break;
Vanessa620 0:02b166bd1478 448
Vanessa620 0:02b166bd1478 449 case (DOT_NC):
Iducanyi 2:b0ed0b16b9c8 450 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 451 command.printf("Punto \n");
Iducanyi 2:b0ed0b16b9c8 452 #endif
Iducanyi 2:b0ed0b16b9c8 453 punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]);
Vanessa620 0:02b166bd1478 454 break;
Vanessa620 0:02b166bd1478 455
Vanessa620 0:02b166bd1478 456 case (LINE_NC):
Iducanyi 2:b0ed0b16b9c8 457 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 458 command.printf("draw line\n");
Iducanyi 2:b0ed0b16b9c8 459 #endif
Iducanyi 2:b0ed0b16b9c8 460 linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
Vanessa620 0:02b166bd1478 461 break;
Vanessa620 0:02b166bd1478 462
Vanessa620 0:02b166bd1478 463 case (RECTANGLE_NC):
Iducanyi 2:b0ed0b16b9c8 464 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 465 command.printf("cuadrado \n");
Iducanyi 2:b0ed0b16b9c8 466 #endif
Iducanyi 2:b0ed0b16b9c8 467 rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
Vanessa620 0:02b166bd1478 468 break;
Iducanyi 2:b0ed0b16b9c8 469
Vanessa620 0:02b166bd1478 470 case (CICLE_NC):
Iducanyi 2:b0ed0b16b9c8 471 circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
Iducanyi 2:b0ed0b16b9c8 472 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 473 command.printf("Circulo\n");
Iducanyi 2:b0ed0b16b9c8 474 #endif
Vanessa620 0:02b166bd1478 475 break;
Iducanyi 2:b0ed0b16b9c8 476
Vanessa620 0:02b166bd1478 477 case (HOME_NC):
Iducanyi 2:b0ed0b16b9c8 478 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 479 command.printf("Yendo a Home \n");
Iducanyi 2:b0ed0b16b9c8 480 #endif
Iducanyi 2:b0ed0b16b9c8 481 home();
Vanessa620 0:02b166bd1478 482 break;
Iducanyi 2:b0ed0b16b9c8 483
Vanessa620 0:02b166bd1478 484 case (RESOLUCION_NC):
Iducanyi 2:b0ed0b16b9c8 485 resolucion(buffer_command[INITPARAMETER]);
Iducanyi 2:b0ed0b16b9c8 486 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 487 command.printf("Tiempo pasos \n");
Iducanyi 2:b0ed0b16b9c8 488 #endif
Vanessa620 0:02b166bd1478 489 break;
Iducanyi 2:b0ed0b16b9c8 490
Vanessa620 0:02b166bd1478 491 case (TIEMPOPASOS_NC):
Iducanyi 2:b0ed0b16b9c8 492 sstime(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]);
Iducanyi 2:b0ed0b16b9c8 493 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 494 command.printf("Tiempo pasos \n");
Iducanyi 2:b0ed0b16b9c8 495 #endif
Vanessa620 0:02b166bd1478 496 break;
Iducanyi 2:b0ed0b16b9c8 497
Vanessa620 0:02b166bd1478 498 case (STOP_NC):
Iducanyi 2:b0ed0b16b9c8 499 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 500 command.printf("Stop \n");
Iducanyi 2:b0ed0b16b9c8 501 #endif
Vanessa620 0:02b166bd1478 502 break;
Vanessa620 0:02b166bd1478 503
Vanessa620 0:02b166bd1478 504 case (PAUSA_NC):
Iducanyi 2:b0ed0b16b9c8 505 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 506 command.printf("Pausa \n");
Iducanyi 2:b0ed0b16b9c8 507 #endif
Iducanyi 2:b0ed0b16b9c8 508 pausa();
Vanessa620 0:02b166bd1478 509 break;
Iducanyi 2:b0ed0b16b9c8 510
Vanessa620 0:02b166bd1478 511 case (REANUDAR_NC):
Iducanyi 2:b0ed0b16b9c8 512 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 513 command.printf("Reanudar \n");
Iducanyi 2:b0ed0b16b9c8 514 #endif
Vanessa620 0:02b166bd1478 515 break;
Vanessa620 0:02b166bd1478 516
Vanessa620 0:02b166bd1478 517 default:
Iducanyi 2:b0ed0b16b9c8 518 #if DEBUG
Iducanyi 2:b0ed0b16b9c8 519 command.printf("comando no encontrado\n");
Iducanyi 2:b0ed0b16b9c8 520 #endif
Vanessa620 0:02b166bd1478 521 }}
Vanessa620 0:02b166bd1478 522
Vanessa620 0:02b166bd1478 523 int main() {
Iducanyi 1:c0ff30bf8db2 524
Iducanyi 1:c0ff30bf8db2 525 //command.baud(9600);
Iducanyi 1:c0ff30bf8db2 526 //device.baud(9600);
Vanessa620 0:02b166bd1478 527 #if DEBUG
Vanessa620 0:02b166bd1478 528 command.printf("inicio con debug\n");
Vanessa620 0:02b166bd1478 529 #else
Vanessa620 0:02b166bd1478 530 command.printf("inicio sin debug\n");
Vanessa620 0:02b166bd1478 531 #endif
Iducanyi 1:c0ff30bf8db2 532 command.attach(&interrup,Serial::RxIrq);
Iducanyi 1:c0ff30bf8db2 533 //command.attach(&interrup);
Iducanyi 2:b0ed0b16b9c8 534 while(1) {
Iducanyi 2:b0ed0b16b9c8 535 if(command.readable()) {
Iducanyi 2:b0ed0b16b9c8 536 command.putc(command.getc());
Iducanyi 2:b0ed0b16b9c8 537 command.attach(&interrup,Serial::RxIrq);
Iducanyi 2:b0ed0b16b9c8 538 }
Iducanyi 2:b0ed0b16b9c8 539 /*if(uart.readable()) {
Iducanyi 2:b0ed0b16b9c8 540 pc.putc(uart.getc());
Iducanyi 2:b0ed0b16b9c8 541 command.attach(&interrup,Serial::RxIrq);
Iducanyi 2:b0ed0b16b9c8 542 }*/
Iducanyi 2:b0ed0b16b9c8 543 }
Vanessa620 0:02b166bd1478 544
Vanessa620 0:02b166bd1478 545 }