pwm speed servo
Servo.cpp@0:80c1c369a6f4, 2019-02-15 (annotated)
- Committer:
- kenken0721
- Date:
- Fri Feb 15 08:12:50 2019 +0000
- Revision:
- 0:80c1c369a6f4
servo speed; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenken0721 | 0:80c1c369a6f4 | 1 | #include "Servo.h" |
kenken0721 | 0:80c1c369a6f4 | 2 | #include "mbed.h" |
kenken0721 | 0:80c1c369a6f4 | 3 | |
kenken0721 | 0:80c1c369a6f4 | 4 | Servo::Servo(PinName Pin) : ServoPin(Pin) {} |
kenken0721 | 0:80c1c369a6f4 | 5 | |
kenken0721 | 0:80c1c369a6f4 | 6 | void Servo::init(int pos,float speed){ |
kenken0721 | 0:80c1c369a6f4 | 7 | update(speed); |
kenken0721 | 0:80c1c369a6f4 | 8 | currentPosition = pos; |
kenken0721 | 0:80c1c369a6f4 | 9 | newPosition = pos; |
kenken0721 | 0:80c1c369a6f4 | 10 | Speed.attach_us(this, &Servo::move, 5000); |
kenken0721 | 0:80c1c369a6f4 | 11 | } |
kenken0721 | 0:80c1c369a6f4 | 12 | |
kenken0721 | 0:80c1c369a6f4 | 13 | void Servo::update(float speed){ |
kenken0721 | 0:80c1c369a6f4 | 14 | if(speed <= 0.0){ |
kenken0721 | 0:80c1c369a6f4 | 15 | currentPosition = newPosition; |
kenken0721 | 0:80c1c369a6f4 | 16 | }else if(speed > 1.0){ |
kenken0721 | 0:80c1c369a6f4 | 17 | speed = 1.0; |
kenken0721 | 0:80c1c369a6f4 | 18 | } |
kenken0721 | 0:80c1c369a6f4 | 19 | _speed = speed * 5; |
kenken0721 | 0:80c1c369a6f4 | 20 | } |
kenken0721 | 0:80c1c369a6f4 | 21 | |
kenken0721 | 0:80c1c369a6f4 | 22 | void Servo::move(){ |
kenken0721 | 0:80c1c369a6f4 | 23 | float dev = newPosition - currentPosition; |
kenken0721 | 0:80c1c369a6f4 | 24 | if( dev > 0 ){ |
kenken0721 | 0:80c1c369a6f4 | 25 | currentPosition += _speed; |
kenken0721 | 0:80c1c369a6f4 | 26 | }else if( dev < 0 ){ |
kenken0721 | 0:80c1c369a6f4 | 27 | currentPosition -= _speed; |
kenken0721 | 0:80c1c369a6f4 | 28 | } |
kenken0721 | 0:80c1c369a6f4 | 29 | int pulse = int(currentPosition); |
kenken0721 | 0:80c1c369a6f4 | 30 | ServoPin.pulsewidth_us(pulse); |
kenken0721 | 0:80c1c369a6f4 | 31 | } |
kenken0721 | 0:80c1c369a6f4 | 32 | |
kenken0721 | 0:80c1c369a6f4 | 33 | void Servo::setPosition(int pos,float speed){ |
kenken0721 | 0:80c1c369a6f4 | 34 | newPosition = pos; |
kenken0721 | 0:80c1c369a6f4 | 35 | update(speed); |
kenken0721 | 0:80c1c369a6f4 | 36 | } |