Cornelius Bezuidenhout / Servo

Library to control servos

Servo.cpp@2:738b08cb03ea, 2017-10-21 (annotated)

Committer:
Cornelius Bezuidenhout
Date:
Sat Oct 21 21:36:48 2017 +0200
Revision:
2:738b08cb03ea
Parent:
1:940dffb14ff6
Child:
3:37e6c7b8fd2d
Fixed file extensions (-_-)

Who changed what in which revision?

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