Library is coded to control servo This library is part of my project which is built on FRDM-KL25Z and motor shield l293D v1
ServoMotorControl.cpp@0:22a971e060ed, 2017-09-24 (annotated)
- Committer:
- vtqNhi
- Date:
- Sun Sep 24 18:41:24 2017 +0000
- Revision:
- 0:22a971e060ed
This library is created to control servo motor by send pulsewidth to pwm pin.;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vtqNhi | 0:22a971e060ed | 1 | #include "mbed.h" |
vtqNhi | 0:22a971e060ed | 2 | #include "ServoMotorControl.h" |
vtqNhi | 0:22a971e060ed | 3 | |
vtqNhi | 0:22a971e060ed | 4 | static float clamp(float value, float min, float max) { |
vtqNhi | 0:22a971e060ed | 5 | if(value < min) { |
vtqNhi | 0:22a971e060ed | 6 | return min; |
vtqNhi | 0:22a971e060ed | 7 | } else if(value > max) { |
vtqNhi | 0:22a971e060ed | 8 | return max; |
vtqNhi | 0:22a971e060ed | 9 | } else { |
vtqNhi | 0:22a971e060ed | 10 | return value; |
vtqNhi | 0:22a971e060ed | 11 | } |
vtqNhi | 0:22a971e060ed | 12 | } |
vtqNhi | 0:22a971e060ed | 13 | |
vtqNhi | 0:22a971e060ed | 14 | |
vtqNhi | 0:22a971e060ed | 15 | Servo::Servo(PinName pwm) : _pwm(pwm) |
vtqNhi | 0:22a971e060ed | 16 | { |
vtqNhi | 0:22a971e060ed | 17 | _pwm = 0; |
vtqNhi | 0:22a971e060ed | 18 | } |
vtqNhi | 0:22a971e060ed | 19 | |
vtqNhi | 0:22a971e060ed | 20 | void Servo::turn2Angle(int degree) |
vtqNhi | 0:22a971e060ed | 21 | { |
vtqNhi | 0:22a971e060ed | 22 | float angle2value; |
vtqNhi | 0:22a971e060ed | 23 | switch (degree){ |
vtqNhi | 0:22a971e060ed | 24 | case 0: angle2value = 1; break; |
vtqNhi | 0:22a971e060ed | 25 | case 15: angle2value = 1.25; break; |
vtqNhi | 0:22a971e060ed | 26 | case 30: angle2value = 1.5; break; |
vtqNhi | 0:22a971e060ed | 27 | case 45: angle2value = 1.75; break; |
vtqNhi | 0:22a971e060ed | 28 | case 60: angle2value = 2; break; |
vtqNhi | 0:22a971e060ed | 29 | case 75: angle2value = 2.25; break; |
vtqNhi | 0:22a971e060ed | 30 | case 90: angle2value = 2.5; break; |
vtqNhi | 0:22a971e060ed | 31 | case 105: angle2value = 2.75; break; |
vtqNhi | 0:22a971e060ed | 32 | case 120: angle2value = 3; break; |
vtqNhi | 0:22a971e060ed | 33 | case 135: angle2value = 3.25; break; |
vtqNhi | 0:22a971e060ed | 34 | case 150: angle2value = 3.5; break; |
vtqNhi | 0:22a971e060ed | 35 | case 165: angle2value = 3.75; break; |
vtqNhi | 0:22a971e060ed | 36 | case 180: angle2value = 4; break; |
vtqNhi | 0:22a971e060ed | 37 | default: angle2value = 1; break;} |
vtqNhi | 0:22a971e060ed | 38 | turn2pulse(angle2value); |
vtqNhi | 0:22a971e060ed | 39 | |
vtqNhi | 0:22a971e060ed | 40 | } |
vtqNhi | 0:22a971e060ed | 41 | |
vtqNhi | 0:22a971e060ed | 42 | void Servo::turn2pulse(float value) |
vtqNhi | 0:22a971e060ed | 43 | { |
vtqNhi | 0:22a971e060ed | 44 | //0 1 |
vtqNhi | 0:22a971e060ed | 45 | //30 1.5 |
vtqNhi | 0:22a971e060ed | 46 | //60 2 |
vtqNhi | 0:22a971e060ed | 47 | //90 2.5 |
vtqNhi | 0:22a971e060ed | 48 | //120 3 |
vtqNhi | 0:22a971e060ed | 49 | //150 3.5 |
vtqNhi | 0:22a971e060ed | 50 | //180 4 |
vtqNhi | 0:22a971e060ed | 51 | if (value < 1) |
vtqNhi | 0:22a971e060ed | 52 | { |
vtqNhi | 0:22a971e060ed | 53 | value = 1; |
vtqNhi | 0:22a971e060ed | 54 | } |
vtqNhi | 0:22a971e060ed | 55 | else if (value > 4) |
vtqNhi | 0:22a971e060ed | 56 | { |
vtqNhi | 0:22a971e060ed | 57 | value = 4; |
vtqNhi | 0:22a971e060ed | 58 | } |
vtqNhi | 0:22a971e060ed | 59 | value = value / 1500; |
vtqNhi | 0:22a971e060ed | 60 | _pwm.pulsewidth(value); |
vtqNhi | 0:22a971e060ed | 61 | } |
vtqNhi | 0:22a971e060ed | 62 | float Servo::positionRead() |
vtqNhi | 0:22a971e060ed | 63 | { |
vtqNhi | 0:22a971e060ed | 64 | return _position; |
vtqNhi | 0:22a971e060ed | 65 | } |