Servo.cpp

00001 /* mbed R/C Servo Library
00002  *  
00003  * Copyright (c) 2007-2010 sford, cstyles
00004  *
00005  * Permission is hereby granted, free of charge, to any person obtaining a copy
00006  * of this software and associated documentation files (the "Software"), to deal
00007  * in the Software without restriction, including without limitation the rights
00008  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00009  * copies of the Software, and to permit persons to whom the Software is
00010  * furnished to do so, subject to the following conditions:
00011  *
00012  * The above copyright notice and this permission notice shall be included in
00013  * all copies or substantial portions of the Software.
00014  *
00015  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00016  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00017  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00018  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00019  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00021  * THE SOFTWARE.
00022  */
00023  
00024 #include "Servo.h"
00025 #include "mbed.h"
00026 
00027 static float clamp(float value, float min, float max) {
00028     if(value < min) {
00029         return min;
00030     } else if(value > max) {
00031         return max;
00032     } else {
00033         return value;
00034     }
00035 }
00036 
00037 Servo::Servo(PinName pin) : _pwm(pin) {
00038     calibrate();
00039     write(0.5);
00040 }
00041 
00042 void Servo::write(float percent) {
00043     float offset = _range * 2.0 * (percent - 0.5);
00044     _pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
00045     _p = clamp(percent, 0.0, 1.0);
00046     
00047 }
00048 
00049 void Servo::position(float degrees) {
00050    // float offset = _range * (degrees / _degrees);
00051     if(degrees>0){
00052     
00053         kat= 0.00145+ fabs(degrees)*0.00001111;
00054         _pwm.pulsewidth(kat);
00055    
00056    }
00057    
00058    else if (degrees==0){
00059         kat=0.00145;
00060         _pwm.pulsewidth(kat);
00061      }
00062      
00063      else {
00064          kat= 0.00145- fabs(degrees)*0.00001111;
00065          _pwm.pulsewidth(kat);
00066          }
00067         
00068     //_pwm.pulsewidth(0.00245);
00069     //k=(0.0015 + clamp(offset, -_range, 0.002 +_range));
00070     
00071 }
00072 
00073 void Servo::calibrate(float range, float degrees) {
00074     _range = range;
00075     _degrees = degrees;
00076 }
00077 
00078 float Servo::read() {
00079     return _p;
00080 }
00081 
00082 Servo& Servo::operator= (float percent) { 
00083     write(percent);
00084     return *this;
00085 }
00086 
00087 Servo& Servo::operator= (Servo& rhs) {
00088     write(rhs.read());
00089     return *this;
00090 }
00091 
00092 Servo::operator float() {
00093     return read();
00094 }