Daniel Vizcaya
/
04_RTOS_Embebidos
Entrega 3er corte - sistemas embebidos
draw.cpp
- Committer:
- Bethory
- Date:
- 2018-05-30
- Revision:
- 1:fcdb45ee95b9
- Parent:
- 0:6ad07c9019fd
File content as of revision 1:fcdb45ee95b9:
#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); }