[Wenry_Hill]

Sorry if this doesn't look pretty i'm new to forms. Also if i should add anything else just let me know please. Thank you for helping

TestTalonCommand.cpp file

#include "TestTalonCommand.h"

TestTalonCommand::TestTalonCommand()
	: TimedCommand("TestTalonCommand", 0.5)
{
	Requires(drivetrain);
}

void TestTalonCommand::Initialize()
{
	drivetrain->setMotorSpeed(1,0.5);
}

void TestTalonCommand::End()
{
	drivetrain->setMotorSpeed(1,0);
}

TestTalonCommand.h file

#ifndef TEST_TALON_COMMAND_H
#define TEST_TALON_COMMAND_H

#include "../Types/TimedCommand.h"

class TestTalonCommand : public TimedCommand {

public:
	TestTalonCommand();
	void Initialize();
	void End();
};

#endif

Mecanum.cpp

#include "MecanumDriveTrain.h"

#include "../RobotMap.h"
#if defined(MECANUM_DRIVE)

#include "../Custom/DriveTrain/EncoderDrive.h"
#include <CANTalon.h>
#include <Gyro.h>

#include "../Custom/Netconsole.h"

MecanumDriveTrain::MecanumDriveTrain() : DriveTrain("MecanumDriveTrain")
{
	frontRightMotor = new CANTalon(RobotMap::DriveBase::frontRightMotorID);
	frontLeftMotor = new CANTalon(RobotMap::DriveBase::frontLeftMotorID);
	backRightMotor = new CANTalon(RobotMap::DriveBase::backRightMotorID);
	backLeftMotor = new CANTalon(RobotMap::DriveBase::backLeftMotorID);


	
	gyro = new Gyro(RobotMap::DriveBase::gyroPort);
	gyro->SetSensitivity(RobotMap::DriveBase::gyroSensitivity);

	drive = new EncoderDrive(frontLeftMotor, backLeftMotor, frontRightMotor, backRightMotor);
	drive->SetSafetyEnabled(false);
	drive->ignoreEncoders();
	
	DEBUG_MOTOR(frontLeftMotor);
	DEBUG_MOTOR(frontRightMotor);
	DEBUG_MOTOR(backLeftMotor);
	DEBUG_MOTOR(backRightMotor);
}

MecanumDriveTrain::~MecanumDriveTrain()
{
	delete frontLeftMotor;
	delete frontRightMotor;
	delete backLeftMotor;
	delete backRightMotor;

	delete gyro;
}

void MecanumDriveTrain::move(float xValue, float yValue, float rotate)
{
	drive->MecanumDrive(xValue, yValue, rotate, gyro->GetAngle());
}

void MecanumDriveTrain::stop()
{
	drive->MecanumDrive(0.0, 0.0, 0.0);
}

void MecanumDriveTrain::setMotorSpeed(RobotMap::CanID motorID, float speed)
{	drive->setMotorSpeed(motorID,speed);

}


#endif

MecanumDriveTrain.h file

#ifndef MECANUM_DRIVE_TRAIN_H
#define MECANUM_DRIVE_TRAIN_H

#include "DriveTrain.h"

class CANTalon;
class Gyro;

class MecanumDriveTrain: public DriveTrain {
public:
	MecanumDriveTrain();
	virtual ~MecanumDriveTrain();
	void setMotorSpeed(RobotMap::CanID motorID, float speed);
	
	void move(float xValue, float yValue, float rotateValue);
	virtual void stop();

protected:
	CANTalon* frontLeftMotor;
	CANTalon* frontRightMotor;
	CANTalon* backLeftMotor;
	CANTalon* backRightMotor;

	Gyro* gyro;
};

#define MOVE_WITH_JOYSTICK\
	move(\
		oi->getAxisValue(logicalAxes::DriveX),\
		oi->getAxisValue(logicalAxes::DriveY),\
		oi->getAxisValue(logicalAxes::DriveRotate)\
	)

#endif

EncoderDrive.h file
#ifndef ENCODER_DRIVE_H

#define ENCODER_DRIVE_H

#include <RobotDrive.h>

#include "../../RobotMap.h"

#include <vector>

class SpeedController;
class CANTalon;
class Encoder;
class RatePIDController;
class PIDSource;

class EncoderDrive : public RobotDrive {

public:
	EncoderDrive(
		SpeedController* leftMotor,
		SpeedController* rightMotor,
		Encoder* leftEncoder,
		Encoder* rightEncoder
	);

	EncoderDrive(
		CANTalon* frontLeftMotor,
		CANTalon* backLeftMotor,
		CANTalon* frontRightMotor,
		CANTalon* backRightMotor
	);

	virtual void SetLeftRightMotorOutputs(float leftOutput, float rightOutput);
	virtual void MecanumDrive(double x, double y, double rotate, double angle=0.0);
	void setMaxSpeed(float speed);
	void setMotorSpeed(RobotMap::CanID motorID, float speed);
	void ignoreEncoders();

protected:
	float m_maxSpeed;

	bool m_brokenEncoder;

	std::vector<PIDSource*> sensors;
	std::vector<RatePIDController*> outputs;
	std::vector<SpeedController*> motors;
	std::vector<float> speeds;

	void equalizeMotors();
	void addMotor(PIDSource* sensor, SpeedController* motor);
};

#endif

EncoderDrive.cpp file

#include "EncoderDrive.h"

#include <cmath>
#include <iterator>
#include <algorithm>

#include <CANTalon.h>
#include <Encoder.h>

