David Garrsion / VarSpeedServo

Made changes to individually control servo speeds

Dependents:   VariableSpeedServo

Fork of Servo by Jasper Denkers

Servo.cpp@2:466f005a4dd5, 2015-10-11 (annotated)

Committer:
sierrasmith71
Date:
Sun Oct 11 20:50:48 2015 +0000
Revision:
2:466f005a4dd5
Parent:
0:30b972d2dcec
Child:
3:774dd54867f2
rev1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdenkers 0:30b972d2dcec 1 #include "Servo.h"
jdenkers 0:30b972d2dcec 2 #include "mbed.h"
jdenkers 0:30b972d2dcec 3
jdenkers 0:30b972d2dcec 4 Servo::Servo(PinName Pin) : ServoPin(Pin) {}
sierrasmith71 2:466f005a4dd5 5
sierrasmith71 2:466f005a4dd5 6 int Servo:: SpeedConvert (int _speed) {
sierrasmith71 2:466f005a4dd5 7
sierrasmith71 2:466f005a4dd5 8 if (_speed > 50)
sierrasmith71 2:466f005a4dd5 9 _speed = 50;
sierrasmith71 2:466f005a4dd5 10 else if (_speed < 1)
sierrasmith71 2:466f005a4dd5 11 _speed =1;
sierrasmith71 2:466f005a4dd5 12 speed = 10000/_speed;
sierrasmith71 2:466f005a4dd5 13 return speed;
sierrasmith71 2:466f005a4dd5 14
sierrasmith71 2:466f005a4dd5 15 }
jdenkers 0:30b972d2dcec 16
sierrasmith71 2:466f005a4dd5 17 void Servo::SetPosition(int Pos) { //go to position
sierrasmith71 2:466f005a4dd5 18 NewPosition = Pos;
jdenkers 0:30b972d2dcec 19 }
sierrasmith71 2:466f005a4dd5 20
sierrasmith71 2:466f005a4dd5 21 void Servo::Update()
sierrasmith71 2:466f005a4dd5 22 {
sierrasmith71 2:466f005a4dd5 23
sierrasmith71 2:466f005a4dd5 24
sierrasmith71 2:466f005a4dd5 25 if(NewPosition != CurrentPosition) {
sierrasmith71 2:466f005a4dd5 26
sierrasmith71 2:466f005a4dd5 27 if(NewPosition > CurrentPosition) {
sierrasmith71 2:466f005a4dd5 28 CurrentPosition++;
sierrasmith71 2:466f005a4dd5 29 Position = CurrentPosition;
sierrasmith71 2:466f005a4dd5 30
sierrasmith71 2:466f005a4dd5 31 }
sierrasmith71 2:466f005a4dd5 32 else {
sierrasmith71 2:466f005a4dd5 33 CurrentPosition--;
sierrasmith71 2:466f005a4dd5 34 Position = CurrentPosition;
sierrasmith71 2:466f005a4dd5 35 }
sierrasmith71 2:466f005a4dd5 36 }
sierrasmith71 2:466f005a4dd5 37 Position = CurrentPosition;
sierrasmith71 2:466f005a4dd5 38
sierrasmith71 2:466f005a4dd5 39 }
sierrasmith71 2:466f005a4dd5 40
jdenkers 0:30b972d2dcec 41
jdenkers 0:30b972d2dcec 42 void Servo::StartPulse() {
jdenkers 0:30b972d2dcec 43 ServoPin = 1;
sierrasmith71 2:466f005a4dd5 44
jdenkers 0:30b972d2dcec 45 PulseStop.attach_us(this, &Servo::EndPulse, Position);
jdenkers 0:30b972d2dcec 46 }
jdenkers 0:30b972d2dcec 47
jdenkers 0:30b972d2dcec 48 void Servo::EndPulse() {
jdenkers 0:30b972d2dcec 49 ServoPin = 0;
jdenkers 0:30b972d2dcec 50 }
jdenkers 0:30b972d2dcec 51
sierrasmith71 2:466f005a4dd5 52 void Servo::Enable(int StartPos, int RefreshRate, int _speed) {
sierrasmith71 2:466f005a4dd5 53 SpeedConvert(_speed);
sierrasmith71 2:466f005a4dd5 54 Period = (1000000/RefreshRate); //converts from HZ to period in us
jdenkers 0:30b972d2dcec 55 Position = StartPos;
sierrasmith71 2:466f005a4dd5 56 CurrentPosition = StartPos; //sets CurrentPosition to StartPos value
sierrasmith71 2:466f005a4dd5 57 Pulse.attach_us(this, &Servo::StartPulse, Period); //starts servo period Ticker r
sierrasmith71 2:466f005a4dd5 58 Speed.attach_us(this, &Servo::Update, speed); //starts servo Speed Ticker speed of servo update pulse width steps in us, slow =100000 100ms per step
jdenkers 0:30b972d2dcec 59 }
jdenkers 0:30b972d2dcec 60
sierrasmith71 2:466f005a4dd5 61 void Servo::Disable() { //turns off Ticker Pulse
jdenkers 0:30b972d2dcec 62 Pulse.detach();
jdenkers 0:30b972d2dcec 63 }