Piccolo / Mbed 2 deprecated Piccolo_10

codigo

Dependencies:   mbed

main.cpp@0:02b166bd1478, 2017-11-18 (annotated)

Committer:
Vanessa620
Date:
Sat Nov 18 14:53:56 2017 +0000
Revision:
0:02b166bd1478
Child:
1:c0ff30bf8db2
codigo

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