Drivetrain Idea - "Box Drive"

[cbale2000]

Originally Posted by nathannfm

Lobster Drive

Impressive, probably the most practical way to do that too.

Thanks for that link, it made my day.

[pimathbrainiac]

So if figured out the math/code for this using cjl2625's version of the drivetrain.

Here's a segment (Java, btw):

    final double DISTANCE_X = 1.0;
    final double DISTANCE_Y = 1.0;
    private double leftY;
    private double rightY;
    private double frontX;
    private double backX;
    //rotation is cw
    private double rotationX;
    private double rotationY;
    private double pi = 3.1415926;
    private double curr_gyro_angle_degrees = 0.0;
    private double curr_gyro_angle_radians = 0.0;
    private double temp;
    
    public void TakeJoystickInputsAndDrive(Joystick left, Joystick right)
    {
        calculatedDrive(-0.5 * left.getY(), 0.5 * left.getX(), 0.5 * right.getX());
    }
    
    void calculatedDrive(double y, double x, double rotation)
    {
        //Field-centric adjustments. Comment out for robot-centric.
        curr_gyro_angle_degrees = gyro1.getAngle();
        curr_gyro_angle_radians = curr_gyro_angle_degrees * pi/180; 
        temp = y * Math.cos(curr_gyro_angle_radians) - x * Math.sin(curr_gyro_angle_radians);
        x = y * Math.sin(curr_gyro_angle_radians) + x * Math.cos(curr_gyro_angle_radians);
        y = temp;
        
        rotationX = rotation * DISTANCE_X;
        rotationY = rotation * DISTANCE_Y;
        leftY = y - rotationY;
        rightY = y + rotationY;
        frontX = x - rotationX;
        backX = x + rotationX;
        drive(leftY, rightY, frontX, backX);
    }
    
    void drive(double y1, double y2, double x1, double x2)
    {
        //left, right
        robotDrive2Y.drive(y1, y2);
        //front, back
        robotDrive2X.drive(x1, x2);
    }

This code runs in a command called "DriveWithJoysticks," which is the default command for drivetrain.

Any problems with the code/math that you can see at the moment? I want to make sure this is sound before I pitch this to my team.