Issues with following a path and P value

[cptcomo]

My team has been trying to implement Motion Profiling using the library, and at this point we have run into some weird issues that we can't quite find the cause.

The first issue is that whatever P value we set seems to have very little difference. Having the P value set at 1 like in the sample makes it overshoot our target of 1 meter, and we put our P value at .0000000000000000000000000001 and it was still moving at near max velocity, ending motion with a variance of +/- half a meter.

The second issue is that the robot seems to only move forward regardless of the set path. Setting the only Waypoint to be (1,0,0) puts the robot in the ballpark of a meter, and setting it to (2, 0, 0) puts the robot in the ballpark of 2 meters. However, having a path of (1, 0, 0) then (-1, 0, 0) just makes the robot drive full speed forward to 2-3 meters, same issue with the sample path.

Here is our full code:

`public class Robot extends TimedRobot { Joystick leftStick, rightStick, auxStick;

Waypoint[] points;
EncoderFollower left, right;
TalonSRX talonFrontLeft, talonFrontRight, talonBackLeft, talonBackRight;
AHRS vmx;

PIDController driveController;
double driveOutput = 0;

@Override
public void robotInit() {
    talonFrontLeft = new TalonSRX(1);
    talonFrontRight = new TalonSRX(2);
    talonBackLeft = new TalonSRX(3);
    talonBackRight = new TalonSRX(4);

    talonFrontRight.setInverted(true);
    talonBackRight.setInverted(true);

    leftStick = new Joystick(0);
    rightStick = new Joystick(1);
    auxStick = new Joystick(5);

    try {
        vmx = new AHRS(Port.kUSB1);
        //vmx = new AHRS(SPI.Port.kMXP);
    } catch(Exception e) {
        System.out.println("VMX is null");
    }

    //The first int is pidIdx - 0 for primary closed loop, 1 for cascaded closed loop
    //The second int is timeoutMs - >0 will timeout error if it times out, 0 means dont check at all
    talonFrontLeft.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
    talonFrontRight.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);

}

public void drive(double drive, double turn, double strafe) {
    talonFrontLeft.set(ControlMode.PercentOutput, drive + turn + strafe);
    talonFrontRight.set(ControlMode.PercentOutput, drive - turn - strafe);
    talonBackLeft.set(ControlMode.PercentOutput, drive + turn - strafe);
    talonBackRight.set(ControlMode.PercentOutput, drive - turn + strafe);
}

@Override
public void autonomousInit() {
}

@Override
public void autonomousPeriodic() {

}

@Override
public void teleopInit() {
    points = new Waypoint[] {
            new Waypoint(-4, -1, Pathfinder.d2r(-45)),
            new Waypoint(-2, -2, 0),
            new Waypoint(0, 0, 0)

    };
    talonFrontLeft.setSelectedSensorPosition(0, 0, 0);
    talonFrontRight.setSelectedSensorPosition(0, 0, 0);
    vmx.reset();
}

@Override
public void teleopPeriodic() {
    SmartDashboard.putNumber("Left encoder", getLeftDistance());
    SmartDashboard.putNumber("Right encoder", getRightDistance());
    SmartDashboard.putNumber("Distance Travelled", getEncDistance());
    SmartDashboard.putNumber("VMX Angle", vmx.getYaw());

    if(leftStick.getRawButton(1))
        drive(-leftStick.getY(), rightStick.getX(), leftStick.getX());
    else drive(0, 0, 0);

    if(auxStick.getRawButtonPressed(3)) {
        double dt = 0.05;
        double maxVelocity = 1.7;
        double maxAcceleration = 1.0;
        double maxJerk = 60.0;
        Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC,
                Trajectory.Config.SAMPLES_FAST, dt, maxVelocity, maxAcceleration, maxJerk);
        Trajectory trajectory = Pathfinder.generate(points, config);

        double wheelbaseWidth = 0.43; //Distance between the two sides of DT, in meters.
        TankModifier modifier = new TankModifier(trajectory).modify(wheelbaseWidth);

        left = new EncoderFollower(modifier.getLeftTrajectory());
        right = new EncoderFollower(modifier.getRightTrajectory());

        //configureEncoder(encoderPosition, encoderTickPerRevolution, wheelDiameter)
        left.configureEncoder(talonFrontLeft.getSelectedSensorPosition(0), 4096, .1524);
        right.configureEncoder(talonFrontRight.getSelectedSensorPosition(0), 4096, .1524);

        double p = .1;
        double i = 0;
        double d = 0;
        double velocityInverse = 1/maxVelocity;
        double accelGain = 0;
        left.configurePIDVA(p, i, d, velocityInverse, accelGain);
        right.configurePIDVA(p, i, d, velocityInverse, accelGain);
    }
    if(auxStick.getRawButton(3)) { 
        if(!left.isFinished() && !right.isFinished()) {
            double leftSpeed = left.calculate(talonFrontLeft.getSelectedSensorPosition(0));
            double rightSpeed = right.calculate(talonFrontRight.getSelectedSensorPosition(0));
            double gyroHeading = vmx.getAngle();
            double desiredHeading = Pathfinder.r2d(left.getHeading());
            double angleDifference = Pathfinder.boundHalfDegrees(desiredHeading - gyroHeading);
            double turn = 0.04 * (1.0 / 80.0) * angleDifference;
            SmartDashboard.putNumber("MP Left Speed", leftSpeed);
            SmartDashboard.putNumber("MP Right Speed", rightSpeed);
            SmartDashboard.putNumber("MP Turn", turn);

            System.out.println("Left speed: " + leftSpeed);
            System.out.println("Right speed: " + rightSpeed);
            System.out.println("Turn: " + turn);

            talonFrontLeft.set(ControlMode.PercentOutput, leftSpeed + turn);
            talonBackLeft.set(ControlMode.PercentOutput, leftSpeed + turn);
            talonFrontRight.set(ControlMode.PercentOutput, rightSpeed - turn);
            talonBackRight.set(ControlMode.PercentOutput, rightSpeed - turn); 
        }
        else {
            talonFrontLeft.set(ControlMode.PercentOutput, 0);
            talonBackLeft.set(ControlMode.PercentOutput, 0);
            talonFrontRight.set(ControlMode.PercentOutput, 0);
            talonBackRight.set(ControlMode.PercentOutput, 0); 
        }
    }

    if(auxStick.getRawButtonPressed(4)) {

    }
}

public double getLeftDistance() {
    double d = talonFrontLeft.getSelectedSensorPosition(0) / 4096.0 * 6 * Math.PI;
    //return d + bestFitLine(d);
    return d;
}

public double getRightDistance() {
    double d = talonFrontRight.getSelectedSensorPosition(0) / 4096.0 * 6 * Math.PI;
    //return d + bestFitLine(d);
    return d;
}

public double bestFitLine(double dist) {
    return dist * 0.0618 + .148;
}

public double getEncDistance() {
    return (getLeftDistance() + getRightDistance()) / 2;
}

@Override
public void testPeriodic() {
}

} `