#include "Motor.h"

#include "util.h"


Motor::Motor(volatile uint8_t *port, volatile uint8_t *pwmPort, uint8_t fwdMask, uint8_t revMask) {
  this->port = port;
  this->pwmPort = pwmPort;
  this->fwdMask = fwdMask;
  this->revMask = revMask;
  
  setSpeed(0);
}

void Motor::setSpeed(int speed) {
  this->speed = CLAMP(-255, speed, 255);
  
  if(speed > 0) {
    *port &= ~revMask;
    *port |= fwdMask;
  }
  else if(speed < 0) {
    *port &= ~fwdMask;
    *port |= revMask;
  }
  else *port |= fwdMask|revMask;
  
  *pwmPort = ABS(this->speed);
}