DriveTrain.cpp

#include "DriveTrain.h"
#include "DriverInput.h"
#include "LineManager.h"
#include "WPILib.h"
#include "DebugUtil.h"
#include "RobotConfiguration.h"
#include "PIDtester.h"
#include "Autonomous.h"

int DriveTrain::DRIVE_AUTONOMOUS = 0;
int DriveTrain::DRIVE_TELEOP = 1;

void DriveTrain::Init()
{
	m_left_jag1 = Hardware::GetLeftMotorJag();
	m_right_jag1 = Hardware::GetRightMotorJag();
	m_left_jag2 = Hardware::GetLeftMotorJag2();
	m_right_jag2 = Hardware::GetRightMotorJag2();
	DriveMode = 0;
	/*
	m_drive = new RobotDrive(m_left_jag1, m_right_jag1,
			m_left_jag2, m_right_jag2);
	
	m_drive->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
	m_drive->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);
	m_drive->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
	m_drive->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
	
	m_drive->SetExpiration(0.1);
	
	m_drive->SetMaxOutput(150);
	*/
	printf("\tInitializing drive train:\n");
	DebugMotor(m_left_jag1);
	DebugMotor(m_left_jag2);
	DebugMotor(m_right_jag1);
	DebugMotor(m_right_jag2);
	printf("\tDone.\n");
}

void DriveTrain::DebugMotor(CANJaguar *motor)
{
	printf("\t\tMotor:\n");
	printf("\t\t\tP = %2.2f\n", motor->GetP());
	printf("\t\t\tI = %2.2f\n", motor->GetI());
	printf("\t\t\tD = %2.2f\n", motor->GetD());
	
}

void DriveTrain::Process()
{
	float output_left, output_right;
	//float mult = RobotConfiguration::GetDriveMaxOutput();
	float mult = 1;
	output_left = output_right = 0;
	if (DriverInput::GetLineFollowButton() && DriveMode == DriveTrain::DRIVE_TELEOP)
	{
		
		output_left = mult * LineManager::GetLeftSpeed();
		output_right = mult * LineManager::GetRightSpeed(); 
		
	}
	else if(PIDtester::GetPIDtester()) 
	{
		output_left = mult * PIDtester::GetLeftSpeed();
		output_right = mult * PIDtester::GetRightSpeed();		
	}
	else if(DriveMode == DriveTrain::DRIVE_AUTONOMOUS){
		output_left = mult * Autonomous::GetLeftSpeed();
		output_right = mult * Autonomous::GetRightSpeed();
	}
	else{
		output_left = mult * DriverInput::GetLeftSpeed();
		output_right = mult * DriverInput::GetRightSpeed();
	}
	
	m_left_jag1->Set(-output_left);
	m_left_jag2->Set(-output_left);
	
	m_right_jag1->Set(output_right);
	m_right_jag2->Set(output_right);
	
	//DPRINTF("Patrick, you're not crazy! %d\n", ::rand());
	
	/*
	DPRINTF("GetLineFollowButton = %d\n", DriverInput::GetLineFollowButton());
	
	DPRINTF("Speed: %2.2f\tPosition: %2.2f\n", m_left_jag1->GetSpeed(), m_left_jag1->GetPosition());
	*/	
}

void DriveTrain::SetDriveMode(int mode)
{
	DriveMode = mode;
}