We are a rookie team that is trying to setup our drive system to drive straight by utilizing a gyro. We are having an issue with our gyro (ADXRS450) that came in the KoP. When we call getAngle() while our robot is stationary the angle constantly increases. We have tried gyro.reset() when the robot is initialized and the accumulators do reset to zero. However, they start to climb instantly (without touching the robot). Is this something that is supposed to happen or could we have something incorrectly set?
Below is our code:
Code:
package org.usfirst.frc.team6484.robot;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This is a sample program to demonstrate how to use a gyro sensor to make a
* robot drive straight. This program uses a joystick to drive forwards and
* backwards while the gyro is used for direction keeping.
*/
public class Robot extends IterativeRobot {
private static final double kAngleSetpoint = 0.0;
private static final double kP = 0.005; // propotional turning constant
// gyro calibration constant, may need to be adjusted;
// gyro value of 360 is set to correspond to one full revolution
private static final double kVoltsPerDegreePerSecond = 0.0128;
private static final int kLeftMotorFrontPort = 0;
private static final int kLeftMotorBackPort = 1;
private static final int kRightMotorFrontPort = 2;
private static final int kRightMotorBackPort = 3;
private static final int kGyroPort = 1;
private static final int kJoystickPort = 0;
private RobotDrive myRobot = new RobotDrive(kLeftMotorFrontPort, kLeftMotorBackPort, kRightMotorFrontPort, kRightMotorBackPort);
private ADXRS450_Gyro gyro = new ADXRS450_Gyro();
private Joystick joystick = new Joystick(kJoystickPort);
@Override
public void robotInit() {
gyro.reset();
// gyro.calibrate();
}
/**
* The motor speed is set from the joystick while the RobotDrive turning
* value is assigned from the error between the setpoint and the gyro angle.
*/
@Override
public void teleopInit(){
}
@Override
public void teleopPeriodic() {
double turningValue = (kAngleSetpoint - gyro.getAngle()) * kP;
double compensation = Math.copySign(turningValue, joystick.getY());
myRobot.arcadeDrive(joystick.getY(), compensation);
}
}