Daniel Vizcaya / Mbed OS 04_RTOS_Embebidos

Entrega 3er corte - sistemas embebidos

draw.cpp@1:fcdb45ee95b9, 2018-05-30 (annotated)

Committer:
Bethory
Date:
Wed May 30 04:46:28 2018 +0000
Revision:
1:fcdb45ee95b9
Parent:
0:6ad07c9019fd 
Entrega Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bethory 0:6ad07c9019fd 1
Bethory 0:6ad07c9019fd 2 #include "draw.h"
Bethory 0:6ad07c9019fd 3 #include "mbed.h"
Bethory 0:6ad07c9019fd 4 #include "math.h"
Bethory 0:6ad07c9019fd 5
Bethory 0:6ad07c9019fd 6
Bethory 0:6ad07c9019fd 7
Bethory 0:6ad07c9019fd 8 PwmOut myServoX(D3);
Bethory 0:6ad07c9019fd 9 PwmOut myServoY(D4);
Bethory 0:6ad07c9019fd 10 PwmOut myServoZ(D5);
Bethory 0:6ad07c9019fd 11
Bethory 0:6ad07c9019fd 12 uint8_t posx_old=0; // posición anterior del eje X
Bethory 0:6ad07c9019fd 13 uint8_t posy_old=0; // posición anterior del eje Y
Bethory 0:6ad07c9019fd 14 uint8_t ss_time=100; // tiempo de espera para moverse 1 mm en microsegundos
Bethory 0:6ad07c9019fd 15
Bethory 0:6ad07c9019fd 16 void put_sstime(uint8_t vtime){
Bethory 0:6ad07c9019fd 17 ss_time=vtime;
Bethory 0:6ad07c9019fd 18
Bethory 0:6ad07c9019fd 19 }
Bethory 0:6ad07c9019fd 20
Bethory 0:6ad07c9019fd 21 int coord2us(float coord)
Bethory 0:6ad07c9019fd 22 {
Bethory 0:6ad07c9019fd 23 if(0 <= coord <= MAXPOS)
Bethory 0:6ad07c9019fd 24 return int(coord*30+500);// u6
Bethory 0:6ad07c9019fd 25 return 500;
Bethory 0:6ad07c9019fd 26 }
Bethory 0:6ad07c9019fd 27
Bethory 0:6ad07c9019fd 28 void sstime(uint8_t x, uint8_t y)
Bethory 0:6ad07c9019fd 29 {
Bethory 0:6ad07c9019fd 30 double dx=abs(x-posx_old);
Bethory 0:6ad07c9019fd 31 double dy=abs(y-posy_old);
Bethory 0:6ad07c9019fd 32 double dist= sqrt(dx*dx+dy*dy);
Bethory 0:6ad07c9019fd 33 wait_ms((int)(ss_time*dist));
Bethory 0:6ad07c9019fd 34 posx_old =x;
Bethory 0:6ad07c9019fd 35 posy_old=y;
Bethory 0:6ad07c9019fd 36
Bethory 0:6ad07c9019fd 37 }
Bethory 0:6ad07c9019fd 38
Bethory 0:6ad07c9019fd 39
Bethory 0:6ad07c9019fd 40 void draw(){
Bethory 0:6ad07c9019fd 41 myServoZ.pulsewidth_us(coord2us(MAXPOS));
Bethory 0:6ad07c9019fd 42 wait_ms(ss_time*4);
Bethory 0:6ad07c9019fd 43 }
Bethory 0:6ad07c9019fd 44
Bethory 0:6ad07c9019fd 45 void nodraw(){
Bethory 0:6ad07c9019fd 46 myServoZ.pulsewidth_us(coord2us(POSDRAW));
Bethory 0:6ad07c9019fd 47 wait_ms(ss_time*4);
Bethory 0:6ad07c9019fd 48 }
Bethory 0:6ad07c9019fd 49
Bethory 0:6ad07c9019fd 50
Bethory 0:6ad07c9019fd 51 void vertex2d(uint8_t x, uint8_t y){
Bethory 0:6ad07c9019fd 52
Bethory 0:6ad07c9019fd 53 int pulseX = coord2us(x);
Bethory 0:6ad07c9019fd 54 int pulseY = coord2us(y);
Bethory 0:6ad07c9019fd 55
Bethory 0:6ad07c9019fd 56 myServoX.pulsewidth_us(pulseX);
Bethory 0:6ad07c9019fd 57 myServoY.pulsewidth_us(pulseY);
Bethory 0:6ad07c9019fd 58
Bethory 0:6ad07c9019fd 59 sstime(x,y);
Bethory 0:6ad07c9019fd 60
Bethory 0:6ad07c9019fd 61 }
Bethory 0:6ad07c9019fd 62
Bethory 0:6ad07c9019fd 63 void initdraw(float x, float y){
Bethory 0:6ad07c9019fd 64 vertex2d (x,y);
Bethory 0:6ad07c9019fd 65 draw();
Bethory 0:6ad07c9019fd 66 }
Bethory 0:6ad07c9019fd 67
Bethory 0:6ad07c9019fd 68 void home(){
Bethory 0:6ad07c9019fd 69 nodraw();
Bethory 0:6ad07c9019fd 70 vertex2d(0 ,0);
Bethory 0:6ad07c9019fd 71 }
Bethory 0:6ad07c9019fd 72
Bethory 0:6ad07c9019fd 73
Bethory 0:6ad07c9019fd 74
Bethory 0:6ad07c9019fd 75 void init_servo()
Bethory 0:6ad07c9019fd 76 {
Bethory 0:6ad07c9019fd 77 myServoX.period_ms(20);
Bethory 0:6ad07c9019fd 78 myServoY.period_ms(20);
Bethory 0:6ad07c9019fd 79 myServoZ.period_ms(20);
Bethory 0:6ad07c9019fd 80 }