7efec6d03c
lots of stuff no one will read these Former-commit-id: 14dcd018bd1b6f74d4b35aa33d9185aca59e9213
142 lines
4.7 KiB
Java
142 lines
4.7 KiB
Java
package org.firstinspires.ftc.teamcode;
|
|
|
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
|
|
|
|
|
|
|
public class MotorBox {
|
|
private DcMotor frontRight, frontLeft, rearRight, rearLeft;
|
|
private double frontRightPower, frontLeftPower, rearRightPower, rearLeftPower;
|
|
|
|
public MotorBox(DcMotor frontRight, DcMotor frontLeft, DcMotor rearRight, DcMotor rearLeft, boolean toPosition){
|
|
init(frontRight, frontLeft, rearRight, rearLeft, toPosition);
|
|
}
|
|
|
|
// drive code
|
|
// works with both encoder power and encoder position theoretically
|
|
public String drivePower(double yDrive, double xDrive, double turn, double speed) {
|
|
|
|
// sets up the initial power values based on input
|
|
frontRightPower = yDrive - xDrive;
|
|
frontLeftPower = yDrive + xDrive;
|
|
rearRightPower = yDrive + xDrive;
|
|
rearLeftPower = yDrive - xDrive;
|
|
|
|
// debugging
|
|
String returnVal = "Front Right = " + frontRightPower + "\nFront Left = " + frontLeftPower + "\nRear Right = " + rearRightPower + "\nRear Left = " + rearLeftPower +
|
|
"\nyDrive = " + yDrive + "\nxDrive = " + xDrive +
|
|
"\nTurn = " + turn;
|
|
|
|
// does the turn calculations
|
|
if (turn != 0) {
|
|
addTurn(turn);
|
|
}
|
|
|
|
// multiplies by speed
|
|
frontRightPower *= speed;
|
|
frontLeftPower *= speed;
|
|
rearRightPower *= speed;
|
|
rearLeftPower *= speed;
|
|
|
|
// applies the power
|
|
frontRight.setPower(frontRightPower);
|
|
frontLeft.setPower(-1 * frontLeftPower);
|
|
rearRight.setPower(rearRightPower);
|
|
rearLeft.setPower(-1 * rearLeftPower);
|
|
|
|
// return for debugging
|
|
return returnVal;
|
|
}
|
|
|
|
public int drivePositionY(int distance){
|
|
int numEncoders;
|
|
|
|
//code to convert the distance to a number of encoders
|
|
numEncoders = distance;
|
|
|
|
frontRight.setTargetPosition(-1 * numEncoders);
|
|
frontLeft.setTargetPosition(numEncoders);
|
|
rearRight.setTargetPosition(-1 * numEncoders);
|
|
rearLeft.setTargetPosition(numEncoders);
|
|
|
|
return numEncoders;
|
|
}
|
|
|
|
public int drivePositionX(int distance){
|
|
int numEncoders;
|
|
|
|
//code to convert distance to a number of encoders
|
|
numEncoders = distance;
|
|
|
|
frontRight.setTargetPosition(numEncoders);
|
|
frontLeft.setTargetPosition(numEncoders);
|
|
rearRight.setTargetPosition(-1 * numEncoders);
|
|
rearLeft.setTargetPosition(-1 * numEncoders);
|
|
|
|
return numEncoders;
|
|
}
|
|
|
|
|
|
public int turn(int angle){
|
|
int numEncoders;
|
|
|
|
// code to convert angle into a number of encoders
|
|
// will have a negative number for negative angles
|
|
// turning right is positive left is negative
|
|
numEncoders = angle;
|
|
|
|
frontRight.setTargetPosition(numEncoders);
|
|
frontLeft.setTargetPosition(numEncoders);
|
|
rearRight.setTargetPosition(numEncoders);
|
|
rearLeft.setTargetPosition(numEncoders);
|
|
|
|
return numEncoders;
|
|
}
|
|
|
|
// changes value corresponding to turn
|
|
// two states based on if the MotorControl
|
|
private void addTurn(double turn){
|
|
frontRightPower -= turn;
|
|
rearRightPower -= turn;
|
|
frontLeftPower += turn;
|
|
rearLeftPower +=turn;
|
|
|
|
}
|
|
|
|
|
|
|
|
// initializes the motors, sets up the correct encoder mode, and sets the toPosition boolean
|
|
private void init(DcMotor motor1, DcMotor motor2, DcMotor motor3, DcMotor motor4, boolean toPosition){
|
|
frontRight = motor1;
|
|
frontLeft = motor2;
|
|
rearRight = motor3;
|
|
rearLeft = motor4;
|
|
|
|
// stops and resets encoders
|
|
frontRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
|
frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
|
rearRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
|
rearLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
|
|
|
// sets the toPosition boolean
|
|
|
|
// turns on encoders
|
|
if (toPosition) {
|
|
frontRight.setTargetPosition(200);
|
|
frontLeft.setTargetPosition(200);
|
|
rearRight.setTargetPosition(200);
|
|
rearLeft.setTargetPosition(200);
|
|
frontRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
frontLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
rearRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
rearLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
|
}
|
|
else{
|
|
frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
rearRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
rearLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
|
}
|
|
}
|
|
}
|