#include "RatePIDController.h"
#include "EncoderRatePIDSource.h"
#include "CANTalonRatePIDSource.h"

#include "../../RobotMap.h"

#include "../Netconsole.h"

EncoderDrive::EncoderDrive(
	SpeedController* leftMotor,
	SpeedController* rightMotor,
	Encoder* leftEncoder,
	Encoder* rightEncoder
): RobotDrive(leftMotor, rightMotor),
	m_maxSpeed(RobotMap::DriveBase::maxSpeed),
	m_brokenEncoder(false)
{
	addMotor(new EncoderRatePIDSource(leftEncoder), leftMotor);
	addMotor(new EncoderRatePIDSource(rightEncoder), rightMotor);

	speeds.resize(motors.size());
}

EncoderDrive::EncoderDrive(
	CANTalon* frontLeftMotor,
	CANTalon* backLeftMotor,
	CANTalon* frontRightMotor,
	CANTalon* backRightMotor
): RobotDrive(frontLeftMotor, backLeftMotor, frontRightMotor, backRightMotor),
	m_maxSpeed(RobotMap::DriveBase::maxSpeed),
	m_brokenEncoder(false)
{
	addMotor(new CANTalonRatePIDSource(frontLeftMotor), frontLeftMotor);
	addMotor(new CANTalonRatePIDSource(backLeftMotor), backLeftMotor);
	addMotor(new CANTalonRatePIDSource(frontRightMotor), frontRightMotor);
	addMotor(new CANTalonRatePIDSource(backRightMotor), backRightMotor);

	speeds.resize(motors.size());
}

void EncoderDrive::setMaxSpeed(float speed)
{
	m_maxSpeed = speed;
}

void EncoderDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput)
{
	if (m_brokenEncoder)
	{
		RobotDrive::SetLeftRightMotorOutputs(leftOutput, rightOutput);
		return;
	}

	speeds[0] = leftOutput;
	speeds[1] = -rightOutput; // Should we set this motor inverted?
	equalizeMotors();
}


void EncoderDrive::MecanumDrive(double x, double y, double rotate, double angle)
{	
	

	if (m_brokenEncoder)
	{
		Netconsole::print<double>("X", x);
		Netconsole::print<double>("Y", y);
		Netconsole::print<double>("Rotate", rotate);
		
		RobotDrive::MecanumDrive_Cartesian(x, -y, rotate, angle);
		return;
	}

	static bool reported = false;
	if (!reported)
	{
		HALReport(
			HALUsageReporting::kResourceType_RobotDrive,
			motors.size(),
			HALUsageReporting::kRobotDrive_MecanumCartesian
		);
		reported = true;
	}

	// Compensate for gyro angle.
	RotateVector(x, y, angle);

	speeds[kFrontLeftMotor] = x + y + rotate;
	speeds[kFrontRightMotor] = -x + y - rotate;
	speeds[kRearLeftMotor] = -x + y + rotate;
	speeds[kRearRightMotor] = x + y - rotate;

	// Normalize joystick values
	float maxSpeed = std::abs(speeds[kFrontLeftMotor]);
	for (int i = 1; i < kMaxNumberOfMotors; i++)
	{
		float currentSpeed = std::abs(speeds[i]);
		if (currentSpeed > maxSpeed)
		{
			maxSpeed = currentSpeed;
		}
	}
	if (maxSpeed > 1.0)
	{
		for (int i=0; i < kMaxNumberOfMotors; i++)
		{
			speeds[i] = speeds[i] / maxSpeed;
		}
	}

	equalizeMotors();
}

void EncoderDrive::addMotor(PIDSource* sensor, SpeedController* motor)
{
	sensors.push_back(sensor);
	motors.push_back(motor);

	RatePIDController* output = new RatePIDController(
		RobotMap::DriveBase::P,
		RobotMap::DriveBase::I,
		RobotMap::DriveBase::D,
		1,
		sensor,
		motor
	);
	output->SetInputRange(-m_maxSpeed, m_maxSpeed);
	output->SetPercentTolerance('1');
	output->Enable();
	outputs.push_back(output);
}


void EncoderDrive::equalizeMotors()
{
	int motorCount = motors.size();
	float maxInput = 1;
	float currentInput = 0;
	float peggedInput = 0;
	int peggedIndex = -1;

	// If one motor is pegged, don't let the others pass it
	// NOTE: Are there cases where motors could be pegged at different values?
	for (int i=0; i < motorCount; i++)
	{
		currentInput = std::abs(motors[i]->Get());
		if (currentInput > .99)
		{
			peggedInput = std::abs(sensors[i]->PIDGet()) / m_maxSpeed;
			if (peggedInput < maxInput)
			{
				maxInput = peggedInput;
				peggedIndex = i;
			}
		}
	}

	if (peggedIndex != -1)
	{
		for (int i=0; i < motorCount; i++)
		{
			if (i == peggedIndex)
			{
				continue;
			}

			if (speeds[i] >= 0)
			{
				speeds[i] = std::min(speeds[i], maxInput);
			}
			else
			{
				speeds[i] = std::max(speeds[i], -maxInput);
			}

		}
	}
	for (int i=0; i < motorCount; i++)
	{
		outputs[i]->SetSetpoint(speeds[i] * m_maxSpeed);
	}

	m_safetyHelper->Feed();
}

void EncoderDrive::ignoreEncoders()
{
	m_brokenEncoder = true;
}

void EncoderDrive::setMotorSpeed(RobotMap::CanID motorID, float speed)
{
	motors[motorID]->Set(speed);
}