Cornelius Bezuidenhout / Servo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Servo.hpp@1:940dffb14ff6, 2017-10-21 (annotated)

Committer:
Cornelius Bezuidenhout
Date:
Sat Oct 21 21:32:00 2017 +0200
Revision:
1:940dffb14ff6
Parent:
0:6c0e56235914
Child:
2:738b08cb03ea
Initial version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Cornelius Bezuidenhout 1:940dffb14ff6 1 #include "Servo.hpp"
Cornelius Bezuidenhout 1:940dffb14ff6 2
Cornelius Bezuidenhout 1:940dffb14ff6 3 Servo::Servo(PinName pwmPin, float zeroAngle, float minAngle, float maxAngle) :
Cornelius Bezuidenhout 1:940dffb14ff6 4 _servo(pwmPin)
Cornelius Bezuidenhout 1:940dffb14ff6 5 {
Cornelius Bezuidenhout 1:940dffb14ff6 6 PW_PER_DEG = 0.0005f/45.0f;
Cornelius Bezuidenhout 1:940dffb14ff6 7 ZEROPW = 0.0014f;
Cornelius Bezuidenhout 1:940dffb14ff6 8
Cornelius Bezuidenhout 1:940dffb14ff6 9 _minAngle = minAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 10 _maxAngle = maxAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 11 _angle = zeroAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 12 _servo.period_us(5000);
Cornelius Bezuidenhout 1:940dffb14ff6 13 _servo.pulsewidth(0);
Cornelius Bezuidenhout 1:940dffb14ff6 14 SaveZero();
Cornelius Bezuidenhout 1:940dffb14ff6 15 }
Cornelius Bezuidenhout 1:940dffb14ff6 16
Cornelius Bezuidenhout 1:940dffb14ff6 17 void Servo::SetServo()
Cornelius Bezuidenhout 1:940dffb14ff6 18 {
Cornelius Bezuidenhout 1:940dffb14ff6 19 float pw = ZEROPW+_angle*PW_PER_DEG;
Cornelius Bezuidenhout 1:940dffb14ff6 20
Cornelius Bezuidenhout 1:940dffb14ff6 21 pw = ( pw < MIN_PW ? MIN_PW : pw > MAX_PW ? MAX_PW : pw);
Cornelius Bezuidenhout 1:940dffb14ff6 22
Cornelius Bezuidenhout 1:940dffb14ff6 23 _servo.pulsewidth(pw);
Cornelius Bezuidenhout 1:940dffb14ff6 24 }
Cornelius Bezuidenhout 1:940dffb14ff6 25
Cornelius Bezuidenhout 1:940dffb14ff6 26 void Servo::SaveZero()
Cornelius Bezuidenhout 1:940dffb14ff6 27 {
Cornelius Bezuidenhout 1:940dffb14ff6 28 ZEROPW = (ZEROPW+_angle*PW_PER_DEG);
Cornelius Bezuidenhout 1:940dffb14ff6 29 }
Cornelius Bezuidenhout 1:940dffb14ff6 30
Cornelius Bezuidenhout 1:940dffb14ff6 31 void Servo::Calibrate(float actualAngle)
Cornelius Bezuidenhout 1:940dffb14ff6 32 {
Cornelius Bezuidenhout 1:940dffb14ff6 33 PW_PER_DEG = (_angle*PW_PER_DEG)/actualAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 34 }
Cornelius Bezuidenhout 1:940dffb14ff6 35
Cornelius Bezuidenhout 1:940dffb14ff6 36 void Servo::Set(float angle)
Cornelius Bezuidenhout 1:940dffb14ff6 37 {
Cornelius Bezuidenhout 1:940dffb14ff6 38 if( _angle < _minAngle)
Cornelius Bezuidenhout 1:940dffb14ff6 39 _angle = _minAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 40 else if( _angle > _maxAngle )
Cornelius Bezuidenhout 1:940dffb14ff6 41 _angle = _maxAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 42 else
Cornelius Bezuidenhout 1:940dffb14ff6 43 _angle = angle;
Cornelius Bezuidenhout 1:940dffb14ff6 44
Cornelius Bezuidenhout 1:940dffb14ff6 45 SetServo();
Cornelius Bezuidenhout 1:940dffb14ff6 46 }
Cornelius Bezuidenhout 1:940dffb14ff6 47
Cornelius Bezuidenhout 1:940dffb14ff6 48 void Servo::Move(float relAngle)
Cornelius Bezuidenhout 1:940dffb14ff6 49 {
Cornelius Bezuidenhout 1:940dffb14ff6 50 float newAngle = _angle + relAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 51
Cornelius Bezuidenhout 1:940dffb14ff6 52 if( newAngle < _minAngle )
Cornelius Bezuidenhout 1:940dffb14ff6 53 _angle = _minAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 54 else if( newAngle > _maxAngle )
Cornelius Bezuidenhout 1:940dffb14ff6 55 _angle = _maxAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 56 else
Cornelius Bezuidenhout 1:940dffb14ff6 57 _angle = newAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 58
Cornelius Bezuidenhout 1:940dffb14ff6 59 SetServo();
Cornelius Bezuidenhout 1:940dffb14ff6 60 }
Cornelius Bezuidenhout 1:940dffb14ff6 61
Cornelius Bezuidenhout 1:940dffb14ff6 62 void Servo::SetZero(float angle)
Cornelius Bezuidenhout 1:940dffb14ff6 63 {
Cornelius Bezuidenhout 1:940dffb14ff6 64 ZEROPW = (ZEROPW+angle*PW_PER_DEG);
Cornelius Bezuidenhout 1:940dffb14ff6 65 }
Cornelius Bezuidenhout 1:940dffb14ff6 66
Cornelius Bezuidenhout 1:940dffb14ff6 67 void Servo::Off()
Cornelius Bezuidenhout 1:940dffb14ff6 68 {
Cornelius Bezuidenhout 1:940dffb14ff6 69 _servo.pulsewidth(0);
Cornelius Bezuidenhout 1:940dffb14ff6 70 }
Cornelius Bezuidenhout 1:940dffb14ff6 71
Cornelius Bezuidenhout 1:940dffb14ff6 72 float Servo::GetAngle() {
Cornelius Bezuidenhout 1:940dffb14ff6 73 return _angle;
Cornelius Bezuidenhout 1:940dffb14ff6 74 }
Cornelius Bezuidenhout 1:940dffb14ff6 75
Cornelius Bezuidenhout 1:940dffb14ff6 76 void Servo::GotoMin() {
Cornelius Bezuidenhout 1:940dffb14ff6 77 _angle = _minAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 78 SetServo();
Cornelius Bezuidenhout 1:940dffb14ff6 79 }
Cornelius Bezuidenhout 1:940dffb14ff6 80
Cornelius Bezuidenhout 1:940dffb14ff6 81 void Servo::GotoMax() {
Cornelius Bezuidenhout 1:940dffb14ff6 82 _angle = _maxAngle;
Cornelius Bezuidenhout 1:940dffb14ff6 83 SetServo();
Cornelius Bezuidenhout 1:940dffb14ff6 84 }