Line Following

[pigpc1993]

Our line following code works great until it start going straight. The speed has a sudden jolt and then gets off track. We tried setting the speed as low as .05 and yet it still jolts the same amount

Drive Method

public class Drive implements Mechanism{
    private static final double SPEED_STOP            =  0.0;
    private static final double SPEED_FWD_MAX         =  -.2;
    private static final double SPEED_REV_MAX         = .2;
    RobotDrive m_drive = new RobotDrive(2,1,4,3);
    DStation d = new DStation();

    public Drive(){
        m_drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
    }
    public void initialize(){
        stop();
    }

    public void run(double x, double y){
    }
    
    public void joystick(double x, double y){
        m_drive.arcadeDrive(y, x);
        d.toLCDLine(2, "" + x);
        d.toLCDLine(3, "" + y);
    }
    
     public void stop(){
        m_drive.arcadeDrive(SPEED_STOP,SPEED_STOP,false);
    }

    public void goForward() {
        m_drive.arcadeDrive(SPEED_FWD_MAX, SPEED_STOP, false);
    }

    public void goBackward() {
        m_drive.arcadeDrive(SPEED_REV_MAX, SPEED_STOP, false);
    }
    public void goLeft() {
        m_drive.arcadeDrive(SPEED_STOP, -SPEED_FWD_MAX, false);
    }
    public void goRight() {
        m_drive.arcadeDrive(SPEED_STOP, -SPEED_REV_MAX, false);
    }
    public void goRightBackward() {
        m_drive.arcadeDrive(SPEED_REV_MAX, SPEED_FWD_MAX, false);
    }
    public void goLeftBackward() {
        m_drive.arcadeDrive(SPEED_REV_MAX, SPEED_REV_MAX, false);
    }
    public void goForwardLeft() {
        m_drive.arcadeDrive(SPEED_FWD_MAX, SPEED_FWD_MAX, false);
    }
    public void goForwardRight() {
        m_drive.arcadeDrive(SPEED_FWD_MAX, SPEED_REV_MAX, false);
    }
}

Line Follow Method

public class LightSensor extends Maneuver {

    DigitalInput l1;
    DigitalInput l2;
    DigitalInput l3;
    Drive drive;
    DStation station;

    public LightSensor(DigitalInput l1, DigitalInput l2, DigitalInput l3, Drive drive,DStation station,  Maneuver pass, Maneuver fail, Maneuver timeout,
            double maxTime){
        super(pass, fail, timeout, maxTime);
        this.drive = drive;
        this.station = station;
        this.l1 = l1;
        this.l2 = l2;
        this.l3 = l3;

    }

    public void run(){

            station.toLCDLine(2, l1.get() + "");
            station.toLCDLine(3, l2.get() + "");
            station.toLCDLine(4, l3.get() + "");
        if((l1.get() == false) && (l2.get() == true) && (l3.get() == false)){
            drive.goForward();
        }
        else if((l1.get() == false) && (l2.get() == false) && (l3.get() == true)){
            drive.goRight();
        }
        else if((l1.get() == true) && (l2.get() == false) && (l3.get() == false)){
            drive.goLeft();
        }
        else if((l1.get() == true) && (l2.get() == true) && (l3.get() == true)){
            drive.stop();
        }
        else{
                drive.goRight();
        }
    }

    public void stop(){
        drive.stop();
            station.toLCDLine(1, "Stop");
    }