Library is coded to control servo This library is part of my project which is built on FRDM-KL25Z and motor shield l293D v1
Diff: ServoMotorControl.cpp
- Revision:
- 0:22a971e060ed
diff -r 000000000000 -r 22a971e060ed ServoMotorControl.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ServoMotorControl.cpp Sun Sep 24 18:41:24 2017 +0000 @@ -0,0 +1,65 @@ +#include "mbed.h" +#include "ServoMotorControl.h" + +static float clamp(float value, float min, float max) { + if(value < min) { + return min; + } else if(value > max) { + return max; + } else { + return value; + } +} + + +Servo::Servo(PinName pwm) : _pwm(pwm) +{ + _pwm = 0; +} + +void Servo::turn2Angle(int degree) +{ + float angle2value; + switch (degree){ + case 0: angle2value = 1; break; + case 15: angle2value = 1.25; break; + case 30: angle2value = 1.5; break; + case 45: angle2value = 1.75; break; + case 60: angle2value = 2; break; + case 75: angle2value = 2.25; break; + case 90: angle2value = 2.5; break; + case 105: angle2value = 2.75; break; + case 120: angle2value = 3; break; + case 135: angle2value = 3.25; break; + case 150: angle2value = 3.5; break; + case 165: angle2value = 3.75; break; + case 180: angle2value = 4; break; + default: angle2value = 1; break;} + turn2pulse(angle2value); + +} + +void Servo::turn2pulse(float value) +{ + //0 1 + //30 1.5 + //60 2 + //90 2.5 + //120 3 + //150 3.5 + //180 4 + if (value < 1) + { + value = 1; + } + else if (value > 4) + { + value = 4; + } + value = value / 1500; + _pwm.pulsewidth(value); +} +float Servo::positionRead() +{ + return _position; +} \ No newline at end of file