71 lines
1.8 KiB
Java
71 lines
1.8 KiB
Java
package robot.subsystems;
|
|
|
|
import robot.Pose2d;
|
|
|
|
/**
|
|
* Drive subsystem - business logic only.
|
|
* Hardware interface defined as inner interface.
|
|
* Tests use inline mocks - no FTC SDK needed.
|
|
*/
|
|
public class Drive {
|
|
private final Hardware hardware;
|
|
private Pose2d pose;
|
|
|
|
public Drive(Hardware hardware) {
|
|
this.hardware = hardware;
|
|
this.pose = new Pose2d();
|
|
}
|
|
|
|
/**
|
|
* Drive using field-centric controls.
|
|
* @param forward Forward/backward speed (-1.0 to 1.0)
|
|
* @param strafe Left/right speed (-1.0 to 1.0)
|
|
* @param rotate Rotation speed (-1.0 to 1.0)
|
|
*/
|
|
public void drive(double forward, double strafe, double rotate) {
|
|
// Your drive logic here
|
|
double heading = hardware.getHeading();
|
|
|
|
// Example: field-centric conversion
|
|
double cos = Math.cos(heading);
|
|
double sin = Math.sin(heading);
|
|
|
|
double rotatedForward = forward * cos - strafe * sin;
|
|
double rotatedStrafe = forward * sin + strafe * cos;
|
|
|
|
hardware.setPowers(rotatedForward, rotatedStrafe, rotate);
|
|
}
|
|
|
|
/**
|
|
* Stop all drive motors.
|
|
*/
|
|
public void stop() {
|
|
hardware.setPowers(0, 0, 0);
|
|
}
|
|
|
|
/**
|
|
* Get current estimated pose.
|
|
*/
|
|
public Pose2d getPose() {
|
|
return pose;
|
|
}
|
|
|
|
/**
|
|
* Hardware interface - implement this for real robot.
|
|
*/
|
|
public interface Hardware {
|
|
/**
|
|
* Get robot heading in radians.
|
|
*/
|
|
double getHeading();
|
|
|
|
/**
|
|
* Set drive motor powers.
|
|
* @param forward Forward power
|
|
* @param strafe Strafe power
|
|
* @param rotate Rotation power
|
|
*/
|
|
void setPowers(double forward, double strafe, double rotate);
|
|
}
|
|
}
|