Here's what I've interpreted so far. The rotate left/right may be flipped.
This is a rather strange convention.
leftMotorSpeed = moveValue - rotateValue;
rightMotorSpeed = Math.max(moveValue, rotateValue);
It's probably trying to make sure the right motor is going faster than the left (i.e. turning left), but regardless this is probably not the ideal way to code this.
Code:
public class Drivetrain {
//right speed controllers
Talon talon1 = new Talon(1); //front
Talon talon2 = new Talon(2); //middle
Talon talon3 = new Talon(3); //rear
//left speed controllers
Talon talon4 = new Talon(4); //front
Talon talon5 = new Talon(5); //middle
Talon talon6 = new Talon(6); //rear
DoubleSolenoid shifter = new DoubleSolenoid(1, 5, 6);
Encoder leftEncoder = new Encoder(10, 11);
Encoder rightEncoder = new Encoder(13, 14);
public Drivetrain(){
leftEncoder.start();
rightEncoder.start();
}
//DigitalInput port14 = new DigitalInput(14);
public void arcadeDrive(double moveValue, double rotateValue){
double leftMotorSpeed = 0, rightMotorSpeed = 0;
rotateValue = -rotateValue; //x axis value from joystick. inverted to get the correct rotation direction
moveValue = limit(moveValue); //y axis value from joystick. make sure its between -1 and 1
rotateValue = limit(rotateValue); //make sure the rotation value from before is between -1 and 1
if (moveValue > 0.0) {
//driving forward?
if (rotateValue > 0.0) {
//turning left?
leftMotorSpeed = moveValue - rotateValue;
rightMotorSpeed = Math.max(moveValue, rotateValue);
} else {
//stopped or turning right?
leftMotorSpeed = Math.max(moveValue, -rotateValue);
rightMotorSpeed = moveValue + rotateValue;
}
} else {
//driving backward or stopped
if (rotateValue > 0.0) {
//turning left?
leftMotorSpeed = -Math.max(-moveValue, rotateValue);
rightMotorSpeed = moveValue + rotateValue;
} else {
//stopped or turning right?
leftMotorSpeed = moveValue - rotateValue;
rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
}
}
//Setting talon values
talon1.set(-rightMotorSpeed); //invert the front right speed controller (probably because the leads were swapped)
talon2.set(rightMotorSpeed);
talon3.set(rightMotorSpeed);
talon4.set(leftMotorSpeed);
talon5.set(-leftMotorSpeed); //invert the center left speed controller (probably because the leads were swapped)
talon6.set(-leftMotorSpeed); //invert the rear left speed controller (probably because the leads were swapped)
}
protected static double limit(double num) {
if (num > 1.0) {
return 1.0;
}
if (num < -1.0) {
return -1.0;
}
return num;
}
public void highGear(){
//retract pneumatic
shifter.set(DoubleSolenoid.Value.kReverse);
}
public void lowGear(){
//extend pneumatic
shifter.set(DoubleSolenoid.Value.kForward);
}
public void brake(){
}
public int getRightDistance(){
return rightEncoder.get();
}
public int getLeftDistance(){
return leftEncoder.get();
}
public void resetEncoders(){
leftEncoder.reset();
rightEncoder.reset();
}
}