66 lines
1.4 KiB
C++
66 lines
1.4 KiB
C++
/*
|
|
* 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"
|
|
|
|
/**
|
|
* Encapsulates two RobotDrive objects and keeps them synced by sending
|
|
* identical ArcadeDrive calls to each. Also handles massaging of Joystick
|
|
* data to smooth out drive operations.
|
|
*/
|
|
class TankDrive {
|
|
|
|
public:
|
|
TankDrive(CANTalon *l1, CANTalon *l2, CANTalon* r1, CANTalon *r2) {
|
|
dt1 = new RobotDrive(l1, r1);
|
|
dt2 = new RobotDrive(l2, r2);
|
|
}
|
|
virtual ~TankDrive() {
|
|
delete dt1;
|
|
delete dt2;
|
|
}
|
|
|
|
/**
|
|
* Calls ArcadeDrive on the two RobotDrive objects for the TankDrive.
|
|
* Uses #defined DEADZONE_RADIUS to keep the robot from freaking out.
|
|
* Some math on the "rot" variable could make the driving smoother, I think.
|
|
*/
|
|
void Drive(Joystick *js) {
|
|
float x = js->GetX();
|
|
float y = js->GetY();
|
|
float 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;
|
|
// TODO: do some math here to smooth out turning?
|
|
float rot = x * th;
|
|
|
|
dt1->ArcadeDrive(speed, rot, false);
|
|
dt2->ArcadeDrive(speed, rot, false);
|
|
}
|
|
|
|
private:
|
|
RobotDrive *dt1, *dt2;
|
|
};
|
|
|
|
#endif /* SRC_TANKDRIVE_H_ */
|