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"

class TankDrive {

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

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