/**
 *
 *  myServo.cpp
 *  Jan Dvorak z Vozerovic - dvorkaman@centrum.cz - dvorkaman.php5.cz
 */   
 
#ifndef myServo_cpp
#define myServo_cpp

#include "WProgram.h"
#include "myServo.h"
#include "Servo.h"

/***************************************************
/**
 * Constructor
 * @param int input pin 
 */
 myServo::myServo(int pin)
 {
    this->pin=pin;
    
    //init values
    getPosition(); //get angle
    running=false;   
    interval=15; //ms   
    endAngle=0;
    lastMillis=0;    
 } 
 
/***************************************************
/**
 * Servo loop 
 */
void myServo::servoLoop()
 {
    //servo interval
    long a=millis();
    if( a-lastMillis<interval )return;    
    lastMillis=a;
    
    //is moving => calculate new angle?
    if( running )
    {
        angle+=increment;
        if( angle>=endAngle && increment>0 ){angle=endAngle;running=false;}
          else if( angle<=endAngle && increment<0 ){angle=endAngle;running=false;}
    }
        
    //write
    s.write( (int)angle);
 }
 
/***************************************************
/**
 * Move to new position 
 */
void myServo::moveToPosition(int newAngle,int steps)
{
    if( steps<=0 || newAngle<=0 || newAngle>=180 )return; //invalid input
    if( running )return;
    
    running=true;
    endAngle=newAngle;
    increment=(endAngle-angle)/steps;
    
    if( increment<=0.1 && increment>=-0.1 )
    {
      if( endAngle>angle )increment=0.1; else increment=-0.1;
    }
} 

/***************************************************
/**
 * Abort 
 */
void myServo::abort()
{
   running=false;
} 
  
/***************************************************
/**
 * Attach to pin 
 */
 void myServo::attach()
 {
    s.attach(pin);    
 }
 
/***************************************************
/**
 * Detach pin
 */
 void myServo::detach()
 {
    s.detach();
 }
 
/***************************************************
/**
 * Get current position 
 */
 int myServo::getPosition()
 {
    angle=s.read();
    return angle;
 }
 
 /***************************************************
/**
 * is moving
 */
 boolean myServo::isMoving()
 {    
    return running;
 }
 
/***************************************************
/**
 * Set current position
 * @param int new position 
 */
 void myServo::setPosition(int position)
 {
   if( running )return;
   s.write(position);
 }
 
/***************************************************
/**
 * Set servo interval
 * @param int interval
 */
 void myServo::setServoInterval(int interval)
 {
    if( interval<0 )return; //invalid input
    
    this->interval=interval;
 }
 
 
#endif