draw.cpp

00001 
00002 #include "draw.h"
00003 #include "mbed.h"
00004 #include "math.h"
00005 
00006 
00007 
00008 PwmOut myServoX(PB_3);
00009 PwmOut myServoY(PB_4);
00010 PwmOut myServoZ(PB_5);
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(750+coord*1900/50);// u6
00025     return 750;
00026 
00027 }
00028 
00029 void sstime(uint8_t x, uint8_t y)
00030 {
00031     double dx=abs(x-posx_old);
00032     double dy=abs(y-posy_old);
00033     double dist= sqrt(dx*dx+dy*dy);
00034     wait_ms((int)(ss_time*dist));
00035     posx_old =x;
00036     posy_old=y;
00037     
00038  }
00039  
00040 void draw(){
00041 myServoZ.pulsewidth_us(POSDRAW);
00042 wait_ms(ss_time*2);
00043 }
00044 
00045 void nodraw(){
00046 myServoZ.pulsewidth_us(MAXPOS);
00047 wait_ms(ss_time*2);
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 
00081     
00082 }