DESPLAZAMIENTO PICCOLO

Dependencies:   mbed

Fork of 01-04EntregaPrimerCorte by ferney alberto beltran molina

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers draw.cpp Source File

draw.cpp

00001 
00002 #include "draw.h"
00003 #include "mbed.h"
00004 #include "math.h"
00005 
00006 
00007 PwmOut myServoZ(PB_3);
00008 PwmOut myServoX(PB_4);
00009 PwmOut myServoY(PB_5);
00010 
00011 uint8_t  posx_old=0;     // posición anterior del eje X
00012 uint8_t  posy_old=0;     // posición anterior del eje Y
00013 uint8_t  ss_time=50;     // tiempo  de espera para moverse 1 mm en microsegundos
00014 
00015 void put_sstime(uint8_t vtime){
00016     ss_time=vtime;
00017     
00018 }
00019 
00020 int coord2us(float coord)
00021 {
00022     if(0 <= coord <= MAXPOS)
00023         return int(750+coord*1900/50);// u6
00024     return 750;
00025 
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 void draw(){
00040 myServoZ.pulsewidth_us(coord2us(POSDRAW));
00041 wait_ms(ss_time*10);
00042 }
00043 
00044 void nodraw(){
00045 myServoZ.pulsewidth_us(coord2us(POSNODRAW));
00046 wait_ms(ss_time*10);
00047 }
00048 
00049 
00050 void vertex2d(uint8_t x, uint8_t y){
00051 
00052     int pulseX = coord2us(x);
00053     int pulseY = coord2us(y);
00054     
00055     myServoX.pulsewidth_us(pulseX);
00056     myServoY.pulsewidth_us(pulseY);
00057    
00058     sstime(x,y); 
00059 
00060 }
00061 
00062 void initdraw(float x, float y){
00063     vertex2d (x,y);
00064     draw();
00065 }
00066 
00067 void home(){
00068     nodraw();
00069     vertex2d(0 ,0);
00070 }    
00071 
00072 
00073 
00074 void init_servo()
00075 {
00076    myServoX.period_ms(20);
00077    myServoY.period_ms(20);
00078    myServoZ.period_ms(20);
00079 
00080     
00081 }