we used this code for strafing and moving, turning is its own can of worms.
Code:
public class WesleyCrusher {
private Victor FRvictor = new Victor(2); //right motor
private Victor FLvictor = new Victor(1); //left motor
private Victor RRvictor = new Victor(4); //right motor
private Victor RLvictor = new Victor(3); //left motor
private Jaguar ballSucker = new Jaguar(7); //ball sucker
boolean DEBUG = true;
void checkingDrive(double magnitude, double rotation) { //DONT TOUCH THIS
magnitude = limit(magnitude); //1 is highest, so make n > 1, 1
rotation = limit(rotation);
double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed; //temporary motor speed values
frontLeftSpeed = ((magnitude - rotation)); //calculate speed values
frontRightSpeed = ((magnitude + rotation));
rearLeftSpeed = ((magnitude + rotation));
rearRightSpeed = ((magnitude - rotation));
if(DEBUG) {
double maxMotor = Math.max(
Math.max(Math.abs(frontLeftSpeed), Math.abs(frontRightSpeed)),
Math.max(Math.abs(rearLeftSpeed), Math.abs(rearRightSpeed))
);
if (maxMotor > 1){
frontRightSpeed = frontRightSpeed / maxMotor;
rearRightSpeed = rearRightSpeed / maxMotor;
frontLeftSpeed = frontLeftSpeed / maxMotor;
rearLeftSpeed = rearLeftSpeed / maxMotor;
}
}
frontLeftSpeed = limit(fiddleWithSpeed(frontLeftSpeed));
frontRightSpeed = limit(fiddleWithSpeed(frontRightSpeed));
rearLeftSpeed = limit(fiddleWithSpeed(rearLeftSpeed));
rearRightSpeed = limit(fiddleWithSpeed(rearRightSpeed));
if(magnitude != 0 || rotation !=0) {
System.out.println("#############################################");
System.out.println("Mangnitude: " + magnitude + " Rotation: " + rotation); //debugOut
System.out.println("FLSpeed: " + frontLeftSpeed + " FRSpeed: " + frontRightSpeed);
System.out.println("RLSpeed: " + rearLeftSpeed + " RRSpeed: " + rearRightSpeed);
}
FLvictor.set(-frontLeftSpeed); //set the speeds on the motors
FRvictor.set(frontRightSpeed);
RLvictor.set(-rearLeftSpeed);
RRvictor.set(rearRightSpeed);
} //void checkingDrive
void turn(double power) {
if(Math.abs(power) > 0) {
RRvictor.set(power);
RLvictor.set(power);
}
}
double limit(double num) {
if (num > 1.0) {
return 1.0;
}
if (num < -1.0) {
return -1.0;
}
return num;
}
void ballSuckerOn() {
ballSucker.set(0.75);
}
void ballSuckerOff() {
ballSucker.set(0);
}
}
Yes, i name all my code classes after bridge crew of the Enterprise-D