DriveTrain.cpp

#include "DriveTrain.h"
#include "Config.h"

using namespace std;

DriveTrain::DriveTrain(int fl, int fr, int rl, int rr)
{
	FLMotor = new CANJaguar(fl);
	FRMotor = new CANJaguar(fr);
	RLMotor = new CANJaguar(rl);
	RRMotor = new CANJaguar(rr);
	Odometer = new Encoder(9,10,false,Encoder::k4X);
	
	ResetCANJaguars();
	
	Odometer->Start();
	double WheelCircumference = Config::GetSetting("drive_WheelCircumference",18.85);
	double GearRatio = Config:: GetSetting("drive_GearRatio",30.2934);
	int EncoderLines = (int)Config::GetSetting("drive_encoderlines",360);
	double DistancePerPulse = WheelCircumference / EncoderLines / GearRatio ;
	Odometer->SetDistancePerPulse(DistancePerPulse);
	cout << "DistancePerPulse =" << DistancePerPulse << endl;
	
	Sol1 = new DoubleSolenoid(1,2);
}

void DriveTrain::ResetCANJaguars()
{
	InitCANJags(FLMotor);
	InitCANJags(FRMotor);
	InitCANJags(RLMotor);
	InitCANJags(RRMotor);
}

void DriveTrain::InitCANJags(CANJaguar* motor)
{
		for(int x=0;x<5;x++)
		{
			if(motor->GetControlMode() != CANJaguar::kSpeed) //Checks whether the motor is in speed control mode
			{
				motor->ChangeControlMode(CANJaguar::kSpeed); //Changes to speed control
				motor->SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);	//Tells the program that a quad encoder is being used
				motor->ConfigEncoderCodesPerRev((int)Config::GetSetting("drive_encoderlines",360)); //Sets 360 as the lines per encoder revolution
				motor->SetPID(DRIVE_KP,DRIVE_KI,DRIVE_KD); //Sets the proportional, integral, and derivative values for closed loop speed
				motor->ConfigNeutralMode(CANJaguar::kNeutralMode_Coast); 
				motor->EnableControl(0.0);//Starts controlling output based on feedback
			} else  { 
				break; 
			}
		
		}
}

double DriveTrain::GetDist()
{
	return Odometer->GetDistance() * -1;
}

void DriveTrain::ResetOdometer()
{
	Odometer->Reset();
}

void DriveTrain::StopOdometer()
{
	Odometer->Stop();
}

void DriveTrain::LogHeaders(ofstream &fout)
{	//Creates the headers for the logging file
	char * motor_names[4] = { "Front Left", "Front Right", "Rear Left", "Rear Right" };
	
	for (int i = 0; i < 4; i++) {
		char * name = motor_names[i];
		fout << name << " RPM Target,";
		fout << name << " RPM Actual,";
		fout << name << " Voltage,";
		fout << name << " Current,";
		fout << name << " Faults,";
		fout << name << " Reset,";
		fout << name << " Mode,";
	}

	fout << "Odometer,";
	fout << "Tank/Mecanum*,";
}
void DriveTrain::LogMotors(ofstream &fout)
{	//Stores different outputs from the motors in the logging file
	CANJaguar * motors[4] = { FLMotor, FRMotor, RLMotor, RRMotor };
	
	for (int i = 0; i < 4; i++) 
	{
		CANJaguar * motor = motors[i];
		fout << motor->Get() << ",";
		fout << motor->GetSpeed() << ",";
		fout << motor->GetOutputVoltage() << ",";
		fout << motor->GetOutputCurrent() << ",";
		fout << motor->GetFaults() << ",";
		fout << motor->GetPowerCycled() << ",";
		fout << motor->GetControlMode() << ",";
	}
	
	fout << GetDist() << ",";
	fout << Tank << ",";
	
}

