Servo.cpp

00001 #include "Servo.h"
00002 
00003 Servo::Servo(PinName sgnl) : SignalLine(sgnl), Period(0.02),
00004     UpperRange(0.045), Center(0.075), LowerRange(-0.045), Position(0)
00005 {
00006     SignalLine.period(0.02);
00007         //set period to most common value;
00008     SignalLine.write(0.075);
00009         //set position to most common center
00010 }
00011 
00012 void Servo::position(float ratio) 
00013 {
00014     Position = ratio;
00015     if (ratio > 1)
00016     {
00017         ratio = 1;
00018     }
00019     else if (ratio < -1)
00020     {
00021         ratio = -1;
00022     }
00023         //saturate position to the range boundaries
00024     if (ratio >= 0)
00025     {
00026         SignalLine.write((ratio * UpperRange) + Center);
00027     }
00028     else 
00029     {
00030         SignalLine.write((ratio * LowerRange) + Center);
00031     }
00032         //set normalized position (-1 to 1)
00033 }
00034 
00035 float Servo::position()
00036 {
00037     float ratio = SignalLine.read() - Center;
00038     if (ratio > UpperRange)
00039     {
00040         ratio = UpperRange;
00041     }
00042     else if (ratio < LowerRange)
00043     {
00044         ratio = LowerRange;
00045     }
00046         //saturate position to the range boundaries
00047     if(ratio >= 0)
00048     {
00049         return ratio / UpperRange;
00050     }
00051     else 
00052     {
00053         return ratio / LowerRange;
00054     }
00055         //return normalized position
00056 }
00057 
00058 void Servo::maximum(float seconds) 
00059 {
00060     UpperRange = (seconds / Period) - Center;
00061     position(Position);
00062 }
00063 
00064 void Servo::maximum_ms(int milliseconds) 
00065 {
00066     UpperRange = ((milliseconds / 1000) / Period) - Center;
00067     position(Position);
00068 }
00069 
00070 void Servo::maximum_us(int microseconds) 
00071 {
00072     UpperRange = ((microseconds / 1000000) / Period) - Center;
00073     position(Position);
00074 }
00075 
00076 void Servo::center(float seconds)
00077 {
00078     Center = seconds / Period;
00079     position(Position);
00080 }
00081 
00082 void Servo::center_ms(int milliseconds)
00083 {
00084     Center = (milliseconds / 1000) / Period;
00085     position(Position);
00086 }
00087 
00088 void Servo::center_us(int microseconds)
00089 {
00090     Center = (microseconds / 1000000) / Period;
00091     position(Position);
00092 }
00093 
00094 void Servo::minimum(float seconds)
00095 {
00096     LowerRange = (seconds / Period) - Center;
00097     position(Position);
00098 }
00099 
00100 void Servo::minimum_ms(int milliseconds)
00101 {
00102     LowerRange = ((milliseconds / 1000) / Period) - Center;
00103     position(Position);
00104 }
00105 
00106 void Servo::minimum_us(int microseconds)
00107 {
00108     LowerRange = ((microseconds / 1000000) / Period) - Center;
00109     position(Position);
00110 }
00111 
00112 void Servo::period(float seconds)
00113 {
00114     Period = seconds;
00115     SignalLine.period(seconds);
00116 }
00117 
00118 void Servo::period_ms(int milliseconds)
00119 {
00120     Period = milliseconds / 1000;
00121     SignalLine.period_ms(milliseconds);
00122 }
00123 
00124 void Servo::period_us(int microseconds)
00125 {
00126     Period = microseconds / 1000000;
00127     SignalLine.period_us(microseconds);
00128 }
00129 
00130 void Servo::pulsewidth(float seconds)
00131 {
00132     Position = (seconds / Period) - Center;
00133     if (Position >=0)
00134     {
00135         Position /= UpperRange;
00136     }
00137     else
00138     {
00139         Position /= LowerRange * -1;
00140     }
00141     SignalLine.pulsewidth(seconds);
00142 }
00143 
00144 void Servo::pulsewidth_ms(int milliseconds)
00145 {
00146     Position = ((milliseconds / 1000) / Period) - Center;
00147     if (Position >=0)
00148     {
00149         Position /= UpperRange;
00150     }
00151     else
00152     {
00153         Position /= LowerRange * -1;
00154     }
00155     SignalLine.pulsewidth_ms(milliseconds);
00156 }
00157 
00158 void Servo::pulsewidth_us(int microseconds)
00159 {
00160     Position = ((microseconds / 1000000) / Period) - Center;
00161     if (Position >=0)
00162     {
00163         Position /= UpperRange;
00164     }
00165     else
00166     {
00167         Position /= LowerRange * -1;
00168     }
00169     SignalLine.pulsewidth_us(microseconds);
00170 }
00171 
00172 float Servo::operator =(float assignment)
00173 {
00174     position(assignment);
00175     return assignment;
00176 }
00177 
00178 Servo::operator float()
00179 {
00180     return position();
00181 }