Thread: Autonomous Help
View Single Post
  #25   Spotlight this post!  
Unread 23-01-2015, 15:47
curtis0gj curtis0gj is offline
Registered User
FRC #5033 (Beavertronics)
Team Role: Programmer
 
Join Date: Jan 2015
Rookie Year: 2015
Location: Canada
Posts: 121
curtis0gj will become famous soon enough
Re: Autonomous Help

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...

Code:
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);	
				}
				
    			}
Reply With Quote