#include "DriveStraight.h"
#include "../../Subsystems/DriveTrain.h"

DriveStraight::DriveStraight(double driveDistance, float driveSpeed) 
{
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	Requires(driveTrain);
	m_driveDistance = driveDistance;
	m_driveSpeed = driveSpeed;
}

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

// Called repeatedly when this Command is scheduled to run
void DriveStraight::Execute() 
{
	float angle = driveTrain->GetGyroAngle();
	driveTrain->DriveArcadeAuto(m_driveSpeed, angle * kP);
}

// Make this return true when this Command no longer needs to run execute()
bool DriveStraight::IsFinished() 
{
	printf("encoder: %lf\n", driveTrain->GetDistance());
	return driveTrain->GetDistance() > m_driveDistance;
}

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

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