Cornelius Bezuidenhout / Servo

Library to control servos

Servo.cpp@4:95b334f2306c, 2017-10-21 (annotated)

Committer:
Cornelius Bezuidenhout
Date:
Sat Oct 21 21:58:49 2017 +0200
Revision:
4:95b334f2306c
Parent:
3:37e6c7b8fd2d 
Added GetMin() and GetMax() functions

Who changed what in which revision?

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