#include "AutoTurn.h"

AutoTurn::AutoTurn(int inDesiredAngle, float inTurnSpeed) 
{
	// Use requires() here to declare subsystem dependencies
	Requires(driveTrain);
	desiredAngle = inDesiredAngle;
	turnSpeed = inTurnSpeed;
}
// TO DO: Set dead band for turning so wheels dont spaz at the beginning 

// Called just before this Command runs the first time
void AutoTurn::Initialize() 
{
	driveTrain->GyroReset();
}

// Called repeatedly when this Command is scheduled to run
void AutoTurn::Execute() 
{
	float angle = driveTrain->GetGyroAngle();
	if(angle > 0)
		{
			driveTrain->DriveTankDrive(-turnSpeed, turnSpeed);
		}
	else if(angle < 0)
		{
			driveTrain->DriveTankDrive(turnSpeed, -turnSpeed);
		}
}

// Make this return true when this Command no longer needs to run execute()
bool AutoTurn::IsFinished() 
{
	SmartDashboard::PutNumber("Gyro Angle", driveTrain->GetGyroAngle());
	return driveTrain->GetGyroAngle()>desiredAngle || driveTrain->GetGyroAngle() < -desiredAngle;
}

// Called once after isFinished returns true
void AutoTurn::End() 
{
	
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void AutoTurn::Interrupted() 
{
	
}
