PPosTest.java

public class PPosTest {
	private static final double MIN_TURN_SPEED = 0.1;
    private static final double kP_TURN = -0.01; //todo: tune value

	public static void main(String[] args) {
		PPosTest test = new PPosTest();
		
		for (double targetVelocity = 0.15; targetVelocity < 0.31; targetVelocity += 0.1) { 
			for (int i = 0; i < 10; i++) {
				test.execute(targetVelocity, 0.0, (double)i * 3.0);
			}
			System.out.println("---");
			for (int i = 0; i < 10; i++) {
				test.execute(targetVelocity, 0.0, -((double)i * 3.0));
			}
			System.out.println("-------------------------------------------------------------");
		}
	}
	
	protected void execute(double targetVelocity, double currHeading, double desiredHeading ) {
        double angleError = desiredHeading - currHeading;
        double powerDeduction;

        if( Math.abs(kP_TURN * angleError) > 45) {
            powerDeduction = 0.9;
        } else {
            powerDeduction = kP_TURN * angleError;
        }

        //cap powerDeduction so robot speed never drops below MIN_TURN_SPEED and always moves forwards
        powerDeduction = Math.max(powerDeduction, targetVelocity - MIN_TURN_SPEED);

        double lset = 0, rset = 0;
        if(angleError > 0) {
            lset = targetVelocity;
            rset = targetVelocity - powerDeduction;
        } else if (angleError < 0) {
        	lset = targetVelocity - powerDeduction;
        	rset = targetVelocity;
        } else {
        	lset = targetVelocity;
        	rset = targetVelocity;
        }

        System.out.printf("TargeVel: %5.2f;   CurrHdg: %6.2f; DesiredHdg: %6.2f; Error: %7.2f;  LSet: %7.3f; RSet: %7.3f;  powerDeduct=%7.3f\n",
        		targetVelocity, currHeading, desiredHeading, angleError, lset, rset, powerDeduction);
    }
}

/*************************
Output from above:

TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:   0.00; Error:    0.00;  LSet:   0.150; RSet:   0.150;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:   3.00; Error:    3.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:   6.00; Error:    6.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:   9.00; Error:    9.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  12.00; Error:   12.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  15.00; Error:   15.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  18.00; Error:   18.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  21.00; Error:   21.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  24.00; Error:   24.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  27.00; Error:   27.00;  LSet:   0.150; RSet:   0.100;  powerDeduct=  0.050
---
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  -0.00; Error:   -0.00;  LSet:   0.150; RSet:   0.150;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  -3.00; Error:   -3.00;  LSet:   0.100; RSet:   0.150;  powerDeduct=  0.050
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  -6.00; Error:   -6.00;  LSet:   0.090; RSet:   0.150;  powerDeduct=  0.060
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg:  -9.00; Error:   -9.00;  LSet:   0.060; RSet:   0.150;  powerDeduct=  0.090
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg: -12.00; Error:  -12.00;  LSet:   0.030; RSet:   0.150;  powerDeduct=  0.120
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg: -15.00; Error:  -15.00;  LSet:   0.000; RSet:   0.150;  powerDeduct=  0.150
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg: -18.00; Error:  -18.00;  LSet:  -0.030; RSet:   0.150;  powerDeduct=  0.180
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg: -21.00; Error:  -21.00;  LSet:  -0.060; RSet:   0.150;  powerDeduct=  0.210
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg: -24.00; Error:  -24.00;  LSet:  -0.090; RSet:   0.150;  powerDeduct=  0.240
TargeVel:  0.15;   CurrHdg:   0.00; DesiredHdg: -27.00; Error:  -27.00;  LSet:  -0.120; RSet:   0.150;  powerDeduct=  0.270
-------------------------------------------------------------
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:   0.00; Error:    0.00;  LSet:   0.250; RSet:   0.250;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:   3.00; Error:    3.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:   6.00; Error:    6.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:   9.00; Error:    9.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  12.00; Error:   12.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  15.00; Error:   15.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  18.00; Error:   18.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  21.00; Error:   21.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  24.00; Error:   24.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  27.00; Error:   27.00;  LSet:   0.250; RSet:   0.100;  powerDeduct=  0.150
---
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  -0.00; Error:   -0.00;  LSet:   0.250; RSet:   0.250;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  -3.00; Error:   -3.00;  LSet:   0.100; RSet:   0.250;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  -6.00; Error:   -6.00;  LSet:   0.100; RSet:   0.250;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg:  -9.00; Error:   -9.00;  LSet:   0.100; RSet:   0.250;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg: -12.00; Error:  -12.00;  LSet:   0.100; RSet:   0.250;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg: -15.00; Error:  -15.00;  LSet:   0.100; RSet:   0.250;  powerDeduct=  0.150
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg: -18.00; Error:  -18.00;  LSet:   0.070; RSet:   0.250;  powerDeduct=  0.180
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg: -21.00; Error:  -21.00;  LSet:   0.040; RSet:   0.250;  powerDeduct=  0.210
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg: -24.00; Error:  -24.00;  LSet:   0.010; RSet:   0.250;  powerDeduct=  0.240
TargeVel:  0.25;   CurrHdg:   0.00; DesiredHdg: -27.00; Error:  -27.00;  LSet:  -0.020; RSet:   0.250;  powerDeduct=  0.270
-------------------------------------------------------------

**************************/