package com.grt192.benchtest.mechanism;
import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Mechanism;
/**
*
* @author anand
*/
public class BenchDriveTrain extends Mechanism {
private GRTJaguar frontLeft;
private GRTJaguar frontRight;
private GRTJaguar backLeft;
private GRTJaguar backRight;
private boolean debug;
public BenchDriveTrain(int fl, int fr, int bl, int br, boolean debug) {
frontLeft = new GRTJaguar(fl);
frontLeft.start();
frontRight = new GRTJaguar(fr);
frontRight.start();
backLeft = new GRTJaguar(bl);
backLeft.start();
backRight = new GRTJaguar(br);
backRight.start();
addActuator("backleft", backLeft);
addActuator("backright", backRight);
addActuator("frontleft", frontLeft);
addActuator("frontright", frontRight);
this.debug = debug;
}
public void stop() {
tankDrive(0, 0);
}
public void spinRight() {
spinRight(1.0);
}
public void spinRight(double speed) {
tankDrive(speed, -speed);
}
public void spinLeft() {
spinLeft(1.0);
}
public void spinLeft(double speed) {
tankDrive(-speed, speed);
}
public void driveForward() {
driveForward(1.0);
}
public void driveForward(double speed) {
tankDrive(speed, speed);
}
public void driveBackward() {
driveBackward(1.0);
}
public void driveBackward(double speed) {
tankDrive(-speed, -speed);
}
public void carDrive(double x, double y, boolean spin) {
if (spin) {
tankDrive(-y, y);
} else {
double leftSpeed = x;
double rightSpeed = x;
if (y < 0) {
leftSpeed *= (1.0 + y);
} else if (y > 0) {
rightSpeed *= (1.0 - y);
}
if (debug) {
System.out.println(">> " + x + " " + y);
}
tankDrive(leftSpeed, rightSpeed);
}
}
public void tankDrive(double leftSpeed, double rightSpeed) {
if (debug) {
System.out.println("< " + leftSpeed + " > " + rightSpeed);
}
frontLeft.enqueueCommand(-leftSpeed);
backLeft.enqueueCommand(-leftSpeed);
frontRight.enqueueCommand(-rightSpeed);
backRight.enqueueCommand(-rightSpeed);
}
}