DESPLAZAMIENTO PICCOLO
Fork of 01-04EntregaPrimerCorte by
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sat Jul 23 2022 09:41:09 by
1.7.2
