Daniel Vizcaya
/
04_RTOS_Embebidos
Entrega 3er corte - sistemas embebidos
Diff: draw.cpp
- Revision:
- 0:6ad07c9019fd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/draw.cpp Wed May 30 00:01:50 2018 +0000 @@ -0,0 +1,80 @@ + +#include "draw.h" +#include "mbed.h" +#include "math.h" + + + +PwmOut myServoX(D3); +PwmOut myServoY(D4); +PwmOut myServoZ(D5); + +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(coord*30+500);// u6 + return 500; +} + +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(coord2us(MAXPOS)); +wait_ms(ss_time*4); +} + +void nodraw(){ +myServoZ.pulsewidth_us(coord2us(POSDRAW)); +wait_ms(ss_time*4); +} + + +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