75 lines
2.8 KiB
Java
75 lines
2.8 KiB
Java
package robot.hardware;
|
|
|
|
import robot.subsystems.Drive;
|
|
|
|
// Uncomment when deploying to robot:
|
|
// import com.qualcomm.robotcore.hardware.DcMotor;
|
|
// import com.qualcomm.robotcore.hardware.HardwareMap;
|
|
// import com.qualcomm.robotcore.hardware.IMU;
|
|
// import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
|
|
/**
|
|
* Mecanum drive hardware implementation.
|
|
* Implements Drive.Hardware interface.
|
|
*
|
|
* DEPLOYMENT NOTE:
|
|
* Uncomment FTC imports and implementation when ready to deploy.
|
|
* Keep commented during development/testing on PC.
|
|
*/
|
|
public class MecanumDrive implements Drive.Hardware {
|
|
|
|
// Uncomment when deploying:
|
|
// private final DcMotor frontLeft;
|
|
// private final DcMotor frontRight;
|
|
// private final DcMotor backLeft;
|
|
// private final DcMotor backRight;
|
|
// private final IMU imu;
|
|
|
|
public MecanumDrive(/* HardwareMap hardwareMap */) {
|
|
// Uncomment when deploying:
|
|
// frontLeft = hardwareMap.get(DcMotor.class, "frontLeft");
|
|
// frontRight = hardwareMap.get(DcMotor.class, "frontRight");
|
|
// backLeft = hardwareMap.get(DcMotor.class, "backLeft");
|
|
// backRight = hardwareMap.get(DcMotor.class, "backRight");
|
|
// imu = hardwareMap.get(IMU.class, "imu");
|
|
//
|
|
// // Configure motors
|
|
// frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
// frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
// backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
// backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
|
}
|
|
|
|
@Override
|
|
public double getHeading() {
|
|
// Stub for testing - returns 0
|
|
return 0.0;
|
|
|
|
// Uncomment when deploying:
|
|
// return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
|
|
}
|
|
|
|
@Override
|
|
public void setPowers(double forward, double strafe, double rotate) {
|
|
// Stub for testing - does nothing
|
|
|
|
// Uncomment when deploying:
|
|
// // Mecanum drive kinematics
|
|
// double frontLeftPower = forward + strafe + rotate;
|
|
// double frontRightPower = forward - strafe - rotate;
|
|
// double backLeftPower = forward - strafe + rotate;
|
|
// double backRightPower = forward + strafe - rotate;
|
|
//
|
|
// // Normalize powers
|
|
// double maxPower = Math.max(1.0, Math.max(
|
|
// Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)),
|
|
// Math.max(Math.abs(backLeftPower), Math.abs(backRightPower))
|
|
// ));
|
|
//
|
|
// frontLeft.setPower(frontLeftPower / maxPower);
|
|
// frontRight.setPower(frontRightPower / maxPower);
|
|
// backLeft.setPower(backLeftPower / maxPower);
|
|
// backRight.setPower(backRightPower / maxPower);
|
|
}
|
|
}
|