/*
Motor.cpp
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
*/

#if defined(ARDUINO) && ARDUINO >= 100
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include "Motor.h"

void Motor::mode(byte mode){
  digitalWrite(Motor::_pin1, (mode>>0)&1);
  digitalWrite(Motor::_pin2, (mode>>1)&1);
}

void Motor::mode(byte mode, byte speed){
  switch(mode){
  case GO:
    analogWrite(Motor::_pin1, speed);
    digitalWrite(Motor::_pin2, LOW);
    break;
  case BACK:
    analogWrite(Motor::_pin1, ~speed);
    digitalWrite(Motor::_pin2, HIGH);
    break;
  default:
    digitalWrite(Motor::_pin1, (mode>>0)&1);
    digitalWrite(Motor::_pin2, (mode>>1)&1);
    break;
  }
}

void Motor::speed(int speed){
  if(speed<0) Motor::mode(BACK, -speed);
  else Motor::mode(GO, speed);
}

void Motor::attach(byte pin1, byte pin2){
  Motor::_pin1 = pin1;
  Motor::_pin2 = pin2;
  pinMode(Motor::_pin1, OUTPUT);
  pinMode(Motor::_pin2, OUTPUT);
  digitalWrite(Motor::_pin1, LOW);
  digitalWrite(Motor::_pin2, LOW);
}