Code:
package com.shadowh511.tomo.camera.moving;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.DriverStationLCD;
import com.sun.squawk.util.MathUtils;
/**
*
* @author sam
*/
public class Driver {
private Victor FLvictor;
private Victor FRvictor;
private Victor RLvictor;
private Victor RRvictor;
private boolean wheel4;
private boolean mecanum;
/*
* Usage: Driver robot = new Driver(1,2);
*/
public Driver(int L, int R) {
FLvictor = new Victor(L);
FRvictor = new Victor(R);
wheel4 = false;
mecanum = false;
}
/*
* Usage: Driver robot = new Driver(1,2,3,4);
*/
public Driver(int FL, int FR, int RL, int RR) {
wheel4 = true;
FLvictor = new Victor(FL);
FRvictor = new Victor(FR);
RLvictor = new Victor(RL);
RRvictor = new Victor(RR);
mecanum = false;
}
/*
* Usage: Driver robot = new Driver(1,2,3,4,true);
*/
public Driver(int FL, int FR, int RL, int RR, boolean mecanum) {
wheel4 = true;
FLvictor = new Victor(FL);
FRvictor = new Victor(FR);
RLvictor = new Victor(RL);
RRvictor = new Victor(RR);
mecanum = true;
}
/*
* This function takes in one input, speed. If you dont have the mecanum
* flag defined, it will skip to just setting the speed. Otherwise, it will
* set the speeds of the first two Victors. If you are using four wheels
* in the constructor, it will also set those speeds accordingly.
*/
public void drive(double speed) {
if(!mecanum){
FLvictor.set(speed);
FRvictor.set(-speed);
if(wheel4) {
RLvictor.set(speed);
RRvictor.set(-speed);
}
} else {
this.mecanumDrive(speed, 0);
}
}
/*
* This functon takes in two inputs, speed and curve. It then creates
* temporary speed variables. It borrows WPI's curve calculation algorithm
* to creatively figure out the individual motor values. It then calls
* tankDrive to set the motor speeds.
*/
public void drive(double speed, double curve) {
double leftSpeed, rightSpeed;
if (curve < 0) { //borrowed from edu.wpi.first.wpilibj.RobotDrive, calculates the curve
double value = MathUtils.log(-curve);
double ratio = (value - 0) / (value + 0);
if (ratio == 0) {
ratio = .0000000001;
}
leftSpeed = speed / ratio;
rightSpeed = speed;
} else if (curve > 0) {
double value = MathUtils.log(curve);
double ratio = (value - 0) / (value + 0);
if (ratio == 0) {
ratio = .0000000001;
}
leftSpeed = speed;
rightSpeed = speed / ratio;
} else {
leftSpeed = speed;
rightSpeed = speed;
} //end borrowing
this.tankDrive(leftSpeed, rightSpeed); //yay for brilliant hacks!!!
}
/*
* This takes two inputs, left speed and right speed
* it sets the first two victors
* then it checks if you called Driver with 4 motor ports
* if that is true, it sets the values for the back two motors
*/
public void tankDrive(double left, double right) {
if (!mecanum) {
FLvictor.set(left);
FRvictor.set(right);
if(wheel4) {
RLvictor.set(left);
RRvictor.set(right);
}
}
}
/*
* This function takes two inputs, magnitude (forward/reverse speed)
* and rotation (strafe). It first makes sure that the values aren't
* greater than 1 and then makes four temporary motor speed values.
* It then calculates the speed values. After finding out which motor
* is running the fastest, it divides the other motor values by the
* fastest motor speed if its positive. It then limits the result of
* fiddling with the speed of each motor. Finally, it sets the speeds
* for each motor
*/
void mecanumDrive(double magnitude, double rotation) { //DONT TOUCH THIS
if(mecanum) {
magnitude = limit(magnitude); //1 is highest, so make n > 1, 1
rotation = limit(rotation); //its not nessecary to do this, but better safe then sorry
double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed; //temporary motor speed values
frontLeftSpeed = ((magnitude - rotation)); //calculate speed values
frontRightSpeed = ((magnitude + rotation));
rearLeftSpeed = ((magnitude + rotation));
rearRightSpeed = ((magnitude - rotation));
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));
FLvictor.set(-frontLeftSpeed); //set the speeds on the motors
FRvictor.set(frontRightSpeed); //these four lines could be replaced
RLvictor.set(-rearLeftSpeed); //by a set() function, taking in 4
RRvictor.set(rearRightSpeed); //doubles and then setting the Victor values
}
else { //If you did not enable the mecanum flag on initilization
DriverStationLCD lcd;
lcd = DriverStationLCD.getInstance();
lcd.println(DriverStationLCD.Line.kMain6, 0, "NO MECANUM");
}
} //void checkingDrive
/*
* Our hack to turn our mecanum wheels, call it separately
*/
void turn(double power) {
if(Math.abs(power) > 0) {
RRvictor.set(power);
RLvictor.set(power);
}
}
/*
* Makes sure we dont send a n<1 value to the victors, they dont like that
*/
double limit(double num) {
if (num > 1.0) {
return 1.0;
}
if (num < -1.0) {
return -1.0;
}
return num;
}
/*
* This brilliant hack was make by our team's all around awesome mentor, Dimitri
* Dont touch it
*
* Takes one input, input. if the input is positive, it squares it. If its
* negative, it squares it while preserving the sign. It returns the input.
*/
double fiddleWithSpeed(double input) {
if(input > 0) {
input = input * input;
} else {
input = -input * input;
}
return input;
}
}