Servo.cpp

00001 #include "Servo.h"
00002 #include "mbed.h"
00003 
00004 Servo::Servo(PinName Pin) : ServoPin(Pin) {}
00005 
00006 int Servo:: SpeedConvert (int _speed)
00007 {
00008 
00009     if (_speed > 50)
00010         _speed = 50;
00011     else if (_speed < 1)
00012         _speed =1;
00013     speed = 10000/_speed;
00014     return speed;
00015 
00016 }
00017 
00018 void Servo::SetPosition(int Pos)             //servo go to position
00019 {
00020     NewPosition = Pos;
00021 }
00022 
00023 void Servo::Update()
00024 {
00025     if(NewPosition != CurrentPosition) {
00026 
00027         if(NewPosition > CurrentPosition) {
00028             CurrentPosition++;
00029             Position =  CurrentPosition;
00030 
00031         } else  {
00032             CurrentPosition--;
00033             Position = CurrentPosition;
00034         }
00035     }
00036     Position = CurrentPosition;
00037 }
00038 
00039 
00040 void Servo::StartPulse()
00041 {
00042     ServoPin = 1;
00043 
00044     PulseStop.attach_us(this, &Servo::EndPulse, Position);
00045 }
00046 
00047 void Servo::EndPulse()
00048 {
00049     ServoPin = 0;
00050 }
00051 
00052 void Servo::Enable(int StartPos, int RefreshRate, int _speed)
00053 {
00054     SpeedConvert(_speed);
00055     Period = (1000000/RefreshRate);                            //converts from HZ to period in  us
00056     Position = StartPos;
00057     NewPosition = StartPos;
00058     CurrentPosition = StartPos;                              //sets CurrentPosition to  StartPos value
00059     Pulse.attach_us(this, &Servo::StartPulse, Period);       //starts servo period Ticker r
00060     Speed.attach_us(this, &Servo::Update, speed);            //starts servo Speed Ticker  speed of servo update pulse width steps in  us,    slow =100000 100ms per step
00061 }
00062 
00063 void Servo::Disable()           //turns off Ticker Pulse
00064 {
00065     Pulse.detach();
00066 }