Complete library that let you control and read your servo's in all ways: position, degrees, step.You can define also min and max position. It is also compatibile with Simon Ford's Servo library calls
Fork of DuryServo by
Diff: Servo.cpp
- Revision:
- 0:88fd1c2402c9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.cpp Thu Jun 13 17:54:36 2013 +0000 @@ -0,0 +1,161 @@ +/* mbed R/C Servo Library + * + * Copyright (c) 2013 Fabio Durigon + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "Servo.h" +#include "mbed.h" + +Servo::Servo(PinName pin) : pwmout(pin) +{ + // Default values for common servos + DefinePulseTotal(0.001,90); +} + +float Servo::Write(float Pos) +{ + // Normalize range + if (Pos < 0) Pos=0; + else if (Pos > 1) Pos=1; + + currpos=Pos; + currdegrees=(currpos*degreesrange)-degreeshrange; + float pulsewidth=(currpos*pulserange)+pulsemin; + pwmout.pulsewidth(pulsewidth); + + return(pulsewidth); +} + +float Servo::write(float Pos) +{ + return(Write(Pos)); +} + +float Servo::WriteDegrees(float Degrees) +{ + // Normalize Degrees + if (Degrees < -degreeshrange) Degrees=-degreeshrange; + else if (Degrees > degreeshrange) Degrees=degreeshrange; + + currdegrees=Degrees; + currpos=(currdegrees+degreeshrange)/degreesrange; + float pulsewidth=(currpos*pulserange)+pulsemin; + pwmout.pulsewidth(pulsewidth); + + return(currdegrees); +} + +void Servo::DefinePulseMin(float PulseSec, float Degrees) +{ + pulsemin=PulseSec; + pulserange=pulsemax-pulsemin; + degreesmin=Degrees; + degreesrange=degreesmax-degreesmin; + degreeshrange=degreesrange/2; +} + +void Servo::DefinePulseMax(float PulseSec, float Degrees) +{ + pulsemax=PulseSec; + pulserange=pulsemax-pulsemin; + degreesmax=Degrees; + degreesrange=degreesmax-degreesmin; + degreeshrange=degreesrange/2; +} + +void Servo::DefinePulseTotal(float TotPulseSec, float TotDegrees) +{ + pulserange=TotPulseSec; + // set min and max taking 1.5 ms as default center pulsewidth + pulsemin=0.0015-(TotPulseSec/2); + pulsemax=0.0015+(TotPulseSec/2); + + degreesrange=TotDegrees; + degreeshrange=TotDegrees/2; + // set min and max taking 0° as center degree + degreesmin=-degreeshrange; + degreesmax=degreeshrange; +} + +float Servo::Read(void) +{ + return(currpos); +} + +float Servo::read(void) +{ + return(currpos); +} + +unsigned char Servo::ReadByte(void) +{ + return(currpos*255); // convert value <0.0 - 1.0> to <0 - 255> +} + +float Servo::ReadDegrees(void) +{ + return(currdegrees); +} + +float Servo::ReadPulseMin(void) +{ + return(pulsemin); +} + +float Servo::ReadPulseMax(void) +{ + return(pulsemax); +} + +float Servo::ReadDegreesMin(void) +{ + return(degreesmin); +} + +float Servo::ReadDegreesMax(void) +{ + return(degreesmax); +} + +Servo& Servo::operator= (float Range) { + Write(Range); + return *this; +} + +Servo& Servo::operator= (Servo& sobj) { + Write(sobj.Read()); + return *this; +} + +Servo::operator float() { + return Read(); +} + +/* +// Debug Prints +void Servo::PrintValues(void) +{ + printf("SERVOLIB: currpos = %f, currdegrees = %f\r\n", currpos, currdegrees); + printf("SERVOLIB: pulsemin = %f, degreesmin = %f\r\n", pulsemin, degreesmin); + printf("SERVOLIB: pulsemax = %f degreesmax = %f\r\n", pulsemax, degreesmax); + printf("SERVOLIB: pulserange = %f degreesrange = %f degreeshrange = %f\r\n", pulserange, degreesrange, degreeshrange); +} +*/ \ No newline at end of file