void DriveTrain::Drive(double LeftXAxis, double LeftYAxis, double RightXAxis, double RightYAxis, double Gyro, bool FieldOrientated)
{	//Basic driving function for joystick based inputs
	if(Tank == true)
	{
		this->DeadZone(LeftXAxis,LeftYAxis,RightXAxis,RightYAxis); //Checks whether Joysticks are in a range near 0
		this->TankDrive(LeftYAxis,RightYAxis);
	}
	
	else if (Tank == false)
	{
		this->DeadZone(LeftXAxis,LeftYAxis,RightXAxis,RightYAxis); //Checks whether Joysticks are in a range near 0
		if(FieldOrientated)
		{
			this->MecanumDrive(LeftXAxis,LeftYAxis,RightXAxis,Gyro);	
		}
		else
		{
			this->MecanumDrive(LeftXAxis,LeftYAxis,RightXAxis,0.0);
		}
	}
}

void DriveTrain::TankDrive(double left, double right)
{
	FRMotor->Set(-1 * (right * MAX_SPEED));
	RRMotor->Set(-1 * (right * MAX_SPEED));
	FLMotor->Set(     (left * MAX_SPEED));
	RLMotor->Set(     (left * MAX_SPEED));
}

void DriveTrain::SwitchDrive(bool tank,bool mech)
{ //Function that switches from tank drive to mecanum drive and vice versa
	if(tank)
	{
		Sol1->Set(DoubleSolenoid::kReverse);
		Tank = true;
	}
	else if(mech)
	{
		Sol1->Set(DoubleSolenoid::kForward);
		Tank = false;
	}
	else
	{
		Sol1->Set(DoubleSolenoid::kOff);
	}
}

void DriveTrain::Rotate(double r)
{ //Rotates at r speed
	if(Tank == false)
		this->MecanumDrive(0,0,-r,0);
	if(Tank == true)
		this->TankDrive(r,-r);
}

void DriveTrain::DeadZone(double &LeftXAxis,double &LeftYAxis,double &RightXAxis,double &RightYAxis)
{	//Function to see whether joystick values are in a range near 0
	if((LeftXAxis < 0.05) && (LeftXAxis > -0.05))
		LeftXAxis = 0;
	if((LeftYAxis < 0.05) && (LeftYAxis > -0.05))
		LeftYAxis = 0;
	if((RightXAxis < 0.05) && (RightXAxis > -0.05))
		RightXAxis = 0;
	if((RightYAxis < 0.05) && (RightYAxis > -0.05))
		RightYAxis = 0;
}

void DriveTrain::MecanumDrive(double x, double y, double r, double gyro)
{
	double xIn = x;		//reversed to make the electronics the front of the robot
	double yIn = y;
	double rotation = r;

	yIn = -yIn;

	this->RotateVector(xIn, yIn, gyro);

	double wheelSpeeds[4];
	wheelSpeeds[0] = -1 * ( xIn + yIn + rotation);
	wheelSpeeds[1] =       -xIn + yIn - rotation;
	wheelSpeeds[2] = -1 * (-xIn + yIn + rotation);
	wheelSpeeds[3] =        xIn + yIn - rotation;

	this->Normalize(wheelSpeeds);

	FLMotor->Set(wheelSpeeds[0] * MAX_SPEED);
	FRMotor->Set(wheelSpeeds[1] * MAX_SPEED);
	RLMotor->Set(wheelSpeeds[2] * MAX_SPEED);
	RRMotor->Set(wheelSpeeds[3] * MAX_SPEED);
}

bool DriveTrain::GetMechanum()
{
	return !Tank;
}

void DriveTrain::RotateVector(double &x, double &y, double angle)
{
	double cosA = cos(angle * (3.14159 / 180.0));
	double sinA = sin(angle * (3.14159 / 180.0));
	double xOut = x * cosA - y * sinA;
	double yOut = x * sinA + y * cosA;
	x = xOut;
	y = yOut;
}

void DriveTrain::Normalize(double *wheelSpeeds)
{	//Changes the wheel speeds to be in a scale from -1 to 1
	double maxMagnitude = fabs(wheelSpeeds[0]);
			INT32 i;
			for (i=1; i<4; i++)
			{
				double temp = fabs(wheelSpeeds[i]);
				if (maxMagnitude < temp) maxMagnitude = temp;
			}
			if (maxMagnitude > 1.0)
			{
				for (i=0; i<4; i++)
				{
					wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
				}
			}	

}