FRC2016-old/DriveBase/src/TankDrive.h

/*
 * TankDrive.h
 *
 *  Created on: Jan 28, 2016
 *      Author: Jason
 */

#ifndef SRC_TANKDRIVE_H_
#define SRC_TANKDRIVE_H_

#ifndef DEADZONE_RADIUS
#define DEADZONE_RADIUS 0.05
#endif // DEADZONE_RADIUS

#include "WPILib.h"
#include <math.h>

class TankDrive {

public:
	TankDrive(CANTalon *left[], CANTalon *right[])
	{
		Left = new CANTalon[] {left[0], left[1]};
		Right = new CANTalon[] {right[0], right[1]};
	}
	virtual ~TankDrive() { delete Left[0]; delete Left[1]; delete Right[0]; delete Right[1]; }

	void Drive(Joystick *js)
	{
		float x = js->GetX(),
				y = js->GetY(),
				th = js->GetThrottle();

		// set deadzone
		if(x > -DEADZONE_RADIUS && x < DEADZONE_RADIUS)
		{
			x = 0f;
		}
		if(y > -DEADZONE_RADIUS && y < DEADZONE_RADIUS)
		{
			y = 0f;
		}

		float left=0f, right=0f;

		if(x == 0)
		{
			left = th * y;
			right = left;
		}
		else if(x < 0)
		{
			right = th;
			left = th * cos(x);
		}
		else
		{
			right = th * cos(x);
			left = th;
		}

		Left[0]->Set(left);
		Left[1]->Set(left);
		Right[0]->Set(right);
		Right[1]->Set(right);
	}

private:

	CANTalon* Left[];
	CANTalon* Right[];
};

#endif /* SRC_TANKDRIVE_H_ */