Juan Camilo Peña
/
ProyectoC1
Funciones: Ejecutar Guadar Vertex2D Draw No-draw Stop End
Diff: Dibujar.cpp
- Revision:
- 2:834c4394fd95
- Parent:
- 0:744135d2e43b
diff -r 1e86d6321238 -r 834c4394fd95 Dibujar.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Dibujar.cpp Wed Mar 14 00:43:30 2018 +0000 @@ -0,0 +1,79 @@ +#include "Dibujar.h" +#include "mbed.h" +#include "math.h" + +PwmOut myServoX(PB_3); +PwmOut myServoY(PB_4); +PwmOut myServoZ(PB_5); + +uint8_t posx_old=0; // posición anterior del eje X +uint8_t posy_old=0; // posición anterior del eje Y +uint8_t ss_time=100; // tiempo de espera para moverse 1 mm en microsegundos + +void put_sstime(uint8_t vtime){ + ss_time=vtime; + +} + +int coord2us(float coord) +{ + if(0 <= coord <= MAXPOS) + return int(750+coord*1900/50);// u6 + return 750; + +} + +void sstime(uint8_t x, uint8_t y) +{ + double dx=abs(x-posx_old); + double dy=abs(y-posy_old); + double dist= sqrt(dx*dx+dy*dy); + wait_ms((int)(ss_time*dist)); + posx_old =x; + posy_old=y; + + } + +void draw(){ +myServoZ.pulsewidth_us(POSDRAW); +wait_ms(ss_time*2); +} + +void nodraw(){ +myServoZ.pulsewidth_us(MAXPOS); +wait_ms(ss_time*2); +} + + +void vertex2d(uint8_t x, uint8_t y){ + + int pulseX = coord2us(x); + int pulseY = coord2us(y); + + myServoX.pulsewidth_us(pulseX); + myServoY.pulsewidth_us(pulseY); + + sstime(x,y); + +} + +void initdraw(float x, float y){ + vertex2d (x,y); + draw(); +} + +void home(){ + nodraw(); + vertex2d(0 ,0); +} + + + +void init_servo() +{ + myServoX.period_ms(20); + myServoY.period_ms(20); + myServoZ.period_ms(20); + + +} \ No newline at end of file