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);
}
}