#include "avr.h"
#include "adc.h"
#include "util.h"

void setMotorSpeed(MOTOR *motor, int speed) {
  if(speed > 0) {
    *motor->port &= ~motor->revMask;
    *motor->port |= motor->fwdMask;
  }
  else if(speed < 0) {
    *motor->port &= ~motor->fwdMask;
    *motor->port |= motor->revMask;
  }
  else *motor->port |= motor->fwdMask|motor->revMask;
  
  *motor->pwmPort = CLAMP(0, ABS(speed), 255);
}

int getButton() {
  uint16_t val = getADCValue(7);
  
  if(val < 144) return 5;
  if(val < 228) return 4;
  if(val < 304) return 3;
  if(val < 376) return 2;
  if(val < 600) return 1;
  
  return 0;
}

void waitForButton(int i) {
  while(getButton() != CLAMP(0, i, 5));
  while(getButton() != 0);
}