/*
  Michael Simmons
  Master Project: gRAPI: an indoor four-wheeled robot API for a gumstix connex
  
  Last Modified: 17 Feb 2009
  
  Motor class
*/

#include "Motor.h"

// Constructor
Motor::Motor(int handle, int low, int high, int pwm, int delay) : I2cPWM(handle, low, high, pwm, delay)
{
  currentPer = 0;
  direction = stopped;
} // I2cPwm

void Motor::stop()
{
  currentPer = 0;
  direction = stopped;
  I2cPWM::middle();
} // stop

void Motor::forward(int percent)
{
  I2cPWM::increase(percent);
  currentPer = percent;
  direction = forwd;
} // forward

void Motor::reverse(int percent)
{
  I2cPWM::decrease(percent);
  currentPer = percent;
  direction = backwd;
} // reverse

int Motor::increase(int percent)
{
  int flag;

  currentPer += percent;
  if(currentPer > 100)
  {
    currentPer = 100;
    flag = -1;
  }
  else flag = 0;

  switch(direction)
  {
    case forwd:
      forward(currentPer);
      break;
    case stopped:
      forward(currentPer);
      break;
    case backwd:
      reverse(currentPer);
      break;
  }
  return flag;
} // increase

int Motor::decrease(int percent)
{
  currentPer -= percent;
  if(currentPer <= 0)
  {
    currentPer = 0;
    direction = stopped;
    stop();
    return -1;
  }

  switch(direction)
  {
    case forwd:
      forward(currentPer);
      break;
    case stopped:
      reverse(currentPer);
      break;
    case backwd:
      reverse(currentPer);
      break;
  }
  return 0;
} // decrease