nicolas rojas / Mbed 2 deprecated segundo_copy

pausa

Dependencies:   mbed

Fork of primercorte by edson antonio vargas villarreal

draw.cpp@5:9187685429b3, 2018-06-06 (annotated)

Committer:
nicolasrojas
Date:
Wed Jun 06 18:31:04 2018 +0000
Revision:
5:9187685429b3
Parent:
4:244a242e0428 
pausa

Who changed what in which revision?

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