#include "CheckDrive.h"
#include <cmath>
#include "Commands/CommandGroup.h"
#include "../../DentRobot.h"
#include "../../RobotMap.h"
CheckDrive::CheckDrive(int motorID) : CommandGroup("CheckDrive"){
  Requires(DentRobot::drivetrain);
  motor = motorID;
}
void CheckDrive::Initialize(){
  SetTimeout(1.0);
}
void CheckDrive::Execute(){
  switch(motor){
    case DRIVE_FRONT_LEFT_CAN:
      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTLEFT,1);
      break;
    case DRIVE_FRONT_RIGHT_CAN:
      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->FRONTRIGHT,1);
      break;
    case DRIVE_BACK_LEFT_CAN:
      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKLEFT,1);
      break;
    case DRIVE_BACK_RIGHT_CAN:
      DentRobot::drivetrain->TestMotor(DentRobot::drivetrain->BACKRIGHT,1);
      break;
    default:
      break;
  }
}
bool CheckDrive::IsFinished(){
  return false;
}
void CheckDrive::End(){
}
void CheckDrive::Interrupted(){
  End();
}