Piccolo / Mbed 2 deprecated Piccolo_10

codigo

Dependencies:   mbed

main.cpp@1:c0ff30bf8db2, 2017-11-23 (annotated)

Committer:
Iducanyi
Date:
Thu Nov 23 21:51:48 2017 +0000
Revision:
1:c0ff30bf8db2
Parent:
0:02b166bd1478
Child:
2:b0ed0b16b9c8
piccolo

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){
Vanessa620 0:02b166bd1478 223 initdraw(xp,yp);
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;
Vanessa620 0:02b166bd1478 235 initdraw(xp,yp);
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){
Vanessa620 0:02b166bd1478 245 initdraw(xp,yp);
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;
Vanessa620 0:02b166bd1478 257 initdraw(xp,yp);
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);
Vanessa620 0:02b166bd1478 314 draw();
Vanessa620 0:02b166bd1478 315 wait(1);
Vanessa620 0:02b166bd1478 316 for(double i=0; i<=90 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 317 {
Vanessa620 0:02b166bd1478 318 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 319 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 320 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 321 #if DEBUG
Vanessa620 0:02b166bd1478 322 command.printf("position x =%lf ; position y =%lf \n",cos(i*3.1416/180),sin(i*3.1416/180));
Vanessa620 0:02b166bd1478 323 #endif
Vanessa620 0:02b166bd1478 324 wait_ms(10);
Vanessa620 0:02b166bd1478 325 }
Vanessa620 0:02b166bd1478 326 for(double i=90; i<=180 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 327 {
Vanessa620 0:02b166bd1478 328 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 329 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 330 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 331 #if DEBUG
Vanessa620 0:02b166bd1478 332 command.printf("position x =%li ; position y =%li \n",x,y);
Vanessa620 0:02b166bd1478 333 #endif
Vanessa620 0:02b166bd1478 334 wait_ms(10);
Vanessa620 0:02b166bd1478 335 }
Vanessa620 0:02b166bd1478 336 for(double i=180; i<=270 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 337 {
Vanessa620 0:02b166bd1478 338 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 339 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 340 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 341 #if DEBUG
Vanessa620 0:02b166bd1478 342 command.printf("position x =%li ; position y =%li\n",x,y);
Vanessa620 0:02b166bd1478 343 #endif
Vanessa620 0:02b166bd1478 344 wait_ms(10);
Vanessa620 0:02b166bd1478 345 }
Vanessa620 0:02b166bd1478 346
Vanessa620 0:02b166bd1478 347 for(double i=270; i<=360 ;i+=RSTEP)
Vanessa620 0:02b166bd1478 348 {
Vanessa620 0:02b166bd1478 349 x=radio*cos(i*3.1416/180);
Vanessa620 0:02b166bd1478 350 y=radio*sin(i*3.1416/180);
Vanessa620 0:02b166bd1478 351 vertex2d(cx+x,cy+y);
Vanessa620 0:02b166bd1478 352 #if DEBUG
Vanessa620 0:02b166bd1478 353 command.printf("position x =%li , position y =%li \n",x,y);
Vanessa620 0:02b166bd1478 354 #endif
Vanessa620 0:02b166bd1478 355 wait_ms(10);
Vanessa620 0:02b166bd1478 356 }
Vanessa620 0:02b166bd1478 357 wait(1);
Vanessa620 0:02b166bd1478 358 nodraw();
Vanessa620 0:02b166bd1478 359 }
Vanessa620 0:02b166bd1478 360
Vanessa620 0:02b166bd1478 361 void home(){
Vanessa620 0:02b166bd1478 362 vertex2d(0,0);
Vanessa620 0:02b166bd1478 363 nodraw();
Vanessa620 0:02b166bd1478 364
Vanessa620 0:02b166bd1478 365 }
Vanessa620 0:02b166bd1478 366
Vanessa620 0:02b166bd1478 367 void resolucion(int res){
Vanessa620 0:02b166bd1478 368
Vanessa620 0:02b166bd1478 369 RSTEP = res;
Vanessa620 0:02b166bd1478 370 #if DEBUG
Vanessa620 0:02b166bd1478 371 command.printf("Resolucion definida en =%i \n", RSTEP);
Vanessa620 0:02b166bd1478 372 #endif
Vanessa620 0:02b166bd1478 373 }
Vanessa620 0:02b166bd1478 374
Vanessa620 0:02b166bd1478 375 void command_sstime(){
Vanessa620 0:02b166bd1478 376 #if DEBUG
Vanessa620 0:02b166bd1478 377 command.printf("config tiempo entre movimientos de los servos: ");
Vanessa620 0:02b166bd1478 378 command.printf("SSTIME = %i\n", buffer_command[1]);
Vanessa620 0:02b166bd1478 379 #endif
Vanessa620 0:02b166bd1478 380 put_sstime(buffer_command[1]);
Vanessa620 0:02b166bd1478 381 }
Vanessa620 0:02b166bd1478 382
Vanessa620 0:02b166bd1478 383 void stop(){
Vanessa620 0:02b166bd1478 384
Vanessa620 0:02b166bd1478 385 #if DEBUG
Vanessa620 0:02b166bd1478 386 command.printf("stop");
Vanessa620 0:02b166bd1478 387 #endif
Vanessa620 0:02b166bd1478 388 exit(0);
Vanessa620 0:02b166bd1478 389 }
Vanessa620 0:02b166bd1478 390
Vanessa620 0:02b166bd1478 391 void pausa(){
Iducanyi 1:c0ff30bf8db2 392 estado = 0;
Vanessa620 0:02b166bd1478 393 }
Vanessa620 0:02b166bd1478 394
Vanessa620 0:02b166bd1478 395 void reanudar(){
Vanessa620 0:02b166bd1478 396
Iducanyi 1:c0ff30bf8db2 397 estado = 1;
Vanessa620 0:02b166bd1478 398 #if DEBUG
Vanessa620 0:02b166bd1478 399 command.printf("reanuda piccolo ");
Vanessa620 0:02b166bd1478 400 #endif
Vanessa620 0:02b166bd1478 401 }
Vanessa620 0:02b166bd1478 402 //********MOTOR PASO A PASO*********//
Vanessa620 0:02b166bd1478 403
Vanessa620 0:02b166bd1478 404 /* Paso A B C D Paso E F G H
Vanessa620 0:02b166bd1478 405 1 1 1 0 0 1 1 1 0 0
Vanessa620 0:02b166bd1478 406 2 0 1 1 0 2 0 1 1 0
Vanessa620 0:02b166bd1478 407 3 0 0 1 1 3 0 0 1 1
Vanessa620 0:02b166bd1478 408 4 1 0 0 1 4 1 0 0 1
Vanessa620 0:02b166bd1478 409 */
Vanessa620 0:02b166bd1478 410
Vanessa620 0:02b166bd1478 411
Vanessa620 0:02b166bd1478 412 //*********Interrupcion*******//
Vanessa620 0:02b166bd1478 413
Vanessa620 0:02b166bd1478 414 void interrup(){
Iducanyi 1:c0ff30bf8db2 415 //if(estado == 1){
Vanessa620 0:02b166bd1478 416 val=command.getc();
Vanessa620 0:02b166bd1478 417 if (val== '<'){
Vanessa620 0:02b166bd1478 418 Read_command();
Vanessa620 0:02b166bd1478 419 if (check_command()){
Vanessa620 0:02b166bd1478 420 command_exe();
Vanessa620 0:02b166bd1478 421 #if DEBUG
Vanessa620 0:02b166bd1478 422 echo_command();
Vanessa620 0:02b166bd1478 423 #endif
Vanessa620 0:02b166bd1478 424 command.printf("%s", buffer_command);
Vanessa620 0:02b166bd1478 425
Vanessa620 0:02b166bd1478 426 }
Vanessa620 0:02b166bd1478 427 }
Vanessa620 0:02b166bd1478 428 else command.printf("error inicio trama: %d ",val);
Vanessa620 0:02b166bd1478 429 command.putc(val);
Iducanyi 1:c0ff30bf8db2 430 return;
Iducanyi 1:c0ff30bf8db2 431 //}
Iducanyi 1:c0ff30bf8db2 432
Vanessa620 0:02b166bd1478 433
Vanessa620 0:02b166bd1478 434 }
Vanessa620 0:02b166bd1478 435
Vanessa620 0:02b166bd1478 436 /////////CASOS PARA LA SELECCION DE COMANDOS/////////////
Vanessa620 0:02b166bd1478 437
Vanessa620 0:02b166bd1478 438 void command_exe()
Vanessa620 0:02b166bd1478 439 {
Vanessa620 0:02b166bd1478 440 #if DEBUG
Vanessa620 0:02b166bd1478 441 command.printf("Ejecutando comando: \n");
Vanessa620 0:02b166bd1478 442 #endif
Vanessa620 0:02b166bd1478 443
Vanessa620 0:02b166bd1478 444 switch (buffer_command[COMM_N]){
Vanessa620 0:02b166bd1478 445
Vanessa620 0:02b166bd1478 446 case (LED_NC):
Vanessa620 0:02b166bd1478 447 #if DEBUG
Vanessa620 0:02b166bd1478 448 command.printf("led on 7 off\n");
Vanessa620 0:02b166bd1478 449 #endif
Vanessa620 0:02b166bd1478 450 command_led(buffer_command[INITPARAMETER]);
Vanessa620 0:02b166bd1478 451 break;
Vanessa620 0:02b166bd1478 452
Vanessa620 0:02b166bd1478 453 case (DOT_NC):
Vanessa620 0:02b166bd1478 454 #if DEBUG
Vanessa620 0:02b166bd1478 455 command.printf("Punto \n");
Vanessa620 0:02b166bd1478 456 #endif
Vanessa620 0:02b166bd1478 457 punto(buffer_command[INITPARAMETER], buffer_command[INITPARAMETER+1]);
Vanessa620 0:02b166bd1478 458 break;
Vanessa620 0:02b166bd1478 459
Vanessa620 0:02b166bd1478 460 case (LINE_NC):
Vanessa620 0:02b166bd1478 461 #if DEBUG
Vanessa620 0:02b166bd1478 462 command.printf("draw line\n");
Vanessa620 0:02b166bd1478 463 #endif
Vanessa620 0:02b166bd1478 464 linea(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
Vanessa620 0:02b166bd1478 465
Vanessa620 0:02b166bd1478 466
Vanessa620 0:02b166bd1478 467
Vanessa620 0:02b166bd1478 468
Vanessa620 0:02b166bd1478 469 break;
Vanessa620 0:02b166bd1478 470
Vanessa620 0:02b166bd1478 471 case (RECTANGLE_NC):
Vanessa620 0:02b166bd1478 472 #if DEBUG
Vanessa620 0:02b166bd1478 473 command.printf("cuadrado \n");
Vanessa620 0:02b166bd1478 474 #endif
Vanessa620 0:02b166bd1478 475 rectangle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2],buffer_command[INITPARAMETER+3]);
Vanessa620 0:02b166bd1478 476
Vanessa620 0:02b166bd1478 477 break;
Vanessa620 0:02b166bd1478 478 case (CICLE_NC):
Vanessa620 0:02b166bd1478 479 circle(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1],buffer_command[INITPARAMETER+2]);
Vanessa620 0:02b166bd1478 480 #if DEBUG
Vanessa620 0:02b166bd1478 481 command.printf("Circulo\n");
Vanessa620 0:02b166bd1478 482 #endif
Vanessa620 0:02b166bd1478 483 break;
Vanessa620 0:02b166bd1478 484 case (HOME_NC):
Vanessa620 0:02b166bd1478 485 #if DEBUG
Vanessa620 0:02b166bd1478 486 command.printf("Yendo a Home \n");
Vanessa620 0:02b166bd1478 487 #endif
Vanessa620 0:02b166bd1478 488 home();
Vanessa620 0:02b166bd1478 489 break;
Vanessa620 0:02b166bd1478 490 case (RESOLUCION_NC):
Vanessa620 0:02b166bd1478 491 resolucion(buffer_command[INITPARAMETER]);
Vanessa620 0:02b166bd1478 492 #if DEBUG
Vanessa620 0:02b166bd1478 493 command.printf("Tiempo pasos \n");
Vanessa620 0:02b166bd1478 494 #endif
Vanessa620 0:02b166bd1478 495 break;
Vanessa620 0:02b166bd1478 496 case (TIEMPOPASOS_NC):
Vanessa620 0:02b166bd1478 497 sstime(buffer_command[INITPARAMETER],buffer_command[INITPARAMETER+1]);
Vanessa620 0:02b166bd1478 498 #if DEBUG
Vanessa620 0:02b166bd1478 499 command.printf("Tiempo pasos \n");
Vanessa620 0:02b166bd1478 500 #endif
Vanessa620 0:02b166bd1478 501 break;
Vanessa620 0:02b166bd1478 502 case (STOP_NC):
Vanessa620 0:02b166bd1478 503 #if DEBUG
Vanessa620 0:02b166bd1478 504 command.printf("Stop \n");
Vanessa620 0:02b166bd1478 505 #endif
Vanessa620 0:02b166bd1478 506 break;
Vanessa620 0:02b166bd1478 507
Vanessa620 0:02b166bd1478 508 case (PAUSA_NC):
Vanessa620 0:02b166bd1478 509 #if DEBUG
Vanessa620 0:02b166bd1478 510 command.printf("Pausa \n");
Vanessa620 0:02b166bd1478 511 #endif
Vanessa620 0:02b166bd1478 512 pausa();
Vanessa620 0:02b166bd1478 513 break;
Vanessa620 0:02b166bd1478 514 case (REANUDAR_NC):
Vanessa620 0:02b166bd1478 515 #if DEBUG
Vanessa620 0:02b166bd1478 516 command.printf("Reanudar \n");
Vanessa620 0:02b166bd1478 517 #endif
Vanessa620 0:02b166bd1478 518 break;
Vanessa620 0:02b166bd1478 519
Vanessa620 0:02b166bd1478 520 default:
Vanessa620 0:02b166bd1478 521
Vanessa620 0:02b166bd1478 522 #if DEBUG
Vanessa620 0:02b166bd1478 523 command.printf("comando no encontrado\n");
Vanessa620 0:02b166bd1478 524 #endif
Vanessa620 0:02b166bd1478 525 }}
Vanessa620 0:02b166bd1478 526
Vanessa620 0:02b166bd1478 527 int main() {
Iducanyi 1:c0ff30bf8db2 528
Iducanyi 1:c0ff30bf8db2 529 //command.baud(9600);
Iducanyi 1:c0ff30bf8db2 530 //device.baud(9600);
Vanessa620 0:02b166bd1478 531 #if DEBUG
Vanessa620 0:02b166bd1478 532 command.printf("inicio con debug\n");
Vanessa620 0:02b166bd1478 533 #else
Vanessa620 0:02b166bd1478 534 command.printf("inicio sin debug\n");
Vanessa620 0:02b166bd1478 535 #endif
Iducanyi 1:c0ff30bf8db2 536 command.attach(&interrup,Serial::RxIrq);
Iducanyi 1:c0ff30bf8db2 537 //command.attach(&interrup);
Iducanyi 1:c0ff30bf8db2 538 while(1);
Iducanyi 1:c0ff30bf8db2 539
Vanessa620 0:02b166bd1478 540
Vanessa620 0:02b166bd1478 541 }