[notmattlythgoe]

Originally Posted by curtis0gj

Hey guys I finally got the encoder going, my electrical guy said it was grounding on something. Anyway I am getting a nice reading on smart db however, when I run autonomous mode the robot turns about 180 stops and turns again and this never ends. I think I am stuck in an if loop and it's not entering the straight driving portion of the program...

public class Robot extends IterativeRobot {
	
	RobotDrive robot;
    Joystick stick;
    Encoder encoder;	
    Gyro gyro1;
    
    static final double Kp = 0.05;
    
    public void robotInit() {
    	
    	 robot = new RobotDrive(0, 1);
         stick = new Joystick(0);
         gyro1 = new Gyro(0);
         gyro1.reset();
         gyro1.initGyro();
         encoder = new Encoder(0, 1, false, EncodingType.k4X);
       
         
         encoder.setDistancePerPulse(1);
       
         encoder.getDistance();
         

    }

   
    public void autonomousPeriodic() {

    	gyro1.reset();
		
    	while(isAutonomous() && isEnabled())  {
    		
    		double angle = gyro1.getAngle();
    		double distance = encoder.get();
    		
    		SmartDashboard.putNumber("angle", angle);
    		SmartDashboard.putNumber("distance", distance);
    		
    		robot.drive(-0.15, -1);
    		
    		if(angle > 150) {
    			
    			robot.drive(0,0);
    			gyro1.reset();
    			encoder.reset();
    			
    			Timer.delay(0.5);

    			break;
    			
    			}
    		
    			Timer.delay(0.1);
    	}
    	
				while (isAutonomous() && isEnabled()) {

						double angle2 = gyro1.getAngle(); 
						double distance2 = encoder.get();
						
						SmartDashboard.putNumber("distance", distance2);
						SmartDashboard.putNumber("angle", angle2);
						
						robot.drive(-0.20, -angle2 * Kp); 
					
						if(encoder.getDistance() < 100)  {
							robot.drive(0,-0); 
							
							break;
						}
						Timer.delay(0.1);	
				}
				
    			}

What is the reported angle during the second part of the autonomous routine? My guess is that it is not scaled enough and it is correcting it in the wrong direction.