Servo.cpp

00001 #include "Servo.h"
00002 
00003 Servo::Servo(PinName port) : PwmOut(port)
00004 {
00005     period = 20000;
00006     maximum = 2400;
00007     minimum = 600;
00008     center = 1500;
00009     position = 0;
00010         //set default values
00011     pulsewidth_us(0);
00012     period_us(period);
00013 }
00014 
00015 void Servo::setposition(int input) 
00016 {   
00017     position = input;
00018     if(position > 255)
00019     {
00020         position = 255;
00021     }
00022     else if (position < -255)
00023     {
00024         position = -255;
00025     }
00026         //saturate
00027     if(position >= 0)
00028     {
00029         pulsewidth_us(position * (maximum - center) / 255 + center);
00030     }
00031     else 
00032     {
00033         pulsewidth_us(position * (center - minimum) / 255 + center);
00034     }
00035         //set position normalized to 255
00036 }
00037 
00038 void Servo::setperiod(int input)
00039 {
00040     period = input;
00041     period_us(period);
00042 }
00043 
00044 void Servo::setcenter(int input)
00045 {
00046     center = input;
00047 }
00048 
00049 void Servo::setmaximum(int input) 
00050 {
00051     maximum = input;
00052 }
00053 
00054 void Servo::setminimum(int input)
00055 {
00056     minimum = input;
00057 }
00058 
00059 Servo& Servo::operator =(int assignment)
00060 {
00061     Servo::setposition(assignment);
00062         //shorthand for setposition
00063     *this = assignment;
00064     return *this;
00065         //allow multiple assignment
00066 }
00067 
00068 Servo::operator int()
00069 {
00070     int measurement = read() * period;
00071     if(measurement >= center)
00072     {
00073         return 255 * (measurement - center) / (maximum - center);
00074     }
00075     else 
00076     {
00077         return 255 * (measurement - position) / (center - minimum);
00078     }
00079 }