jhonatan angel / Mbed 2 deprecated Arana

abrayan

Dependencies:   mbed

draw.cpp@0:89b318e49395, 2018-03-17 (annotated)

Committer:
fabeltranm
Date:
Sat Mar 17 00:57:56 2018 +0000
Revision:
0:89b318e49395
ok codigo

Who changed what in which revision?

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