/*
 * 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"

class TankDrive {

public:
	TankDrive(CANTalon *l1, CANTalon *l2, CANTalon* r1, CANTalon *r2)
	{
		dt1 = RobotDrive(l1, r1);
		dt2 = RobotDrive(l2, r2);
	}
	virtual ~TankDrive() { delete l1; delete l2; delete r1; delete r2; }

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

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

		float speed = y * th;
		float rot = y;
		dt1.ArcadeDrive(speed, rot, false);
		dt2.ArcadeDrive(speed, rot, false);
	}

private:
	RobotDrive dt1, dt2;
};

#endif /* SRC_TANKDRIVE_H_ */