draw.cpp

00001 
00002 #include "draw.h"
00003 #include "mbed.h"
00004 #include "math.h"
00005 
00006 
00007 
00008 PwmOut myServoX(D3);
00009 PwmOut myServoY(D4);
00010 PwmOut myServoZ(D5);
00011 
00012 uint8_t  posx_old=0;     // posición anterior del eje X
00013 uint8_t  posy_old=0;     // posición anterior del eje Y
00014 uint8_t  ss_time=100;     // tiempo  de espera para moverse 1 mm en microsegundos
00015 
00016 void put_sstime(uint8_t vtime){
00017     ss_time=vtime;
00018     
00019 }
00020 
00021 int coord2us(float coord)
00022 {
00023     if(0 <= coord <= MAXPOS)
00024         return int(coord*30+500);// u6
00025     return 500;
00026 }
00027 
00028 void sstime(uint8_t x, uint8_t y)
00029 {
00030     double dx=abs(x-posx_old);
00031     double dy=abs(y-posy_old);
00032     double dist= sqrt(dx*dx+dy*dy);
00033     wait_ms((int)(ss_time*dist));
00034     posx_old =x;
00035     posy_old=y;
00036     
00037  }
00038  
00039  
00040 void draw(){
00041 myServoZ.pulsewidth_us(coord2us(MAXPOS));
00042 wait_ms(ss_time*4);
00043 }
00044 
00045 void nodraw(){
00046 myServoZ.pulsewidth_us(coord2us(POSDRAW));
00047 wait_ms(ss_time*4);
00048 }
00049 
00050 
00051 void vertex2d(uint8_t x, uint8_t y){
00052 
00053     int pulseX = coord2us(x);
00054     int pulseY = coord2us(y);
00055     
00056     myServoX.pulsewidth_us(pulseX);
00057     myServoY.pulsewidth_us(pulseY);
00058    
00059     sstime(x,y); 
00060 
00061 }
00062 
00063 void initdraw(float x, float y){
00064     vertex2d (x,y);
00065     draw();
00066 }
00067 
00068 void home(){
00069     nodraw();
00070     vertex2d(0 ,0);
00071 }    
00072 
00073 
00074 
00075 void init_servo()
00076 {
00077    myServoX.period_ms(20);
00078    myServoY.period_ms(20);
00079    myServoZ.period_ms(20);  
00080 }